ISComm.h

Go to the documentation of this file.

/*
MIT LICENSE

Copyright (c) 2014-2020 Inertial Sense, Inc. - http://inertialsense.com

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#ifndef IS_SIMPLE_INTERFACE_H
#define IS_SIMPLE_INTERFACE_H

#include "data_sets.h"
#include "stddef.h"

#ifdef __cplusplus
extern "C" {
#endif

// -------------------------------------------------------------------------------------------------------------------------------
// Inertial Sense simple communications interface --------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------------------------------------------------
// The simple comm interface does not require any of the com manager APIs and is designed for simple or lightweight scenarios, tiny embedded platforms, etc.
// *****************************************************************************
// ****** Binary messages                                                 ******
// *****************************************************************************

#define _DID_INS_ECEF_QE2B                      DID_INS_4                               
#define _DID_INS_LLA_EULER_NED          DID_INS_1                               
#define _DID_INS_LLA_QN2B                       DID_INS_3                               
#define _DID_IMU_DUAL                           DID_DUAL_IMU                    
#define _DID_IMU_PREINTEGRATED_IMU      DID_PREINTEGRATED_IMU   
#define _DID_GPS1_POS                           DID_GPS1_POS                    
#define _DID_MAG_CAL                            DID_MAG_CAL                             
#define _DID_MAGNETOMETER_1                     DID_MAGNETOMETER_1              
#define _DID_MAGNETOMETER_2                     DID_MAGNETOMETER_2              
#define _DID_BAROMETER                          DID_BAROMETER                   
#define _DID_WHEEL_ENCODER                      DID_WHEEL_ENCODER               
#define _DID_POS_MEASUREMENT            DID_POSITION_MEASUREMENT
#define _DID_DEV_INFO                           DID_DEV_INFO                    
#define _DID_BIT                                        DID_BIT                                 
#define _DID_STROBE_IN_TIME                     DID_STROBE_IN_TIME              
#define _DID_FLASH_CONFIG                       DID_FLASH_CONFIG                
#define _DID_RMC                                        DID_RMC                                 
typedef enum
{
        _PTYPE_NONE                 = 0,                                                
        _PTYPE_PARSE_ERROR          = 0xFFFFFFFF,                               
        _PTYPE_INERTIAL_SENSE_DATA  = 0xEFFFFFFF,                               
        _PTYPE_INERTIAL_SENSE_CMD   = 0xDFFFFFFF,                               
        _PTYPE_INERTIAL_SENSE_ACK   = 0xCFFFFFFF,                               
        _PTYPE_ASCII_NMEA           = 0xBFFFFFFF,                               
        _PTYPE_UBLOX                = 0xAFFFFFFF,                               
        _PTYPE_RTCM3                = 0x9FFFFFFF,                               
} protocol_type_t;

#define IS_COM_BAUDRATE_DEFAULT IS_BAUDRATE_921600

#define MAX_DATASET_SIZE        1024

#define PKT_OVERHEAD_SIZE       8       // = START_BYTE + INFO_BYTE + COUNTER_BYTE + FLAGS_BYTE + CHECKSUM_BYTE_1 + CHECKSUM_BYTE_2 + CHECKSUM_BYTE_3 + END_BYTE

#ifndef PKT_BUF_SIZE
#define PKT_BUF_SIZE            2048
#endif

#define MAX_PKT_OVERHEAD_SIZE   (PKT_OVERHEAD_SIZE + PKT_OVERHEAD_SIZE - 2)  // worst case for packet encoding header / footer

#define MAX_PKT_BODY_SIZE       (((PKT_BUF_SIZE - MAX_PKT_OVERHEAD_SIZE) / 2) & 0xFFFFFFFE) // worst case for packet encoding body, rounded down to even number

#define MAX_P_DATA_BODY_SIZE    (MAX_PKT_BODY_SIZE-sizeof(p_data_hdr_t))    // Data size limit

#define MAX_P_ACK_BODY_SIZE     (MAX_PKT_BODY_SIZE-sizeof(p_ack_hdr_t))     // Ack data size

#define CHECKSUM_SEED 0x00AAAAAA

// Major (in com_manager.h)
#define PROTOCOL_VERSION_CHAR0                  (1)

// version 1: initial release
// version 2: 24 bit checksum support
#define PROTOCOL_VERSION_CHAR1                  (2)

// Minor (in data_sets.h)
// #define PROTOCOL_VERSION_CHAR2               0
// #define PROTOCOL_VERSION_CHAR3               0

#define UBLOX_HEADER_SIZE 6
#define RTCM3_HEADER_SIZE 3

PUSH_PACK_1

typedef enum
{
        IS_BAUDRATE_19200 = 19200,
        IS_BAUDRATE_38400 = 38400,
        IS_BAUDRATE_57600 = 57600,
        IS_BAUDRATE_115200 = 115200,
        IS_BAUDRATE_230400 = 230400,
        IS_BAUDRATE_460800 = 460800,
        IS_BAUDRATE_921600 = 921600,
        IS_BAUDRATE_3000000 = 3000000,

        IS_BAUDRATE_COUNT = 8
} baud_rate_t;

extern const unsigned int g_validBaudRates[IS_BAUDRATE_COUNT];

/*
Packet Overview

Byte
0                       Packet start byte
1                       Packet indo: ID (mask 0x1F) | reserved bits (mask 0xE)
2                       Packet counter (for ACK and retry)
3                       Packet flags

// packet body, may or may not exist depending on packet id - packet body is made up of 4 byte or 8 byte values.
4-7                     Data identifier
8-11            Data length
12-15           Data offset
16-19           Data start
(n-8)-(n-5)     Last piece of data
// end data

n-4                     Reserved
n-3                     Checksum high byte
n-2                     Checksum low byte
n-1                     Packet end byte
*/

// Packet IDs   
typedef uint32_t ePacketIDs;

#define PID_INVALID                         (ePacketIDs)0   
#define PID_ACK                             (ePacketIDs)1   
#define PID_NACK                            (ePacketIDs)2   
#define PID_GET_DATA                        (ePacketIDs)3   
#define PID_DATA                            (ePacketIDs)4   
#define PID_SET_DATA                        (ePacketIDs)5   
#define PID_STOP_BROADCASTS_ALL_PORTS       (ePacketIDs)6   
#define PID_STOP_DID_BROADCAST              (ePacketIDs)7   
#define PID_STOP_BROADCASTS_CURRENT_PORT    (ePacketIDs)8   
#define PID_COUNT                           (ePacketIDs)9   
#define PID_MAX_COUNT                       (ePacketIDs)32  
typedef struct
{
        uint32_t            size;

        uint8_t             buf[PKT_BUF_SIZE];
} buffer_t;

typedef struct
{
        uint8_t             *ptr;

        uint32_t            size;
} bufPtr_t;

typedef struct
{
        uint8_t             *txPtr;

        uint8_t             *rxPtr;

        uint32_t            size;
} bufTxRxPtr_t;

typedef enum
{
        asciiTypeInt = 0,

        asciiTypeUInt = 1,

        asciiTypeFloat = 2,

        asciiTypeDouble = 3
} asciiDataType;

#define ASCII_MESSAGEID_TO_UINT(c4) ((uint32_t)(c4)[0] << 24 | ((uint32_t)(c4)[1] << 16) | ((uint32_t)(c4)[2] << 8) | ((uint32_t)(c4)[3]))

enum ePktHdrFlags
{
        // bit set for little endian, bit cleared for big endian
        CM_PKT_FLAGS_LITTLE_ENDIAN = 0x01,
        CM_PKT_FLAGS_ENDIANNESS_MASK = 0x01,

        // has any valid packet been received
        CM_PKT_FLAGS_RX_VALID_DATA = 0x02,

        // multi-packet data set
        CM_PKT_FLAGS_MORE_DATA_AVAILABLE = 0x04,

        // Allow for arbitrary length in bytes of data, not necessarily multiple of 4. Don't auto-swap bytes for endianness
        CM_PKT_FLAGS_RAW_DATA_NO_SWAP = 0x08,

        // Checksum is the new 24 bit algorithm instead of the old 16 bit algorithm
        CM_PKT_FLAGS_CHECKSUM_24_BIT = 0x10
};

enum ePktSpecialChars
{
        PSC_ASCII_START_BYTE = 0x24,

        PSC_ASCII_END_BYTE = 0x0A,

        PSC_START_BYTE = 0xFF,

        PSC_END_BYTE = 0xFE,

        PSC_RESERVED_KEY = 0xFD,

        UBLOX_START_BYTE1 = 0xB5,

        UBLOX_START_BYTE2 = 0x62,

        RTCM3_START_BYTE = 0xD3
};

typedef struct
{
        unsigned char messageId[4];

        uint8_t* ptr;

        int ptrSize;

        int fieldCount;

        uint16_t* fieldsAndOffsets;
} asciiMessageMap_t;

typedef struct
{
        uint8_t             startByte;

        uint8_t             pid;

        uint8_t             counter;

        uint8_t             flags;
} packet_hdr_t;

typedef struct
{
        uint8_t             cksum3;

        uint8_t             cksum2;

        uint8_t             cksum1;

        uint8_t             stopByte;
} packet_ftr_t;

typedef struct
{
        packet_hdr_t        hdr;

        bufPtr_t            body;
} packet_t;

typedef struct
{
        packet_hdr_t        hdr;                    // Packet header
        bufPtr_t            bodyHdr;                // Body header
        bufPtr_t            txData;                 // Pointer and size of data to send
} pkt_info_t;

typedef struct
{
        uint32_t            id;

        uint32_t            size;

        uint32_t            offset;
} p_data_hdr_t;

typedef struct
{
        p_data_hdr_t        hdr;

        uint8_t             buf[MAX_DATASET_SIZE];
} p_data_t, p_data_set_t;

typedef struct
{
        uint32_t            id;

        uint32_t            size;

        uint32_t            offset;

        uint32_t            bc_period_multiple;
} p_data_get_t;

typedef struct
{
        uint32_t            id;
} p_data_disable_t;

typedef struct
{
        uint32_t            pktInfo;

        uint32_t            pktCounter;
} p_ack_hdr_t;

typedef struct
{
        p_ack_hdr_t         hdr;

        union 
        {
                uint8_t         buf[sizeof(p_data_hdr_t)];
                p_data_hdr_t    dataHdr;
        }                                       body;
} p_ack_t, p_nack_t;

typedef struct
{
        uint8_t* start;

        uint8_t* end;

        uint32_t size;

        uint8_t* head;

        uint8_t* tail;

        uint8_t* scan;

} is_comm_buffer_t;

typedef struct  
{
        uint8_t enableISB;

        uint8_t enableASCII;

        uint8_t enableUblox;

        uint8_t enableRTCM3;
} is_comm_config_t;

typedef struct
{
        is_comm_buffer_t buf;
        
        is_comm_config_t config;

        uint32_t txPktCount;

        uint32_t rxErrorCount;

        uint32_t hasStartByte;

        int32_t parseState;

        p_data_hdr_t dataHdr;

        uint8_t* dataPtr;

        uint8_t* pktPtr;
        
        uint8_t* altDecodeBuf;

        uint32_t ackNeeded;

        packet_t pkt;

} is_comm_instance_t;

POP_PACK

void is_comm_init(is_comm_instance_t* instance, uint8_t *buffer, int bufferSize);

protocol_type_t is_comm_parse_byte(is_comm_instance_t* instance, uint8_t byte);

protocol_type_t is_comm_parse(is_comm_instance_t* instance);

int is_comm_free(is_comm_instance_t* instance);

int is_comm_get_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, uint32_t periodMultiple);

int is_comm_get_data_rmc(is_comm_instance_t* instance, uint64_t rmcBits);

int is_comm_set_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, void* data);

int is_comm_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, void* data);

int is_comm_stop_broadcasts_all_ports(is_comm_instance_t* instance);

int is_comm_stop_broadcasts_current_port(is_comm_instance_t* instance);


// -------------------------------------------------------------------------------------------------------------------------------
// Common packet encode / decode functions
// -------------------------------------------------------------------------------------------------------------------------------
// common encode / decode for com manager and simple interface
int is_encode_binary_packet(void* srcBuffer, unsigned int srcBufferLength, packet_hdr_t* hdr, uint8_t additionalPktFlags, void* encodedPacket, int encodedPacketLength);
int is_decode_binary_packet(packet_t *pkt, unsigned char* pbuf, int pbufSize);
int is_decode_binary_packet_byte(uint8_t** _ptrSrc, uint8_t** _ptrDest, uint32_t* checksum, uint32_t shift);
void is_decode_binary_packet_footer(packet_ftr_t* ftr, uint8_t* ptrSrc, uint8_t** ptrSrcEnd, uint32_t* checksum);
void is_enable_packet_encoding(int enabled); // default is enabled

unsigned int calculate24BitCRCQ(unsigned char* buffer, unsigned int len);
unsigned int getBitsAsUInt32(const unsigned char* buffer, unsigned int pos, unsigned int len);

char copyDataPToStructP(void *sptr, const p_data_t *data, const unsigned int maxsize);

char copyDataPToStructP2(void *sptr, const p_data_hdr_t *dataHdr, const uint8_t *dataBuf, const unsigned int maxsize);

char is_comm_copy_to_struct(void *sptr, const is_comm_instance_t *com, const unsigned int maxsize);

#ifdef __cplusplus
}
#endif

#endif // IS_SIMPLE_INTERFACE_H