Program Listing for File protocol.h
↰ Return to documentation for file (include/master_board_sdk/protocol.h
)
#ifndef PROTOCOL_H
#define PROTOCOL_H
#include <stdint.h>
#define PROTOCOL_VERSION 4
/* Position of the values in the command packet */
#define UD_COMMAND_MODE 0
#define UD_COMMAND_POS_1 1
#define UD_COMMAND_POS_2 3
#define UD_COMMAND_VEL_1 5
#define UD_COMMAND_VEL_2 6
#define UD_COMMAND_IQ_1 7
#define UD_COMMAND_IQ_2 8
#define UD_COMMAND_KP_1 9
#define UD_COMMAND_KP_2 10
#define UD_COMMAND_KD_1 11
#define UD_COMMAND_KD_2 12
#define UD_COMMAND_ISAT_12 13
/* Command packet -> mode : bits */
#define UD_COMMAND_MODE_ES (1<<15)
#define UD_COMMAND_MODE_EM1 (1<<14)
#define UD_COMMAND_MODE_EM2 (1<<13)
#define UD_COMMAND_MODE_EPRE (1<<12)
#define UD_COMMAND_MODE_EI1OC (1<<11)
#define UD_COMMAND_MODE_EI2OC (1<<10)
#define UD_COMMAND_MODE_TIMEOUT (0xFF<<0) //Timeout
/* Qvalues for each fields */
#define UD_QN_POS 24
#define UD_QN_VEL 11
#define UD_QN_IQ 10
#define UD_QN_ISAT 3
#define UD_QN_CR 15
#define UD_QN_ADC 16
#define UD_QN_KP 11
#define UD_QN_KD 10
#define IMU_QN_ACC 11
#define IMU_QN_GYR 11
#define IMU_QN_EF 13
/* Position of the values in the sensor packet */
#define UD_SENSOR_STATUS 0
#define UD_SENSOR_TIMESTAMP 1
#define UD_SENSOR_POS_1 2
#define UD_SENSOR_POS_2 4
#define UD_SENSOR_VEL_1 6
#define UD_SENSOR_VEL_2 7
#define UD_SENSOR_IQ_1 8
#define UD_SENSOR_IQ_2 9
#define UD_SENSOR_CR_1 10
#define UD_SENSOR_CR_2 11
#define UD_SENSOR_ADC_1 12
#define UD_SENSOR_ADC_2 13
/* sensor packet -> status : bits */
#define UD_SENSOR_STATUS_SE (1<<15)
#define UD_SENSOR_STATUS_M1E (1<<14)
#define UD_SENSOR_STATUS_M1R (1<<13)
#define UD_SENSOR_STATUS_M2E (1<<12)
#define UD_SENSOR_STATUS_M2R (1<<11)
#define UD_SENSOR_STATUS_IDX1D (1<<10)
#define UD_SENSOR_STATUS_IDX2D (1<<9)
#define UD_SENSOR_STATUS_IDX1T (1<<8)
#define UD_SENSOR_STATUS_IDX2T (1<<7)
#define UD_SENSOR_STATUS_ERROR (0xF<<0)
/* sensor packet -> status -> ERROR : values */
#define UD_SENSOR_STATUS_ERROR_NO_ERROR 0
#define UD_SENSOR_STATUS_ERROR_ENCODER1 1
#define UD_SENSOR_STATUS_ERROR_SPI_RECV_TIMEOUT 2
#define UD_SENSOR_STATUS_ERROR_CRIT_TEMP 3 // currently unused
#define UD_SENSOR_STATUS_ERROR_POSCONV 4
#define UD_SENSOR_STATUS_ERROR_POS_ROLLOVER 5
#define UD_SENSOR_STATUS_ERROR_ENCODER2 6
#define UD_SENSOR_STATUS_ERROR_OTHER 7
#define UD_LENGTH 14
/* To properly handle SPI type conversion */
#define SUB_REG_u16(p_packet, pos) (*((uint16_t *) ((p_packet) + (pos))))
#define SUB_REG_u32(p_packet, pos) (*((uint32_t *) ((p_packet) + (pos))))
#define SUB_REG_16(p_packet, pos) (*((int16_t *) ((p_packet) + (pos))))
#define SUB_REG_32(p_packet, pos) (*((int32_t *) ((p_packet) + (pos))))
#define D32Q24_TO_D16QN(a,n) ((a)>>(24-(n))&0xFFFF)
#define D32Q24_TO_D8QN(a,n) ((a)>>(24-(n))&0xFF)
#define uD16QN_TO_D32Q24(a,n) (((uint32_t)(a))<<(24-(n)))
#define uD8QN_TO_D32Q24(a,n) (((uint32_t)(a))<<(24-(n)))
#define D16QN_TO_D32Q24(a,n) (((int32_t)(a))<<(24-(n)))
#define D8QN_TO_D32Q24(a,n) (((int32_t)(a))<<(24-(n)))
#define D32QN_TO_FLOAT(a,n) ((float)(a)) / (1<<(n))
#define D16QN_TO_FLOAT(a,n) ((float)(a)) / (1<<(n))
#define D8QN_TO_FLOAT(a,n) ((float)(a)) / (1<<(n))
#define FLOAT_TO_uD32QN(a,n) ((uint32_t) std::min(std::max(((a) * (1<<(n))), 0.0), 4294967295.0))
#define FLOAT_TO_uD16QN(a,n) ((uint16_t) std::min(std::max(((a) * (1<<(n))), 0.0), 65535.0))
#define FLOAT_TO_uD8QN(a,n) ((uint8_t) std::min(std::max(((a) * (1<<(n))), 0.0), 255.0))
#define FLOAT_TO_D32QN(a,n) ((int32_t) std::min(std::max(((a) * (1<<(n))), -2147483647.0), 2147483647.0))
#define FLOAT_TO_D16QN(a,n) ((int16_t) std::min(std::max((a) * (1<<(n)), -32767.0), 32767.0))
#define FLOAT_TO_D8QN(a,n) ((int8_t) std::min(std::max(((a) * (1<<(n))), -127.0), +127.0))
#endif