mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved work on protocol v2, will remove uart support

Browse Source
This commit is contained in:
Ruixiang Du
2020-11-05 13:34:28 +08:00
parent 057696d207
commit 4b1fb472d9
6 changed files with 204 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
ugv_sdk/CMakeLists.txt
View File

@@ -85,14 +85,14 @@ add_library(${PROJECT_NAME}
src/mobile_base.cpp src/mobile_base.cpp
src/agx_msg_parser.c src/agx_msg_parser.c
# robots # robots
src/hunter_base.cpp # src/hunter_base.cpp
src/hunter_can_parser.c # src/hunter_can_parser.c
src/scout_base.cpp # src/scout_base.cpp
src/scout_can_parser.c # src/scout_can_parser.c
src/scout_uart_parser.c # src/scout_uart_parser.c
src/tracer_base.cpp # src/tracer_base.cpp
src/tracer_can_parser.c # src/tracer_can_parser.c
src/tracer_uart_parser.c # src/tracer_uart_parser.c
# src/bunker_base.cpp # src/bunker_base.cpp
# src/bunker_can_parser.c # src/bunker_can_parser.c
) )

6
ugv_sdk/include/ugv_sdk/mobile_base.hpp
View File

@@ -17,6 +17,7 @@
#include <cstdint> #include <cstdint>
#include <thread> #include <thread>
#include <mutex> #include <mutex>
#include <atomic>
#include "wrp_io/async_can.hpp" #include "wrp_io/async_can.hpp"
#include "wrp_io/async_serial.hpp" #include "wrp_io/async_serial.hpp"
@@ -32,6 +33,7 @@ class MobileBase {
MobileBase &operator=(const MobileBase &hunter) = delete; MobileBase &operator=(const MobileBase &hunter) = delete;
void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100); void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// connect to roboot from CAN or serial // connect to roboot from CAN or serial
void Connect(std::string dev_name, int32_t baud_rate = 0); void Connect(std::string dev_name, int32_t baud_rate = 0);
@@ -39,6 +41,9 @@ class MobileBase {
// disconnect from roboot, only valid for serial port // disconnect from roboot, only valid for serial port
void Disconnect(); void Disconnect();
// ask background thread to shutdown properly
void Terminate();
// cmd thread runs at 100Hz (10ms) by default // cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { void SetCmdThreadPeriodMs(int32_t period_ms) {
cmd_thread_period_ms_ = period_ms; cmd_thread_period_ms_ = period_ms;
@@ -62,6 +67,7 @@ class MobileBase {
std::thread cmd_thread_; std::thread cmd_thread_;
int32_t cmd_thread_period_ms_ = 10; int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false; bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions // internal functions
void ConfigureCAN(const std::string &can_if_name = "can0"); void ConfigureCAN(const std::string &can_if_name = "can0");

24
ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h
View File

@@ -10,9 +10,9 @@
#ifndef AGX_MSG_PARSER_H #ifndef AGX_MSG_PARSER_H
#define AGX_MSG_PARSER_H #define AGX_MSG_PARSER_H
// #ifdef __cplusplus #ifdef __cplusplus
// extern "C" { extern "C" {
// #endif #endif
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
@@ -30,16 +30,16 @@ struct can_frame {
}; };
#endif #endif
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg); bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame); void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg); bool DecodeUartData(uint8_t c, AgxMessage *msg);
void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len); void EncodeUartData(const AgxMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len); uint8_t CalcUartDataChecksum(uint8_t *buf, uint8_t len);
// #ifdef __cplusplus #ifdef __cplusplus
// } }
// #endif #endif
#endif /* AGX_PARSER_H */ #endif /* AGX_PARSER_H */

113
ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h
View File

@@ -16,34 +16,46 @@
#include <stdint.h> #include <stdint.h>
/*---------------------------- Motor IDs -------------------------------*/
#define SCOUT_MOTOR1_ID ((uint8_t)0x00) #define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01) #define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02) #define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03) #define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions #define HUNTER_MOTOR1_ID ((uint8_t)0x00)
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) #define HUNTER_MOTOR2_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02) #define HUNTER_MOTOR3_ID ((uint8_t)0x02)
#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) #define TRACER_MOTOR1_ID ((uint8_t)0x00)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) #define TRACER_MOTOR2_ID ((uint8_t)0x01)
/*--------------------------- Message IDs ------------------------------*/
// UART Message IDs
#define UART_MSG_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_MSG_MOTION_STATUS_ID ((uint8_t)0x02)
#define UART_MSG_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_MSG_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_MSG_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_MSG_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_MSG_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_MSG_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_MSG_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Message IDs
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211)
#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x221)
// CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x111) #define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x111)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x221)
#define CAN_MSG_SELECT_CONTROL_MODE_ID ((uint32_t)0x421) #define CAN_MSG_SELECT_CONTROL_MODE_ID ((uint32_t)0x421)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x121) #define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x121)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x231) #define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x231)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x211)
//#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x251)
//#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x252)
//#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x253)
//#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x254)
#define CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x251) #define CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x251)
#define CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x252) #define CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x252)
#define CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x253) #define CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x253)
@@ -57,10 +69,9 @@
/*--------------------- Control/State Constants ------------------------*/ /*--------------------- Control/State Constants ------------------------*/
// Motion Control // Motion Control
#define CTRL_MODE_REMOTE ((uint8_t)0x00) #define CTRL_MODE_RC ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) #define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02) #define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00) #define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01) #define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
@@ -81,19 +92,19 @@
#define LIGHT_MODE_BREATH ((uint8_t)0x02) #define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03) #define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback // System State
#define BASE_STATE_NORMAL ((uint8_t)0x00) #define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01) #define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02) #define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100) #define SYSTEM_FAULT_BAT_LOW_CRITICAL ((uint8_t)0x01)
#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200) #define SYSTEM_FAULT_BAT_LOW_WARNING ((uint8_t)0x02)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400) #define SYSTEM_FAULT_RC_SIG_LOST ((uint8_t)0x04)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800) #define SYSTEM_FAULT_BYTE_RESERVED1 ((uint8_t)0x08)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000) #define SYSTEM_FAULT_BYTE_RESERVED2 ((uint8_t)0x10)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000) #define SYSTEM_FAULT_BYTE_RESERVED3 ((uint8_t)0x20)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000) #define SYSTEM_FAULT_BYTE_RESERVED4 ((uint8_t)0x40)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000) #define SYSTEM_FAULT_BYTE_RESERVED5 ((uint8_t)0x80)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) #define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) #define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
@@ -110,10 +121,25 @@
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect // reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1) #pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol // System Status Feedback
typedef union {
struct {
uint8_t vehicle_state;
uint8_t control_mode;
struct {
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
uint8_t fault_code;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
} status;
uint8_t raw[8];
} SystemStateMessage;
// Motion Control // Motion Control
typedef struct { typedef union {
union { union {
struct { struct {
struct { struct {
@@ -169,25 +195,6 @@ typedef struct {
} data; } data;
} ModSelectMessage; } ModSelectMessage;
// System Status Feedback
typedef struct {
union {
struct {
uint8_t base_state;
uint8_t control_mode;
struct {
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
uint8_t fault_code;
uint8_t reserved0;
uint8_t reserved1;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control // Light Control
typedef struct { typedef struct {
union { union {
@@ -332,7 +339,7 @@ typedef struct {
// status messages // status messages
MotionStatusMessage motion_status_msg; MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg; LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg; SystemStateMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg; MotorDriverStatusMessage motor_driver_status_msg;
MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg; MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg;
MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg; MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg;

192
ugv_sdk/src/agx_msg_parser.c
View File

@@ -33,14 +33,14 @@ typedef enum {
WAIT_FOR_CHECKSUM WAIT_FOR_CHECKSUM
} UartDecodeState; } UartDecodeState;
#define UART_PAYLOAD_BUFFER_SIZE 64 #define UART_MSG_BUFFER_SIZE 64
#define UART_FRAME_SOF_LEN ((uint8_t)2) #define UART_MSG_SOF_LEN ((uint8_t)2)
#define UART_FRAME_FIXED_FIELD_LEN ((uint8_t)4) #define UART_MSG_FIXED_FIELD_LEN ((uint8_t)4)
#define UART_FRAME_SOF1 ((uint8_t)0x5a) #define UART_MSG_SOF1 ((uint8_t)0x5a)
#define UART_FRAME_SOF2 ((uint8_t)0xa5) #define UART_MSG_SOF2 ((uint8_t)0xa5)
#define UART_FRAME_TYPE_CONTROL ((uint8_t)0x55) #define UART_MSG_TYPE_CONTROL ((uint8_t)0x55)
#define UART_FRAME_TYPE_STATUS ((uint8_t)0xaa) #define UART_MSG_TYPE_STATUS ((uint8_t)0xaa)
#define UART_FRAME_NONE_ID ((uint8_t)0x00) #define UART_MSG_NONE_ID ((uint8_t)0x00)
// frame buffer // frame buffer
static struct { static struct {
@@ -50,7 +50,7 @@ static struct {
uint8_t frame_cnt; uint8_t frame_cnt;
uint8_t frame_checksum; uint8_t frame_checksum;
uint8_t internal_checksum; uint8_t internal_checksum;
uint8_t payload_buffer[UART_PAYLOAD_BUFFER_SIZE]; uint8_t payload_buffer[UART_MSG_BUFFER_SIZE];
size_t payload_data_pos; size_t payload_data_pos;
} uart_parsing_data; } uart_parsing_data;
@@ -81,15 +81,15 @@ static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
struct can_frame *tx_frame); struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg) { bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->type = ScoutMsgNone; msg->type = ScoutMsgNone;
switch (rx_frame->can_id) { switch (rx_frame->can_id) {
// in the current implementation, both MsgType and can_frame include 8 * // in the current implementation, both MsgType and can_frame include 8 *
// uint8_t // uint8_t
case CAN_MSG_MOTION_CONTROL_STATUS_ID: { case CAN_MSG_MOTION_STATE_ID: {
msg->type = ScoutMotionStatusMsg; msg->type = ScoutMotionStatusMsg;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; // msg->motion_status_msg.id = CAN_MSG_MOTION_STATE_ID;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t)); rx_frame->can_dlc * sizeof(uint8_t));
break; break;
@@ -101,10 +101,10 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg) {
rx_frame->can_dlc * sizeof(uint8_t)); rx_frame->can_dlc * sizeof(uint8_t));
break; break;
} }
case CAN_MSG_SYSTEM_STATUS_STATUS_ID: { case CAN_MSG_SYSTEM_STATE_ID: {
msg->type = ScoutSystemStatusMsg; msg->type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATE_ID;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, memcpy(msg->body.system_status_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t)); rx_frame->can_dlc * sizeof(uint8_t));
break; break;
} }
@@ -200,12 +200,12 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg) {
return true; return true;
} }
void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame) { void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
switch (msg->type) { switch (msg->type) {
// in the current implementation, both MsgType and can_frame include 8 * // in the current implementation, both MsgType and can_frame include 8 *
// uint8_t // uint8_t
case ScoutMotionStatusMsg: { case ScoutMotionStatusMsg: {
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; tx_frame->can_id = CAN_MSG_MOTION_STATE_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw,
tx_frame->can_dlc); tx_frame->can_dlc);
@@ -219,9 +219,9 @@ void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame) {
break; break;
} }
case ScoutSystemStatusMsg: { case ScoutSystemStatusMsg: {
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; tx_frame->can_id = CAN_MSG_SYSTEM_STATE_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, memcpy(tx_frame->data, msg->body.system_status_msg.raw,
tx_frame->can_dlc); tx_frame->can_dlc);
break; break;
} }
@@ -262,7 +262,7 @@ void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame) {
default: default:
break; break;
} }
// tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id,
// tx_frame->data, tx_frame->can_dlc); // tx_frame->data, tx_frame->can_dlc);
} }
@@ -272,7 +272,7 @@ void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg,
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = tx_frame->data[7] =
CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
} }
void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
struct can_frame *tx_frame) { struct can_frame *tx_frame) {
@@ -280,7 +280,7 @@ void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = tx_frame->data[7] =
CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
} }
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
struct can_frame *tx_frame) { struct can_frame *tx_frame) {
@@ -288,30 +288,30 @@ void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = tx_frame->data[7] =
CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
} }
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) { uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {
uint8_t checksum = 0x00; uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; for (int i = 0; i < (dlc - 1); ++i) checksum += data[i];
return checksum; return checksum;
} }
void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) { void EncodeUartData(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
// SOF // SOF
buf[0] = UART_FRAME_SOF1; buf[0] = UART_MSG_SOF1;
buf[1] = UART_FRAME_SOF2; buf[1] = UART_MSG_SOF2;
// frame len, type, ID // frame len, type, ID
buf[2] = 0x0a; buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_STATUS; buf[3] = UART_MSG_TYPE_STATUS;
switch (msg->type) { switch (msg->type) {
// in the current implementation, both MsgType and can_frame include 8 * // in the current implementation, both MsgType and can_frame include 8 *
// uint8_t // uint8_t
case ScoutMotionStatusMsg: { case ScoutMotionStatusMsg: {
buf[4] = UART_FRAME_MOTION_STATUS_ID; buf[4] = UART_MSG_MOTION_STATUS_ID;
buf[5] = msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte; buf[5] = msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte;
buf[6] = msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte; buf[6] = msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte;
buf[7] = msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte; buf[7] = msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte;
@@ -322,7 +322,7 @@ void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
break; break;
} }
case ScoutLightStatusMsg: { case ScoutLightStatusMsg: {
buf[4] = UART_FRAME_LIGHT_STATUS_ID; buf[4] = UART_MSG_LIGHT_STATUS_ID;
buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable; buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable;
buf[6] = msg->body.light_status_msg.data.status.front_light_mode; buf[6] = msg->body.light_status_msg.data.status.front_light_mode;
buf[7] = msg->body.light_status_msg.data.status.front_light_custom; buf[7] = msg->body.light_status_msg.data.status.front_light_custom;
@@ -333,26 +333,26 @@ void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
break; break;
} }
case ScoutSystemStatusMsg: { case ScoutSystemStatusMsg: {
buf[4] = UART_FRAME_SYSTEM_STATUS_ID; buf[4] = UART_MSG_SYSTEM_STATUS_ID;
buf[5] = msg->body.system_status_msg.data.status.base_state; buf[5] = msg->body.system_status_msg.status.vehicle_state;
buf[6] = msg->body.system_status_msg.data.status.control_mode; buf[6] = msg->body.system_status_msg.status.control_mode;
buf[7] = buf[7] =
msg->body.system_status_msg.data.status.battery_voltage.high_byte; msg->body.system_status_msg.status.battery_voltage.high_byte;
buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte; buf[8] = msg->body.system_status_msg.status.battery_voltage.low_byte;
buf[9] = msg->body.system_status_msg.data.status.fault_code; buf[9] = msg->body.system_status_msg.status.fault_code;
buf[10] = 0; buf[10] = 0;
buf[11] = 0; buf[11] = 0;
break; break;
} }
case ScoutMotorDriverStatusMsg: { case ScoutMotorDriverStatusMsg: {
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID; buf[4] = UART_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID; buf[4] = UART_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID; buf[4] = UART_MSG_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID; buf[4] = UART_MSG_MOTOR4_DRIVER_STATUS_ID;
buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte; buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte;
buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte; buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte;
buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte; buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte;
@@ -374,13 +374,13 @@ void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
break; break;
} }
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + UART_FRAME_SOF_LEN); buf[12] = CalcUartDataChecksum(buf, buf[2] + UART_MSG_SOF_LEN);
// length: SOF + Frame + Checksum // length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1; *len = buf[2] + UART_MSG_SOF_LEN + 1;
} }
bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg) { bool DecodeUartData(uint8_t c, AgxMessage *msg) {
static AgxMessage decoded_msg; static AgxMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg); bool result = ParseChar(c, &decoded_msg);
@@ -391,13 +391,13 @@ bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg) {
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf,
uint8_t *len) { uint8_t *len) {
// SOF // SOF
buf[0] = UART_FRAME_SOF1; buf[0] = UART_MSG_SOF1;
buf[1] = UART_FRAME_SOF2; buf[1] = UART_MSG_SOF2;
// frame len, type, ID // frame len, type, ID
buf[2] = 0x0a; buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_CONTROL; buf[3] = UART_MSG_TYPE_CONTROL;
buf[4] = UART_FRAME_MOTION_CONTROL_ID; buf[4] = UART_MSG_MOTION_CONTROL_ID;
// frame payload // frame payload
buf[5] = msg->data.cmd.linear_velocity.high_byte; buf[5] = msg->data.cmd.linear_velocity.high_byte;
@@ -412,19 +412,19 @@ void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf,
buf[12] = 0x00; buf[12] = 0x00;
// length: SOF + Frame + Checksum // length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1; *len = buf[2] + UART_MSG_SOF_LEN + 1;
} }
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf,
uint8_t *len) { uint8_t *len) {
// SOF // SOF
buf[0] = UART_FRAME_SOF1; buf[0] = UART_MSG_SOF1;
buf[1] = UART_FRAME_SOF2; buf[1] = UART_MSG_SOF2;
// frame len, type, ID // frame len, type, ID
buf[2] = 0x0a; buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_CONTROL; buf[3] = UART_MSG_TYPE_CONTROL;
buf[4] = UART_FRAME_LIGHT_CONTROL_ID; buf[4] = UART_MSG_LIGHT_CONTROL_ID;
// frame payload // frame payload
buf[5] = msg->data.cmd.light_ctrl_enable; buf[5] = msg->data.cmd.light_ctrl_enable;
@@ -436,10 +436,10 @@ void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf,
// frame count, checksum // frame count, checksum
buf[11] = msg->data.cmd.count; buf[11] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + UART_FRAME_SOF_LEN); buf[12] = CalcUartDataChecksum(buf, buf[2] + UART_MSG_SOF_LEN);
// length: SOF + Frame + Checksum // length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1; *len = buf[2] + UART_MSG_SOF_LEN + 1;
} }
bool ParseChar(uint8_t c, AgxMessage *msg) { bool ParseChar(uint8_t c, AgxMessage *msg) {
@@ -448,15 +448,15 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
bool new_frame_parsed = false; bool new_frame_parsed = false;
switch (decode_state) { switch (decode_state) {
case WAIT_FOR_SOF1: { case WAIT_FOR_SOF1: {
if (c == UART_FRAME_SOF1) { if (c == UART_MSG_SOF1) {
uart_parsing_data.frame_id = UART_FRAME_NONE_ID; uart_parsing_data.frame_id = UART_MSG_NONE_ID;
uart_parsing_data.frame_type = 0; uart_parsing_data.frame_type = 0;
uart_parsing_data.frame_len = 0; uart_parsing_data.frame_len = 0;
uart_parsing_data.frame_cnt = 0; uart_parsing_data.frame_cnt = 0;
uart_parsing_data.frame_checksum = 0; uart_parsing_data.frame_checksum = 0;
uart_parsing_data.internal_checksum = 0; uart_parsing_data.internal_checksum = 0;
uart_parsing_data.payload_data_pos = 0; uart_parsing_data.payload_data_pos = 0;
memset(uart_parsing_data.payload_buffer, 0, UART_PAYLOAD_BUFFER_SIZE); memset(uart_parsing_data.payload_buffer, 0, UART_MSG_BUFFER_SIZE);
decode_state = WAIT_FOR_SOF2; decode_state = WAIT_FOR_SOF2;
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
@@ -468,7 +468,7 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
break; break;
} }
case WAIT_FOR_SOF2: { case WAIT_FOR_SOF2: {
if (c == UART_FRAME_SOF2) { if (c == UART_MSG_SOF2) {
decode_state = WAIT_FOR_FRAME_LEN; decode_state = WAIT_FOR_FRAME_LEN;
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
std::cout << "found sof2" << std::endl; std::cout << "found sof2" << std::endl;
@@ -498,8 +498,8 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
} }
case WAIT_FOR_FRAME_TYPE: { case WAIT_FOR_FRAME_TYPE: {
switch (c) { switch (c) {
case UART_FRAME_TYPE_CONTROL: { case UART_MSG_TYPE_CONTROL: {
uart_parsing_data.frame_type = UART_FRAME_TYPE_CONTROL; uart_parsing_data.frame_type = UART_MSG_TYPE_CONTROL;
decode_state = WAIT_FOR_FRAME_ID; decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
std::cout << "control type frame received" << std::endl; std::cout << "control type frame received" << std::endl;
@@ -508,8 +508,8 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
#endif #endif
break; break;
} }
case UART_FRAME_TYPE_STATUS: { case UART_MSG_TYPE_STATUS: {
uart_parsing_data.frame_type = UART_FRAME_TYPE_STATUS; uart_parsing_data.frame_type = UART_MSG_TYPE_STATUS;
decode_state = WAIT_FOR_FRAME_ID; decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
std::cout << "status type frame received" << std::endl; std::cout << "status type frame received" << std::endl;
@@ -521,12 +521,12 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
default: { default: {
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
std::cerr << "ERROR: Not expecting frame of a type other than " std::cerr << "ERROR: Not expecting frame of a type other than "
"UART_FRAME_TYPE_STATUS" "UART_MSG_TYPE_STATUS"
<< std::endl; << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO)) #elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, JLinkWriteString(0,
"ERROR: Not expecting frame of a type other than " "ERROR: Not expecting frame of a type other than "
"UART_FRAME_TYPE_STATUS\n"); "UART_MSG_TYPE_STATUS\n");
#endif #endif
decode_state = WAIT_FOR_SOF1; decode_state = WAIT_FOR_SOF1;
} }
@@ -535,13 +535,13 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
} }
case WAIT_FOR_FRAME_ID: { case WAIT_FOR_FRAME_ID: {
switch (c) { switch (c) {
case UART_FRAME_SYSTEM_STATUS_ID: case UART_MSG_SYSTEM_STATUS_ID:
case UART_FRAME_MOTION_STATUS_ID: case UART_MSG_MOTION_STATUS_ID:
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: case UART_MSG_MOTOR1_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: case UART_MSG_MOTOR2_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: case UART_MSG_MOTOR3_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: case UART_MSG_MOTOR4_DRIVER_STATUS_ID:
case UART_FRAME_LIGHT_STATUS_ID: { case UART_MSG_LIGHT_STATUS_ID: {
uart_parsing_data.frame_id = c; uart_parsing_data.frame_id = c;
decode_state = WAIT_FOR_PAYLOAD; decode_state = WAIT_FOR_PAYLOAD;
#ifdef PRINT_CPP_DEBUG_INFO #ifdef PRINT_CPP_DEBUG_INFO
@@ -573,7 +573,7 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
JLinkRTTPrintf(0, "1 byte added: %d\n", c); JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif #endif
if (uart_parsing_data.payload_data_pos == if (uart_parsing_data.payload_data_pos ==
(uart_parsing_data.frame_len - UART_FRAME_FIXED_FIELD_LEN)) (uart_parsing_data.frame_len - UART_MSG_FIXED_FIELD_LEN))
decode_state = WAIT_FOR_FRAME_COUNT; decode_state = WAIT_FOR_FRAME_COUNT;
break; break;
} }
@@ -617,9 +617,9 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
#elif (defined(PRINT_JLINK_DEBUG_INFO)) #elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "checksum correct\n"); JLinkWriteString(0, "checksum correct\n");
#endif #endif
if (uart_parsing_data.frame_type == UART_FRAME_TYPE_STATUS) if (uart_parsing_data.frame_type == UART_MSG_TYPE_STATUS)
ConstructStatusMessage(msg); ConstructStatusMessage(msg);
else if (uart_parsing_data.frame_type == UART_FRAME_TYPE_CONTROL) else if (uart_parsing_data.frame_type == UART_MSG_TYPE_CONTROL)
ConstructControlMessage(msg); ConstructControlMessage(msg);
++uart_parsing_stats.frame_parsed; ++uart_parsing_stats.frame_parsed;
} else { } else {
@@ -653,7 +653,7 @@ bool ConstructControlMessage(AgxMessage *msg) {
if (msg == NULL) return false; if (msg == NULL) return false;
switch (uart_parsing_data.frame_id) { switch (uart_parsing_data.frame_id) {
case UART_FRAME_MOTION_CONTROL_ID: { case UART_MSG_MOTION_CONTROL_ID: {
msg->type = ScoutMotionControlMsg; msg->type = ScoutMotionControlMsg;
msg->body.motion_control_msg.data.cmd.linear_velocity.high_byte = msg->body.motion_control_msg.data.cmd.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0]; uart_parsing_data.payload_buffer[0];
@@ -673,7 +673,7 @@ bool ConstructControlMessage(AgxMessage *msg) {
uart_parsing_data.payload_buffer[7]; uart_parsing_data.payload_buffer[7];
break; break;
} }
case UART_FRAME_LIGHT_CONTROL_ID: { case UART_MSG_LIGHT_CONTROL_ID: {
msg->type = ScoutLightControlMsg; msg->type = ScoutLightControlMsg;
msg->body.light_control_msg.data.cmd.light_ctrl_enable = msg->body.light_control_msg.data.cmd.light_ctrl_enable =
uart_parsing_data.payload_buffer[0]; uart_parsing_data.payload_buffer[0];
@@ -700,27 +700,27 @@ bool ConstructStatusMessage(AgxMessage *msg) {
if (msg == NULL) return false; if (msg == NULL) return false;
switch (uart_parsing_data.frame_id) { switch (uart_parsing_data.frame_id) {
case UART_FRAME_SYSTEM_STATUS_ID: { case UART_MSG_SYSTEM_STATUS_ID: {
msg->type = ScoutSystemStatusMsg; msg->type = ScoutSystemStatusMsg;
msg->body.system_status_msg.data.status.base_state = msg->body.system_status_msg.status.vehicle_state =
uart_parsing_data.payload_buffer[0]; uart_parsing_data.payload_buffer[0];
msg->body.system_status_msg.data.status.control_mode = msg->body.system_status_msg.status.control_mode =
uart_parsing_data.payload_buffer[1]; uart_parsing_data.payload_buffer[1];
msg->body.system_status_msg.data.status.battery_voltage.high_byte = msg->body.system_status_msg.status.battery_voltage.high_byte =
uart_parsing_data.payload_buffer[2]; uart_parsing_data.payload_buffer[2];
msg->body.system_status_msg.data.status.battery_voltage.low_byte = msg->body.system_status_msg.status.battery_voltage.low_byte =
uart_parsing_data.payload_buffer[3]; uart_parsing_data.payload_buffer[3];
msg->body.system_status_msg.data.status.fault_code = msg->body.system_status_msg.status.fault_code =
uart_parsing_data.payload_buffer[4]; uart_parsing_data.payload_buffer[4];
msg->body.system_status_msg.data.status.reserved0 = msg->body.system_status_msg.status.reserved0 =
uart_parsing_data.payload_buffer[5]; uart_parsing_data.payload_buffer[5];
msg->body.system_status_msg.data.status.reserved1 = msg->body.system_status_msg.status.reserved1 =
uart_parsing_data.frame_cnt; uart_parsing_data.frame_cnt;
msg->body.system_status_msg.data.status.checksum = msg->body.system_status_msg.status.count =
uart_parsing_data.frame_checksum; uart_parsing_data.frame_checksum;
break; break;
} }
case UART_FRAME_MOTION_STATUS_ID: { case UART_MSG_MOTION_STATUS_ID: {
msg->type = ScoutMotionStatusMsg; msg->type = ScoutMotionStatusMsg;
msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte = msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0]; uart_parsing_data.payload_buffer[0];
@@ -736,7 +736,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
msg->body.motion_status_msg.data.cmd.reserved3 = 0x00; msg->body.motion_status_msg.data.cmd.reserved3 = 0x00;
break; break;
} }
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: { case UART_MSG_MOTOR1_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg; msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -756,7 +756,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum; uart_parsing_data.frame_checksum;
break; break;
} }
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: { case UART_MSG_MOTOR2_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg; msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -776,7 +776,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum; uart_parsing_data.frame_checksum;
break; break;
} }
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: { case UART_MSG_MOTOR3_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg; msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -796,7 +796,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum; uart_parsing_data.frame_checksum;
break; break;
} }
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: { case UART_MSG_MOTOR4_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg; msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -816,7 +816,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum; uart_parsing_data.frame_checksum;
break; break;
} }
case UART_FRAME_LIGHT_STATUS_ID: { case UART_MSG_LIGHT_STATUS_ID: {
msg->type = ScoutLightStatusMsg; msg->type = ScoutLightStatusMsg;
msg->body.light_status_msg.data.status.light_ctrl_enable = msg->body.light_status_msg.data.status.light_ctrl_enable =
uart_parsing_data.payload_buffer[0]; uart_parsing_data.payload_buffer[0];
@@ -838,7 +838,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
return true; return true;
} }
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len) { uint8_t CalcUartDataChecksum(uint8_t *buf, uint8_t len) {
uint8_t checksum = 0; uint8_t checksum = 0;
#ifdef USE_XOR_CHECKSUM #ifdef USE_XOR_CHECKSUM
@@ -854,8 +854,8 @@ uint8_t CalcBufferedFrameChecksum() {
uint8_t checksum = 0x00; uint8_t checksum = 0x00;
#ifdef USE_XOR_CHECKSUM #ifdef USE_XOR_CHECKSUM
checksum ^= UART_FRAME_SOF1; checksum ^= UART_MSG_SOF1;
checksum ^= UART_FRAME_SOF2; checksum ^= UART_MSG_SOF2;
checksum ^= uart_parsing_data.frame_len; checksum ^= uart_parsing_data.frame_len;
checksum ^= uart_parsing_data.frame_type; checksum ^= uart_parsing_data.frame_type;
checksum ^= uart_parsing_data.frame_id; checksum ^= uart_parsing_data.frame_id;
@@ -863,8 +863,8 @@ uint8_t CalcBufferedFrameChecksum() {
checksum ^= uart_parsing_data.payload_buffer[i]; checksum ^= uart_parsing_data.payload_buffer[i];
checksum ^= uart_parsing_data.frame_cnt; checksum ^= uart_parsing_data.frame_cnt;
#else #else
checksum += UART_FRAME_SOF1; checksum += UART_MSG_SOF1;
checksum += UART_FRAME_SOF2; checksum += UART_MSG_SOF2;
checksum += uart_parsing_data.frame_len; checksum += uart_parsing_data.frame_len;
checksum += uart_parsing_data.frame_type; checksum += uart_parsing_data.frame_type;
checksum += uart_parsing_data.frame_id; checksum += uart_parsing_data.frame_id;

29
ugv_sdk/src/mobile_base.cpp
View File

@@ -19,6 +19,7 @@ namespace westonrobot {
MobileBase::~MobileBase() { MobileBase::~MobileBase() {
// release resource if occupied // release resource if occupied
Disconnect(); Disconnect();
// joint cmd thread // joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join(); if (cmd_thread_.joinable()) cmd_thread_.join();
} }
@@ -40,6 +41,11 @@ void MobileBase::Disconnect() {
} }
} }
void MobileBase::Terminate() {
keep_running_ = false;
std::terminate();
}
void MobileBase::ConfigureCAN(const std::string &can_if_name) { void MobileBase::ConfigureCAN(const std::string &can_if_name) {
can_if_ = std::make_shared<AsyncCAN>(can_if_name); can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->SetReceiveCallback( can_if_->SetReceiveCallback(
@@ -58,35 +64,44 @@ void MobileBase::ConfigureSerial(const std::string uart_name,
if (serial_if_->IsOpened()) serial_connected_ = true; if (serial_if_->IsOpened()) serial_connected_ = true;
} }
void MobileBase::SetCmdTimeout(bool enable, uint32_t timeout_ms) {
enable_timeout_ = true;
timeout_ms_ = timeout_ms;
}
void MobileBase::StartCmdThread() { void MobileBase::StartCmdThread() {
keep_running_ = true;
cmd_thread_ = std::thread( cmd_thread_ = std::thread(
std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_)); std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true; cmd_thread_started_ = true;
} }
void MobileBase::ControlLoop(int32_t period_ms) { void MobileBase::ControlLoop(int32_t period_ms) {
Timer ctrl_timer;
uint32_t timeout_iter_num; uint32_t timeout_iter_num;
if (enable_timeout_) { if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms); timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
std::cout << "Timeout iteration number: " << timeout_iter_num << std::endl; // std::cout << "Timeout iteration number: " << timeout_iter_num <<
// std::endl;
} }
while (true) { Timer tm;
ctrl_timer.reset(); while (keep_running_) {
tm.reset();
if (enable_timeout_) { if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) { if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd(); SendRobotCmd();
++watchdog_counter_; ++watchdog_counter_;
} else {
std::cout << "Warning: cmd timeout, old cmd not sent to robot" << std::endl;
} }
// else {
// std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
// std::endl;
// }
} else { } else {
SendRobotCmd(); SendRobotCmd();
} }
ctrl_timer.sleep_until_ms(period_ms); tm.sleep_until_ms(period_ms);
} }
} }
} // namespace westonrobot } // namespace westonrobot