Go to the source code of this file.
Classes | |
struct | asciiMessageMap_t |
struct | buffer_t |
struct | bufPtr_t |
struct | bufTxRxPtr_t |
struct | is_comm_buffer_t |
struct | is_comm_config_t |
struct | is_comm_instance_t |
struct | p_ack_hdr_t |
struct | p_ack_t |
struct | p_data_disable_t |
struct | p_data_get_t |
struct | p_data_hdr_t |
struct | p_data_t |
struct | packet_ftr_t |
struct | packet_hdr_t |
struct | packet_t |
struct | pkt_info_t |
Macros | |
#define | _DID_BAROMETER DID_BAROMETER /** (see barometer_t) Barometric pressure sensor data */ |
#define | _DID_BIT DID_BIT /** (see bit_t) System built-in self-test */ |
#define | _DID_DEV_INFO DID_DEV_INFO /** (see dev_info_t) Device information */ |
#define | _DID_FLASH_CONFIG DID_FLASH_CONFIG /** (see nvm_flash_cfg_t) Flash memory configuration */ |
#define | _DID_GPS1_POS DID_GPS1_POS /** (see gps_pos_t) GPS output */ |
#define | _DID_IMU_DUAL DID_DUAL_IMU /** (see dual_imu_t) Dual IMU output: angular rate (rad/s) and linear acceleration (m/s^2) */ |
#define | _DID_IMU_PREINTEGRATED_IMU DID_PREINTEGRATED_IMU /** (see preintegrated_imu_t) Dual IMU output: Conning and sculling integrated at IMU update rate. */ |
#define | _DID_INS_ECEF_QE2B DID_INS_4 /** (see ins_4_t) INS output: ECEF position (m) and velocity (m/s), quaternion from ECEF */ |
#define | _DID_INS_LLA_EULER_NED DID_INS_1 /** (see ins_1_t) INS/AHRS output: euler from NED, LLA (degrees,m), NED pos (m) and vel (m/s) from refLLA */ |
#define | _DID_INS_LLA_QN2B DID_INS_3 /** (see ins_3_t) INS/AHRS output: quaternion from NED, LLA (degrees,m) */ |
#define | _DID_MAG_CAL DID_MAG_CAL /** (see mag_cal_t) Magnetometer calibration */ |
#define | _DID_MAGNETOMETER_1 DID_MAGNETOMETER_1 /** (see magnetometer_t) Magnetometer sensor 1 output */ |
#define | _DID_MAGNETOMETER_2 DID_MAGNETOMETER_2 /** (see magnetometer_t) Magnetometer sensor 2 output */ |
#define | _DID_POS_MEASUREMENT DID_POSITION_MEASUREMENT/** (see pos_measurement_t) Position Measurement data*/ |
#define | _DID_RMC DID_RMC /** (see rmc_t) Realtime message controller */ |
#define | _DID_STROBE_IN_TIME DID_STROBE_IN_TIME /** (see strobe_in_time_t) Timestamp for input strobe */ |
#define | _DID_WHEEL_ENCODER DID_WHEEL_ENCODER /** (see wheel_encoder_t) Wheel encoder sensor data */ |
#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])) |
#define | CHECKSUM_SEED 0x00AAAAAA |
#define | IS_COM_BAUDRATE_DEFAULT IS_BAUDRATE_921600 |
#define | MAX_DATASET_SIZE 1024 |
#define | MAX_P_ACK_BODY_SIZE (MAX_PKT_BODY_SIZE-sizeof(p_ack_hdr_t)) |
#define | MAX_P_DATA_BODY_SIZE (MAX_PKT_BODY_SIZE-sizeof(p_data_hdr_t)) |
#define | MAX_PKT_BODY_SIZE (((PKT_BUF_SIZE - MAX_PKT_OVERHEAD_SIZE) / 2) & 0xFFFFFFFE) |
#define | MAX_PKT_OVERHEAD_SIZE (PKT_OVERHEAD_SIZE + PKT_OVERHEAD_SIZE - 2) |
#define | PID_ACK (ePacketIDs)1 /** (ACK) received valid packet */ |
#define | PID_COUNT (ePacketIDs)9 /** The number of packet identifiers, keep this at the end! */ |
#define | PID_DATA (ePacketIDs)4 /** Data sent in response to PID_GET_DATA (no PID_ACK is sent) */ |
#define | PID_GET_DATA (ePacketIDs)3 /** Request for data to be broadcast, response is PID_DATA. See data structures for list of possible broadcast data. */ |
#define | PID_INVALID (ePacketIDs)0 /** Invalid packet id */ |
#define | PID_MAX_COUNT (ePacketIDs)32 /** The maximum count of packet identifiers, 0x1F (PACKET_INFO_ID_MASK) */ |
#define | PID_NACK (ePacketIDs)2 /** (NACK) received invalid packet */ |
#define | PID_SET_DATA (ePacketIDs)5 /** Data sent, such as configuration options. PID_ACK is sent in response. */ |
#define | PID_STOP_BROADCASTS_ALL_PORTS (ePacketIDs)6 /** Stop all data broadcasts on all ports. Responds with an ACK */ |
#define | PID_STOP_BROADCASTS_CURRENT_PORT (ePacketIDs)8 /** Stop all data broadcasts on current port. Responds with an ACK */ |
#define | PID_STOP_DID_BROADCAST (ePacketIDs)7 /** Stop a specific broadcast */ |
#define | PKT_BUF_SIZE 2048 |
#define | PKT_OVERHEAD_SIZE 8 |
#define | PROTOCOL_VERSION_CHAR0 (1) |
#define | PROTOCOL_VERSION_CHAR1 (2) |
#define | RTCM3_HEADER_SIZE 3 |
#define | UBLOX_HEADER_SIZE 6 |
Typedefs | |
typedef uint32_t | ePacketIDs |
typedef struct p_data_t | p_data_set_t |
typedef struct p_ack_t | p_nack_t |
Functions | |
unsigned int | calculate24BitCRCQ (unsigned char *buffer, 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) |
unsigned int | getBitsAsUInt32 (const unsigned char *buffer, unsigned int pos, unsigned int len) |
char | is_comm_copy_to_struct (void *sptr, const is_comm_instance_t *com, const unsigned int maxsize) |
int | is_comm_data (is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, void *data) |
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) |
POP_PACK void | is_comm_init (is_comm_instance_t *instance, uint8_t *buffer, int bufferSize) |
protocol_type_t | is_comm_parse (is_comm_instance_t *instance) |
protocol_type_t | is_comm_parse_byte (is_comm_instance_t *instance, uint8_t byte) |
int | is_comm_set_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) |
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) |
int | is_encode_binary_packet (void *srcBuffer, unsigned int srcBufferLength, packet_hdr_t *hdr, uint8_t additionalPktFlags, void *encodedPacket, int encodedPacketLength) |
Variables | |
const unsigned int | g_validBaudRates [IS_BAUDRATE_COUNT] |
#define _DID_BAROMETER DID_BAROMETER /** (see barometer_t) Barometric pressure sensor data */ |
#define _DID_DEV_INFO DID_DEV_INFO /** (see dev_info_t) Device information */ |
#define _DID_FLASH_CONFIG DID_FLASH_CONFIG /** (see nvm_flash_cfg_t) Flash memory configuration */ |
#define _DID_GPS1_POS DID_GPS1_POS /** (see gps_pos_t) GPS output */ |
#define _DID_IMU_DUAL DID_DUAL_IMU /** (see dual_imu_t) Dual IMU output: angular rate (rad/s) and linear acceleration (m/s^2) */ |
#define _DID_IMU_PREINTEGRATED_IMU DID_PREINTEGRATED_IMU /** (see preintegrated_imu_t) Dual IMU output: Conning and sculling integrated at IMU update rate. */ |
#define _DID_INS_ECEF_QE2B DID_INS_4 /** (see ins_4_t) INS output: ECEF position (m) and velocity (m/s), quaternion from ECEF */ |
DEFINITIONS AND CONVENTIONS
INS = inertial navigation system AHRS = attitude heading reference system IMU = inertial measurement unit: gyros (rad/s), accelerometers (m/s^2) ECEF = earth-centered earth fixed: x,y,z or vx,vy,vz (m or m/s) LLA = latitude, longitude, altitude (degrees,m) NED = north, east, down (m or m/s) QE2B = quaternion rotation from ECEF frame to local frame. QN2B = quaternion rotation from NED frame to local frame. UVW = velocities in local frame.INS/AHRS
#define _DID_MAG_CAL DID_MAG_CAL /** (see mag_cal_t) Magnetometer calibration */ |
#define _DID_MAGNETOMETER_1 DID_MAGNETOMETER_1 /** (see magnetometer_t) Magnetometer sensor 1 output */ |
#define _DID_MAGNETOMETER_2 DID_MAGNETOMETER_2 /** (see magnetometer_t) Magnetometer sensor 2 output */ |
#define _DID_POS_MEASUREMENT DID_POSITION_MEASUREMENT/** (see pos_measurement_t) Position Measurement data*/ |
#define _DID_STROBE_IN_TIME DID_STROBE_IN_TIME /** (see strobe_in_time_t) Timestamp for input strobe */ |
#define _DID_WHEEL_ENCODER DID_WHEEL_ENCODER /** (see wheel_encoder_t) Wheel encoder sensor data */ |
#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])) |
#define CHECKSUM_SEED 0x00AAAAAA |
#define IS_COM_BAUDRATE_DEFAULT IS_BAUDRATE_921600 |
#define MAX_DATASET_SIZE 1024 |
#define MAX_P_ACK_BODY_SIZE (MAX_PKT_BODY_SIZE-sizeof(p_ack_hdr_t)) |
#define MAX_P_DATA_BODY_SIZE (MAX_PKT_BODY_SIZE-sizeof(p_data_hdr_t)) |
#define MAX_PKT_BODY_SIZE (((PKT_BUF_SIZE - MAX_PKT_OVERHEAD_SIZE) / 2) & 0xFFFFFFFE) |
#define MAX_PKT_OVERHEAD_SIZE (PKT_OVERHEAD_SIZE + PKT_OVERHEAD_SIZE - 2) |
#define PID_ACK (ePacketIDs)1 /** (ACK) received valid packet */ |
#define PID_COUNT (ePacketIDs)9 /** The number of packet identifiers, keep this at the end! */ |
#define PID_DATA (ePacketIDs)4 /** Data sent in response to PID_GET_DATA (no PID_ACK is sent) */ |
#define PID_GET_DATA (ePacketIDs)3 /** Request for data to be broadcast, response is PID_DATA. See data structures for list of possible broadcast data. */ |
#define PID_INVALID (ePacketIDs)0 /** Invalid packet id */ |
#define PID_MAX_COUNT (ePacketIDs)32 /** The maximum count of packet identifiers, 0x1F (PACKET_INFO_ID_MASK) */ |
#define PID_NACK (ePacketIDs)2 /** (NACK) received invalid packet */ |
#define PID_SET_DATA (ePacketIDs)5 /** Data sent, such as configuration options. PID_ACK is sent in response. */ |
#define PID_STOP_BROADCASTS_ALL_PORTS (ePacketIDs)6 /** Stop all data broadcasts on all ports. Responds with an ACK */ |
#define PID_STOP_BROADCASTS_CURRENT_PORT (ePacketIDs)8 /** Stop all data broadcasts on current port. Responds with an ACK */ |
#define PID_STOP_DID_BROADCAST (ePacketIDs)7 /** Stop a specific broadcast */ |
#define PKT_BUF_SIZE 2048 |
#define PKT_OVERHEAD_SIZE 8 |
#define PROTOCOL_VERSION_CHAR0 (1) |
typedef uint32_t ePacketIDs |
typedef struct p_data_t p_data_set_t |
enum asciiDataType |
enum baud_rate_t |
We must not allow any packing or shifting as these data structures must match exactly in memory on all devices Valid baud rates for Inertial Sense hardware
Enumerator | |
---|---|
IS_BAUDRATE_9600 | |
IS_BAUDRATE_19200 | |
IS_BAUDRATE_38400 | |
IS_BAUDRATE_57600 | |
IS_BAUDRATE_115200 | |
IS_BAUDRATE_230400 | |
IS_BAUDRATE_460800 | |
IS_BAUDRATE_921600 | |
IS_BAUDRATE_3000000 | |
IS_BAUDRATE_COUNT |
enum ePktHdrFlags |
enum ePktSpecialChars |
Built in special bytes that will need to be encoded in the binary packet format. This is not an exhaustive list, as other bytes such as ublox and rtcm preambles will be encoded as well, but these messages are not parsed and handled in the com manager, rather they are forwarded via the pass through handler. A byte is encoded by writing a 0xFD byte (encoded byte marker), followed by the encoded byte, which is created by inverting all the bits of the original byte. These bytes are not encoded when written in the proper spot in the packet (i.e. when writing the first byte for a binary packet, the 0xFF byte, no encoding is performed).
enum protocol_type_t |
Protocol Type
unsigned int calculate24BitCRCQ | ( | unsigned char * | buffer, |
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 | ||
) |
unsigned int getBitsAsUInt32 | ( | const unsigned char * | buffer, |
unsigned int | pos, | ||
unsigned int | len | ||
) |
Retrieve the 32 bit unsigned integer value of the specified bits - note that no bounds checking is done on buffer
buffer | the buffer containing the bits |
pos | the start bit position in buffer to read at |
len | the number of bits to read |
char is_comm_copy_to_struct | ( | void * | sptr, |
const is_comm_instance_t * | instance, | ||
const unsigned int | maxsize | ||
) |
int is_comm_data | ( | is_comm_instance_t * | instance, |
uint32_t | dataId, | ||
uint32_t | offset, | ||
uint32_t | size, | ||
void * | data | ||
) |
Same as is_comm_set_data() except NO acknowledge packet is sent in response to this packet.
int is_comm_free | ( | is_comm_instance_t * | instance | ) |
Removed old data and shift unparsed data to the the buffer start if running out of space at the buffer end. Returns number of bytes available in the bufer.
instance | the comm instance passed to is_comm_init |
int is_comm_get_data | ( | is_comm_instance_t * | instance, |
uint32_t | dataId, | ||
uint32_t | offset, | ||
uint32_t | size, | ||
uint32_t | periodMultiple | ||
) |
Encode a binary packet to get data from the device - puts the data ready to send into the buffer passed into is_comm_init
instance | the comm instance passed to is_comm_init |
dataId | the data id to request (see DID_* at top of this file) |
offset | the offset into data to request. Set offset and length to 0 for entire data structure. |
length | the length into data from offset to request. Set offset and length to 0 for entire data structure. |
periodMultiple | how often you want the data to stream out, 0 for a one time message and turn off. |
int is_comm_get_data_rmc | ( | is_comm_instance_t * | instance, |
uint64_t | rmcBits | ||
) |
Encode a binary packet to get predefined list of data sets from the device - puts the data ready to send into the buffer passed into is_comm_init
instance | the comm instance passed to is_comm_init |
RMC | bits specifying data messages to stream. See presets: RMC_PRESET_PPD_BITS = post processing data, RMC_PRESET_INS_BITS = INS2 and GPS data at full rate |
POP_PACK void is_comm_init | ( | is_comm_instance_t * | instance, |
uint8_t * | buffer, | ||
int | bufferSize | ||
) |
Pop off the packing argument, we can safely allow packing and shifting in memory at this point Init simple communications interface - call this before doing anything else
instance | communications instance, please ensure that you have set the buffer and bufferSize |
protocol_type_t is_comm_parse | ( | is_comm_instance_t * | instance | ) |
Decode packet data - when data is available, return value will be the protocol type (see protocol_type_t) and the comm instance dataPtr will point to the start of the valid data. For Inertial Sense binary protocol, comm instance dataHdr contains the data ID (DID), size, and offset.
instance | the comm instance passed to is_comm_init |
Read a set of bytes (fast method) protocol_type_t ptype;
Get available size of comm buffer int n = is_comm_free(comm);
Read data directly into comm buffer if ((n = mySerialPortRead(comm->buf.tail, n))) { Update comm buffer tail pointer comm->buf.tail += n;
Search comm buffer for valid packets while ((ptype = is_comm_parse(comm)) != _PTYPE_NONE) { switch (ptype) { case _PTYPE_INERTIAL_SENSE_DATA: case _PTYPE_INERTIAL_SENSE_CMD: case _PTYPE_INERTIAL_SENSE_ACK: break; case _PTYPE_UBLOX: break; case _PTYPE_RTCM3: break; case _PTYPE_ASCII_NMEA: break; } } }
protocol_type_t is_comm_parse_byte | ( | is_comm_instance_t * | instance, |
uint8_t | byte | ||
) |
Decode packet data - when data is available, return value will be the protocol type (see protocol_type_t) and the comm instance dataPtr will point to the start of the valid data. For Inertial Sense binary protocol, comm instance dataHdr contains the data ID (DID), size, and offset.
instance | the comm instance passed to is_comm_init |
byte | the byte to decode |
Read one byte (simple method) uint8_t c; protocol_type_t ptype; Read from serial buffer until empty while (mySerialPortRead(&c, 1)) { if ((ptype = is_comm_parse_byte(comm, c)) != _PTYPE_NONE) { switch (ptype) { case _PTYPE_INERTIAL_SENSE_DATA: case _PTYPE_INERTIAL_SENSE_CMD: case _PTYPE_INERTIAL_SENSE_ACK: break; case _PTYPE_UBLOX: break; case _PTYPE_RTCM3: break; case _PTYPE_ASCII_NMEA: break; } } }
int is_comm_set_data | ( | is_comm_instance_t * | instance, |
uint32_t | dataId, | ||
uint32_t | offset, | ||
uint32_t | size, | ||
void * | data | ||
) |
Encode a binary packet to set data on the device - puts the data ready to send into the buffer passed into is_comm_init. An acknowledge packet is sent in response to this packet.
instance | the comm instance passed to is_comm_init |
dataId | the data id to set on the device (see DID_* at top of this file) |
offset | the offset to start setting data at on the data structure on the device |
size | the number of bytes to set on the data structure on the device |
data | the actual data to change on the data structure on the device - this should have at least size bytes available |
int is_comm_stop_broadcasts_all_ports | ( | is_comm_instance_t * | instance | ) |
Encode a binary packet to stop all messages being broadcast on the device on all ports - puts the data ready to send into the buffer passed into is_comm_init
instance | the comm instance passed to is_comm_init |
int is_comm_stop_broadcasts_current_port | ( | is_comm_instance_t * | instance | ) |
Encode a binary packet to stop all messages being broadcast on the device on this port - puts the data ready to send into the buffer passed into is_comm_init
instance | the comm instance passed to is_comm_init |
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 | ||
) |
int is_encode_binary_packet | ( | void * | srcBuffer, |
unsigned int | srcBufferLength, | ||
packet_hdr_t * | hdr, | ||
uint8_t | additionalPktFlags, | ||
void * | encodedPacket, | ||
int | encodedPacketLength | ||
) |
const unsigned int g_validBaudRates[IS_BAUDRATE_COUNT] |