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/agx_msg_parser.c
# robots
src/hunter_base.cpp
src/hunter_can_parser.c
src/scout_base.cpp
src/scout_can_parser.c
src/scout_uart_parser.c
src/tracer_base.cpp
src/tracer_can_parser.c
src/tracer_uart_parser.c
# src/hunter_base.cpp
# src/hunter_can_parser.c
# src/scout_base.cpp
# src/scout_can_parser.c
# src/scout_uart_parser.c
# src/tracer_base.cpp
# src/tracer_can_parser.c
# src/tracer_uart_parser.c
# src/bunker_base.cpp
# src/bunker_can_parser.c
)

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

@@ -17,6 +17,7 @@
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>
#include "wrp_io/async_can.hpp"
#include "wrp_io/async_serial.hpp"
@@ -32,6 +33,7 @@ class MobileBase {
MobileBase &operator=(const MobileBase &hunter) = delete;
void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// connect to roboot from CAN or serial
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
void Disconnect();
// ask background thread to shutdown properly
void Terminate();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) {
cmd_thread_period_ms_ = period_ms;
@@ -62,6 +67,7 @@ class MobileBase {
std::thread cmd_thread_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions
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
#define AGX_MSG_PARSER_H
// #ifdef __cplusplus
// extern "C" {
// #endif
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
@@ -30,16 +30,16 @@ struct can_frame {
};
#endif
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg);
void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
bool DecodeUartData(uint8_t c, AgxMessage *msg);
void EncodeUartData(const AgxMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcUartDataChecksum(uint8_t *buf, uint8_t len);
// #ifdef __cplusplus
// }
// #endif
#ifdef __cplusplus
}
#endif
#endif /* AGX_PARSER_H */

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

@@ -16,34 +16,46 @@
#include <stdint.h>
/*---------------------------- Motor IDs -------------------------------*/
#define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_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 HUNTER_MOTOR1_ID ((uint8_t)0x00)
#define HUNTER_MOTOR2_ID ((uint8_t)0x01)
#define HUNTER_MOTOR3_ID ((uint8_t)0x02)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
#define TRACER_MOTOR1_ID ((uint8_t)0x00)
#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_STATUS_ID ((uint32_t)0x221)
#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_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_MOTOR2_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x252)
#define CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x253)
@@ -57,10 +69,9 @@
/*--------------------- Control/State Constants ------------------------*/
// 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_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
@@ -81,19 +92,19 @@
#define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02)
// System State
#define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
#define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
#define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define SYSTEM_FAULT_BAT_LOW_CRITICAL ((uint8_t)0x01)
#define SYSTEM_FAULT_BAT_LOW_WARNING ((uint8_t)0x02)
#define SYSTEM_FAULT_RC_SIG_LOST ((uint8_t)0x04)
#define SYSTEM_FAULT_BYTE_RESERVED1 ((uint8_t)0x08)
#define SYSTEM_FAULT_BYTE_RESERVED2 ((uint8_t)0x10)
#define SYSTEM_FAULT_BYTE_RESERVED3 ((uint8_t)0x20)
#define SYSTEM_FAULT_BYTE_RESERVED4 ((uint8_t)0x40)
#define SYSTEM_FAULT_BYTE_RESERVED5 ((uint8_t)0x80)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
@@ -110,10 +121,25 @@
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#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
typedef struct {
typedef union {
union {
struct {
struct {
@@ -169,25 +195,6 @@ typedef struct {
} data;
} 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
typedef struct {
union {
@@ -332,7 +339,7 @@ typedef struct {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
SystemStateMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
MotorDriverLowSpeedStatusMessage motor_driver_low_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
} UartDecodeState;
#define UART_PAYLOAD_BUFFER_SIZE 64
#define UART_FRAME_SOF_LEN ((uint8_t)2)
#define UART_FRAME_FIXED_FIELD_LEN ((uint8_t)4)
#define UART_FRAME_SOF1 ((uint8_t)0x5a)
#define UART_FRAME_SOF2 ((uint8_t)0xa5)
#define UART_FRAME_TYPE_CONTROL ((uint8_t)0x55)
#define UART_FRAME_TYPE_STATUS ((uint8_t)0xaa)
#define UART_FRAME_NONE_ID ((uint8_t)0x00)
#define UART_MSG_BUFFER_SIZE 64
#define UART_MSG_SOF_LEN ((uint8_t)2)
#define UART_MSG_FIXED_FIELD_LEN ((uint8_t)4)
#define UART_MSG_SOF1 ((uint8_t)0x5a)
#define UART_MSG_SOF2 ((uint8_t)0xa5)
#define UART_MSG_TYPE_CONTROL ((uint8_t)0x55)
#define UART_MSG_TYPE_STATUS ((uint8_t)0xaa)
#define UART_MSG_NONE_ID ((uint8_t)0x00)
// frame buffer
static struct {
@@ -50,7 +50,7 @@ static struct {
uint8_t frame_cnt;
uint8_t frame_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;
} uart_parsing_data;
@@ -81,15 +81,15 @@ static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
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;
switch (rx_frame->can_id) {
// in the current implementation, both MsgType and can_frame include 8 *
// uint8_t
case CAN_MSG_MOTION_CONTROL_STATUS_ID: {
case CAN_MSG_MOTION_STATE_ID: {
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,
rx_frame->can_dlc * sizeof(uint8_t));
break;
@@ -101,10 +101,10 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg) {
rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID: {
case CAN_MSG_SYSTEM_STATE_ID: {
msg->type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data,
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATE_ID;
memcpy(msg->body.system_status_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
break;
}
@@ -200,12 +200,12 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *msg) {
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) {
// in the current implementation, both MsgType and can_frame include 8 *
// uint8_t
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;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw,
tx_frame->can_dlc);
@@ -219,9 +219,9 @@ void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame) {
break;
}
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;
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);
break;
}
@@ -262,7 +262,7 @@ void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame) {
default:
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);
}
@@ -272,7 +272,7 @@ void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg,
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
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,
struct can_frame *tx_frame) {
@@ -280,7 +280,7 @@ void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
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,
struct can_frame *tx_frame) {
@@ -288,30 +288,30 @@ void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
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;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i) checksum += data[i];
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
buf[0] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
buf[0] = UART_MSG_SOF1;
buf[1] = UART_MSG_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_STATUS;
buf[3] = UART_MSG_TYPE_STATUS;
switch (msg->type) {
// in the current implementation, both MsgType and can_frame include 8 *
// uint8_t
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[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;
@@ -322,7 +322,7 @@ void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
break;
}
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[6] = msg->body.light_status_msg.data.status.front_light_mode;
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;
}
case ScoutSystemStatusMsg: {
buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
buf[5] = msg->body.system_status_msg.data.status.base_state;
buf[6] = msg->body.system_status_msg.data.status.control_mode;
buf[4] = UART_MSG_SYSTEM_STATUS_ID;
buf[5] = msg->body.system_status_msg.status.vehicle_state;
buf[6] = msg->body.system_status_msg.status.control_mode;
buf[7] =
msg->body.system_status_msg.data.status.battery_voltage.high_byte;
buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte;
buf[9] = msg->body.system_status_msg.data.status.fault_code;
msg->body.system_status_msg.status.battery_voltage.high_byte;
buf[8] = msg->body.system_status_msg.status.battery_voltage.low_byte;
buf[9] = msg->body.system_status_msg.status.fault_code;
buf[10] = 0;
buf[11] = 0;
break;
}
case ScoutMotorDriverStatusMsg: {
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)
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)
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)
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[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;
@@ -374,13 +374,13 @@ void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
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
*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;
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,
uint8_t *len) {
// SOF
buf[0] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
buf[0] = UART_MSG_SOF1;
buf[1] = UART_MSG_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_MOTION_CONTROL_ID;
buf[3] = UART_MSG_TYPE_CONTROL;
buf[4] = UART_MSG_MOTION_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.linear_velocity.high_byte;
@@ -412,19 +412,19 @@ void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf,
buf[12] = 0x00;
// 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,
uint8_t *len) {
// SOF
buf[0] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
buf[0] = UART_MSG_SOF1;
buf[1] = UART_MSG_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_LIGHT_CONTROL_ID;
buf[3] = UART_MSG_TYPE_CONTROL;
buf[4] = UART_MSG_LIGHT_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.light_ctrl_enable;
@@ -436,10 +436,10 @@ void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf,
// frame count, checksum
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
*len = buf[2] + UART_FRAME_SOF_LEN + 1;
*len = buf[2] + UART_MSG_SOF_LEN + 1;
}
bool ParseChar(uint8_t c, AgxMessage *msg) {
@@ -448,15 +448,15 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
bool new_frame_parsed = false;
switch (decode_state) {
case WAIT_FOR_SOF1: {
if (c == UART_FRAME_SOF1) {
uart_parsing_data.frame_id = UART_FRAME_NONE_ID;
if (c == UART_MSG_SOF1) {
uart_parsing_data.frame_id = UART_MSG_NONE_ID;
uart_parsing_data.frame_type = 0;
uart_parsing_data.frame_len = 0;
uart_parsing_data.frame_cnt = 0;
uart_parsing_data.frame_checksum = 0;
uart_parsing_data.internal_checksum = 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;
#ifdef PRINT_CPP_DEBUG_INFO
@@ -468,7 +468,7 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
break;
}
case WAIT_FOR_SOF2: {
if (c == UART_FRAME_SOF2) {
if (c == UART_MSG_SOF2) {
decode_state = WAIT_FOR_FRAME_LEN;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "found sof2" << std::endl;
@@ -498,8 +498,8 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
}
case WAIT_FOR_FRAME_TYPE: {
switch (c) {
case UART_FRAME_TYPE_CONTROL: {
uart_parsing_data.frame_type = UART_FRAME_TYPE_CONTROL;
case UART_MSG_TYPE_CONTROL: {
uart_parsing_data.frame_type = UART_MSG_TYPE_CONTROL;
decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "control type frame received" << std::endl;
@@ -508,8 +508,8 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
#endif
break;
}
case UART_FRAME_TYPE_STATUS: {
uart_parsing_data.frame_type = UART_FRAME_TYPE_STATUS;
case UART_MSG_TYPE_STATUS: {
uart_parsing_data.frame_type = UART_MSG_TYPE_STATUS;
decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "status type frame received" << std::endl;
@@ -521,12 +521,12 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
default: {
#ifdef PRINT_CPP_DEBUG_INFO
std::cerr << "ERROR: Not expecting frame of a type other than "
"UART_FRAME_TYPE_STATUS"
"UART_MSG_TYPE_STATUS"
<< std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0,
"ERROR: Not expecting frame of a type other than "
"UART_FRAME_TYPE_STATUS\n");
"UART_MSG_TYPE_STATUS\n");
#endif
decode_state = WAIT_FOR_SOF1;
}
@@ -535,13 +535,13 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
}
case WAIT_FOR_FRAME_ID: {
switch (c) {
case UART_FRAME_SYSTEM_STATUS_ID:
case UART_FRAME_MOTION_STATUS_ID:
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
case UART_FRAME_LIGHT_STATUS_ID: {
case UART_MSG_SYSTEM_STATUS_ID:
case UART_MSG_MOTION_STATUS_ID:
case UART_MSG_MOTOR1_DRIVER_STATUS_ID:
case UART_MSG_MOTOR2_DRIVER_STATUS_ID:
case UART_MSG_MOTOR3_DRIVER_STATUS_ID:
case UART_MSG_MOTOR4_DRIVER_STATUS_ID:
case UART_MSG_LIGHT_STATUS_ID: {
uart_parsing_data.frame_id = c;
decode_state = WAIT_FOR_PAYLOAD;
#ifdef PRINT_CPP_DEBUG_INFO
@@ -573,7 +573,7 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif
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;
break;
}
@@ -617,9 +617,9 @@ bool ParseChar(uint8_t c, AgxMessage *msg) {
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "checksum correct\n");
#endif
if (uart_parsing_data.frame_type == UART_FRAME_TYPE_STATUS)
if (uart_parsing_data.frame_type == UART_MSG_TYPE_STATUS)
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);
++uart_parsing_stats.frame_parsed;
} else {
@@ -653,7 +653,7 @@ bool ConstructControlMessage(AgxMessage *msg) {
if (msg == NULL) return false;
switch (uart_parsing_data.frame_id) {
case UART_FRAME_MOTION_CONTROL_ID: {
case UART_MSG_MOTION_CONTROL_ID: {
msg->type = ScoutMotionControlMsg;
msg->body.motion_control_msg.data.cmd.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0];
@@ -673,7 +673,7 @@ bool ConstructControlMessage(AgxMessage *msg) {
uart_parsing_data.payload_buffer[7];
break;
}
case UART_FRAME_LIGHT_CONTROL_ID: {
case UART_MSG_LIGHT_CONTROL_ID: {
msg->type = ScoutLightControlMsg;
msg->body.light_control_msg.data.cmd.light_ctrl_enable =
uart_parsing_data.payload_buffer[0];
@@ -700,27 +700,27 @@ bool ConstructStatusMessage(AgxMessage *msg) {
if (msg == NULL) return false;
switch (uart_parsing_data.frame_id) {
case UART_FRAME_SYSTEM_STATUS_ID: {
case UART_MSG_SYSTEM_STATUS_ID: {
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];
msg->body.system_status_msg.data.status.control_mode =
msg->body.system_status_msg.status.control_mode =
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];
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];
msg->body.system_status_msg.data.status.fault_code =
msg->body.system_status_msg.status.fault_code =
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];
msg->body.system_status_msg.data.status.reserved1 =
msg->body.system_status_msg.status.reserved1 =
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;
break;
}
case UART_FRAME_MOTION_STATUS_ID: {
case UART_MSG_MOTION_STATUS_ID: {
msg->type = ScoutMotionStatusMsg;
msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0];
@@ -736,7 +736,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
msg->body.motion_status_msg.data.cmd.reserved3 = 0x00;
break;
}
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: {
case UART_MSG_MOTOR1_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -756,7 +756,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: {
case UART_MSG_MOTOR2_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -776,7 +776,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: {
case UART_MSG_MOTOR3_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -796,7 +796,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: {
case UART_MSG_MOTOR4_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte =
@@ -816,7 +816,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_LIGHT_STATUS_ID: {
case UART_MSG_LIGHT_STATUS_ID: {
msg->type = ScoutLightStatusMsg;
msg->body.light_status_msg.data.status.light_ctrl_enable =
uart_parsing_data.payload_buffer[0];
@@ -838,7 +838,7 @@ bool ConstructStatusMessage(AgxMessage *msg) {
return true;
}
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len) {
uint8_t CalcUartDataChecksum(uint8_t *buf, uint8_t len) {
uint8_t checksum = 0;
#ifdef USE_XOR_CHECKSUM
@@ -854,8 +854,8 @@ uint8_t CalcBufferedFrameChecksum() {
uint8_t checksum = 0x00;
#ifdef USE_XOR_CHECKSUM
checksum ^= UART_FRAME_SOF1;
checksum ^= UART_FRAME_SOF2;
checksum ^= UART_MSG_SOF1;
checksum ^= UART_MSG_SOF2;
checksum ^= uart_parsing_data.frame_len;
checksum ^= uart_parsing_data.frame_type;
checksum ^= uart_parsing_data.frame_id;
@@ -863,8 +863,8 @@ uint8_t CalcBufferedFrameChecksum() {
checksum ^= uart_parsing_data.payload_buffer[i];
checksum ^= uart_parsing_data.frame_cnt;
#else
checksum += UART_FRAME_SOF1;
checksum += UART_FRAME_SOF2;
checksum += UART_MSG_SOF1;
checksum += UART_MSG_SOF2;
checksum += uart_parsing_data.frame_len;
checksum += uart_parsing_data.frame_type;
checksum += uart_parsing_data.frame_id;

29
ugv_sdk/src/mobile_base.cpp
View File

@@ -19,6 +19,7 @@ namespace westonrobot {
MobileBase::~MobileBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
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) {
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->SetReceiveCallback(
@@ -58,35 +64,44 @@ void MobileBase::ConfigureSerial(const std::string uart_name,
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() {
keep_running_ = true;
cmd_thread_ = std::thread(
std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void MobileBase::ControlLoop(int32_t period_ms) {
Timer ctrl_timer;
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) 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) {
ctrl_timer.reset();
Timer tm;
while (keep_running_) {
tm.reset();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++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 {
SendRobotCmd();
}
ctrl_timer.sleep_until_ms(period_ms);
tm.sleep_until_ms(period_ms);
}
}
} // namespace westonrobot