packets.h

Go to the documentation of this file.

// This file is part of snark, a generic and flexible library
// for robotics research.
//
// Copyright (C) 2011 The University of Sydney
//
// snark is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// snark is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
// for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with snark. If not, see <http://www.gnu.org/licenses/>.

#ifndef SNARK_SENSORS_SICK_PACKET_H_
#define SNARK_SENSORS_SICK_PACKET_H_


#include <boost/array.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <comma/packed/byte.h>
#include <comma/packed/little_endian.h>
#include <comma/packed/big_endian.h>
#include <comma/packed/struct.h>

namespace snark {  namespace sick { namespace ldmrs {

struct timestamp : public comma::packed::packed_struct< timestamp, 8 >
{
    comma::packed::net_uint32 seconds;
    comma::packed::net_uint32 fractions;
};

struct little_endian_timestamp : public comma::packed::packed_struct< little_endian_timestamp, 8 >
{
    comma::packed::uint32 fractions;
    comma::packed::uint32 seconds;
};

struct header : public comma::packed::packed_struct< header, 24 >
{
    enum types { scan_type = 0x2202, command_type = 0x2010, response_type = 0x2020, fault_type = 0x2030 };
    
   static boost::array< unsigned char, 4 > sentinel_value;
    
    comma::packed::net_uint32 sentinel;
    
    comma::packed::net_uint32 previous_size;
    
    comma::packed::net_uint32 payload_size;
    
    comma::packed::byte padding;
    
    comma::packed::byte device_id;
    
    comma::packed::net_uint16 type;
    
    timestamp t;
 
    header();
    
    bool valid() const;
};

struct fault : public comma::packed::packed_struct< fault, 16 >
{
    comma::packed::uint16 faultRegister1;
    comma::packed::uint16 faultRegister2;
    comma::packed::uint16 warningRegister1;
    comma::packed::uint16 warningRegister2;
    boost::array< comma::packed::uint16, 4 > reserved;
    bool fatal() const;
};

std::ostream& operator<<( std::ostream&, const fault& rhs );

struct scan : public comma::packed::packed_struct< scan, 44 >
{
    struct header : public comma::packed::packed_struct< header, 44 >
    {
        enum Statuses { measurementFrequencyReached = 0x0008, externalSyncDetected = 0x0010, synced = 0x0020, syncMaster = 0x0040 };

        comma::packed::uint16 measurement_number;

        comma::packed::uint16 status;

        comma::packed::uint16 sync_phase_offset;

        little_endian_timestamp start; //timestamp start;

        little_endian_timestamp finish; //timestamp finish;

        comma::packed::uint16 steps;

        comma::packed::uint16 start_angle;

        comma::packed::uint16 finish_angle;

        comma::packed::uint16 points_count;

        boost::array< comma::packed::uint16, 7 > padding;
    };

    struct point : public comma::packed::packed_struct< point, 10 >
    {
        struct id : public comma::packed::packed_struct< id, 1 >
        {
            comma::packed::byte value;
            
            unsigned int layer() const;
            
            unsigned int echo() const;
        };
        
        id id;
        
        enum { transparent = 0x01, dust = 0x02, rain = dust, noise = dust, dirt = 0x08 };
        
        comma::packed::byte flags;
        
        comma::packed::uint16 angle;
        
        comma::packed::uint16 range;
        
        comma::packed::uint16 echo_pulse_width;
        
        comma::packed::uint16 padding;

        double elevation() const;
    };

    header scan_header;
    
    point* points();
    
    const point* points() const;

    double angle_as_radians( short angle ) const;

    double angle_as_radians( const point& point ) const;

    class timestamps
    {
        public:
            timestamps( const scan& scan );
            boost::posix_time::ptime operator[]( std::size_t index ) const;
        private:
            const scan& m_scan;
            boost::posix_time::ptime m_start;
            boost::posix_time::ptime m_finish;
            boost::posix_time::time_duration m_elapsed;
            short m_start_angle;
            short m_finish_angle;
            short m_diff;
            unsigned short m_steps;
    };
};

struct scan_packet : public comma::packed::packed_struct< scan_packet, header::size + scan::size >
{
    header packet_header;
    scan packet_scan;
};

struct commands
{
    struct header : public comma::packed::packed_struct< header, 4 >
    {
        comma::packed::uint16 id;
        comma::packed::net_uint16 padding;
    };
    
    struct response_header : public comma::packed::packed_struct< response_header, 2 >
    {
        comma::packed::uint16 id;
        std::size_t payload_size() const;
    };
    
    template < typename C, std::size_t Size >
    struct command : public comma::packed::packed_struct< C, header::size + Size >
    {
        header command_header;
        command() { command_header.id = C::id; }
    };
    
    template < typename C, std::size_t Size >
    struct response : public comma::packed::packed_struct< typename C::response, response_header::size + Size >
    {
        response_header header;
        response() { header.id = C::id; }
        void fail() { header.id = header.id() | 0x8000; }
        bool ok() const { return ( header.id() & 0x8000 ) == 0; }
        bool matches( comma::uint16 id ) { return id == ( header.id() & 0x3fff ); }
    };

    enum types
    {
          reset_dsp_type = 0x0000
        , get_status_type = 0x0001
        , save_configuration_type = 0x0004
        , set_type = 0x0010
        , get_type = 0x0011
        , reset_type = 0x001a
        , start_type = 0x0020
        , stop_type = 0x0021
        , set_ntp_seconds_type = 0x0030
        , set_ntp_fractions_type = 0x0031
    };
    
    struct reset_dsp : public command< reset_dsp, 0 >
    {
        enum { id = reset_dsp_type };
        
        // no response
    };
    
    struct get_status : public command< get_status, 0 >
    {
        enum { id = get_status_type };
        
        struct response : public commands::response< get_status, 30 >
        {
            comma::packed::uint16 firmwareVersion; // e.g. 0x1230 = version 1.2.3, 0x123B = version 1.2.3b
            comma::packed::uint16 fpgaVersion; // e.g. 0x1230 = version 1.2.3, 0x123B = version 1.2.3b
            comma::packed::uint16 status; // todo: define status enum
            comma::packed::uint32 reserved0;
            comma::packed::uint16 temperature; // -( Temperature - 579.2364 ) / 3.63
            comma::packed::uint16 serialNumber0; // YY CW (e. g. YY CW = 0x0740 = year '07, calendar week 40)
            comma::packed::uint16 serialNumber1; // serial number counter
            comma::packed::uint16 reserved1;
            boost::array< comma::packed::uint16, 3 > fpgaVersionDate; // cryptic: YYYY MM DD HH MM FPGA S
            boost::array< comma::packed::uint16, 3 > dspVersionDate; // cryptic: YYYY MM DD HH MM
        };
    };
    
    struct save_configuration : public command< save_configuration, 0 >
    {
        enum { id = save_configuration_type };
        
        struct response : public commands::response< save_configuration, 0 > {};
    };
    
    struct set : public command< set, 6 >
    {
        enum { id = set_type };
        enum IndexValues { ip_address = 0x1000, tcp_port = 0x1001, subnet_mask = 0x1002, gateway = 0x1003, data_output_flag = 0x1012  }; // data output flag: 16 bit, see documentation for meaning, if we need it at all
        comma::packed::uint16 index;
        comma::packed::uint32 value; // todo: since it is little endian, it's really unclear what on the earth their documentation means
        
        struct response : public commands::response< set, 0 > {};
    };
    
    struct get : public command< get, 2 >
    {
        enum { id = get_type };
        comma::packed::uint16 index;
        
        struct response : public commands::response< get, 6 >
        {
            comma::packed::uint16 index;
            comma::packed::uint32 value; // todo: since it is little endian, it's really unclear what on the earth their documentation means
        };
    };
    
    struct reset : public command< reset, 0 >
    {
        enum { id = reset_type };
        
        struct response : public commands::response< reset, 0 > {};
    };
    
    struct start : public command< start, 0 >
    {
        enum { id = start_type };
        
        struct response : public commands::response< start, 0 > {};
    };
    
    struct stop : public command< stop, 0 >
    {
        enum { id = stop_type };
        
        struct response : public commands::response< stop, 0 > {};
    };
    
    struct set_ntp_seconds : public command< set_ntp_seconds, 6 >
    {
        enum { id = set_ntp_seconds_type };
        comma::packed::uint16 reserved; // field missed in the sick documentation
        comma::packed::uint32 seconds;
        
        set_ntp_seconds();
        set_ntp_seconds( comma::uint32 seconds );
        
        struct response : public commands::response< set_ntp_seconds, 0 > {};
    };
    
    struct set_ntp_fractions : public command< set_ntp_fractions, 6 >
    {
        enum { id = set_ntp_fractions_type };
        comma::packed::uint16 reserved; // field missed in the sick documentation
        comma::packed::uint32 fractions;
        
        set_ntp_fractions();
        set_ntp_fractions( comma::uint32 fractions );
        
        struct response : public commands::response< set_ntp_fractions, 0 > {};
    };
    
    template < typename C >
    struct packet : public comma::packed::packed_struct< packet< C >, ldmrs::header::size + C::size >
    {
        ldmrs::header header;
        C command;
        packet() { header.type = ldmrs::header::command_type; header.payload_size = C::size; }
        packet( const C& command ) : command( command ) { header.type = ldmrs::header::command_type; header.payload_size = C::size; }
        
        struct response : public comma::packed::packed_struct< response, ldmrs::header::size + C::response::size >
        {
            ldmrs::header header;
            typename C::response response;
            response() { header.type = ldmrs::header::response_type; header.payload_size = C::response::size; }
            response( const typename C::response& response ) : response( response ) { header.type = ldmrs::header::response_type; header.payload_size = C::response::size; }
        };
    };
};

std::ostream& operator<<( std::ostream& os, const commands::get_status::response& rhs );
std::ostream& operator<<( std::ostream& os, const commands::get::response& rhs );

} } } // namespace snark {  namespace sick { namespace ldmrs {

#endif // #ifndef SNARK_SENSORS_SICK_PACKET_H_