diff --git a/CMakeLists.txt b/CMakeLists.txt index 72573fb..de6b475 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,19 +80,12 @@ add_library(${PROJECT_NAME} src/mobile_robot/scout_robot.cpp ######################## ## protocol v2 support - # parser - src/protocol_v2/agx_msg_parser.c - src/protocol_v2/protocol_v2_parser.cpp - # robots - # src/protocol_v2/scout_base.cpp - # src/protocol_v2/tracer_base.cpp - # src/protocol_v2/ranger_base.cpp + src/protocol_v2/agilex_msg_parser_v2.c + src/protocol_v2/parser_impl/protocol_v2_parser.cpp ######################## - ## legacy protocol v1 support - src/protocol_v1/scout/scout_uart_parser.c - src/protocol_v1/scout/scout_can_parser.c - src/protocol_v1/scout/scout_protocol_v1_parser.cpp - + ## legacy protocol v1 support (transparent to user) + src/protocol_v1/agilex_msg_parser_v1.c + src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp ) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index 345ece7..1b510d0 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -19,9 +19,10 @@ extern "C" { #include #include +#include "ugv_sdk/details/interface/agilex_types.h" + /***************** Control messages *****************/ -// 0x111 typedef struct { float linear_velocity; float angular_velocity; @@ -29,51 +30,22 @@ typedef struct { float steering_angle; } MotionCommandMessage; -// 0x121 -typedef enum { - CONST_OFF = 0x00, - CONST_ON = 0x01, - BREATH = 0x02, - CUSTOM = 0x03 -} LightMode; - -typedef struct { - LightMode mode; - uint8_t custom_value; -} LightOperation; - typedef struct { bool enable_cmd_ctrl; LightOperation front_light; LightOperation rear_light; } LightCommandMessage; -// 0x131 typedef struct { bool enable_braking; } BrakingCommandMessage; -// 0x141 typedef struct { uint8_t motion_mode; } MotionModeMessage; /**************** Feedback messages *****************/ -// 0x211 -typedef enum { - VehicleStateNormal = 0x00, - VehicleStateEStop = 0x01, - VehicleStateException = 0x02 -} VehicleState; - -typedef enum { - // CONTROL_MODE_STANDBY = 0x00, - CONTROL_MODE_RC = 0x00, - CONTROL_MODE_CAN = 0x01, - CONTROL_MODE_UART = 0x02 -} ControlMode; - #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) #define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) #define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) @@ -103,13 +75,6 @@ typedef struct { // 0x231 typedef LightCommandMessage LightStateMessage; -// 0x241 -typedef enum { - RC_SWITCH_UP = 0, - RC_SWITCH_MIDDLE, - RC_SWITCH_DOWN -} RcSwitchState; - typedef struct { RcSwitchState swa; RcSwitchState swb; @@ -122,7 +87,6 @@ typedef struct { int8_t var_a; } RcStateMessage; -// 0x251 - 0x258 typedef struct { uint8_t motor_id; int16_t rpm; @@ -130,6 +94,11 @@ typedef struct { int32_t pulse_count; } ActuatorHSStateMessage; +typedef struct { + uint8_t motion_mode; + uint8_t mode_changing; +} MotionModeFeedbackMessage; + // 0x261 - 0x264 #define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) #define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) @@ -140,12 +109,6 @@ typedef struct { #define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) #define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) -// 0x291 -typedef struct { - uint8_t motion_mode; - uint8_t mode_changing; -} MotionModeFeedbackMessage; - typedef struct { uint8_t motor_id; float driver_voltage; @@ -156,45 +119,38 @@ typedef struct { /***************** Sensor messages ******************/ -// 0x311 typedef struct { float left_wheel; float right_wheel; } OdometryMessage; -// 0x321 typedef struct { float accel_x; float accel_y; float accel_z; } ImuAccelMessage; -// 0x322 typedef struct { float gyro_x; float gyro_y; float gyro_z; } ImuGyroMessage; -// 0x323 typedef struct { float yaw; float pitch; float roll; } ImuEulerMessage; -// 0x331 typedef struct { uint8_t trigger_state; } SafetyBumperMessage; -// 0x340 + num typedef struct { uint8_t sensor_id; uint8_t distance[8]; } UltrasonicMessage; -// 0x350 + num typedef struct { uint8_t sensor_id; float relative_distance; @@ -203,7 +159,6 @@ typedef struct { int8_t channels[3]; } UwbMessage; -// 0x361 typedef struct { uint8_t battery_soc; uint8_t battery_soh; @@ -250,12 +205,10 @@ typedef struct { /************ Query/config messages ****************/ -// 0x411 typedef struct { bool request; } VersionRequestMessage; -// 0x41a typedef struct { uint16_t controller_hw_version; uint16_t motor_driver_hw_version; @@ -263,22 +216,18 @@ typedef struct { uint16_t motor_driver_sw_version; } VersionResponseMessage; -// 0x421 typedef struct { ControlMode mode; } ControlModeConfigMessage; -// 0x431 typedef struct { bool set_as_neutral; } SteerNeutralRequestMessage; -// 0x43a typedef struct { bool neutral_set_successful; } SteerNeutralResponseMessage; -// 0x441 typedef enum { CLEAR_ALL_FAULT = 0x00, CLEAR_MOTOR1_FAULT = 0x01, diff --git a/include/ugv_sdk/details/interface/agilex_types.h b/include/ugv_sdk/details/interface/agilex_types.h new file mode 100644 index 0000000..627738c --- /dev/null +++ b/include/ugv_sdk/details/interface/agilex_types.h @@ -0,0 +1,54 @@ +/* + * agilex_types.h + * + * Created on: Jul 09, 2021 21:57 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_TYPES_H +#define AGILEX_TYPES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef enum { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 +} LightMode; + +typedef struct { + LightMode mode; + uint8_t custom_value; +} LightOperation; + +typedef enum { + VehicleStateNormal = 0x00, + VehicleStateEStop = 0x01, + VehicleStateException = 0x02 +} VehicleState; + +typedef enum { + // CONTROL_MODE_STANDBY = 0x00, + CONTROL_MODE_RC = 0x00, + CONTROL_MODE_CAN = 0x01, + CONTROL_MODE_UART = 0x02 +} ControlMode; + +typedef enum { + RC_SWITCH_UP = 0, + RC_SWITCH_MIDDLE, + RC_SWITCH_DOWN +} RcSwitchState; + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_TYPES_H */ diff --git a/src/protocol_v2/ranger_base.cpp b/include/ugv_sdk/details/robot_base/ranger_base.cpp similarity index 100% rename from src/protocol_v2/ranger_base.cpp rename to include/ugv_sdk/details/robot_base/ranger_base.cpp diff --git a/src/protocol_v2/tracer_base.cpp b/include/ugv_sdk/details/robot_base/tracer_base.cpp similarity index 100% rename from src/protocol_v2/tracer_base.cpp rename to include/ugv_sdk/details/robot_base/tracer_base.cpp diff --git a/include/ugv_sdk/details/protocol_v1/agilex_message_v1.h b/src/protocol_v1/agilex_message_v1.h similarity index 62% rename from include/ugv_sdk/details/protocol_v1/agilex_message_v1.h rename to src/protocol_v1/agilex_message_v1.h index bd03877..a3971c4 100644 --- a/include/ugv_sdk/details/protocol_v1/agilex_message_v1.h +++ b/src/protocol_v1/agilex_message_v1.h @@ -21,37 +21,17 @@ extern "C" { #include #include +#include "ugv_sdk/details/interface/agilex_types.h" + /***************** Control messages *****************/ -typedef enum { - // CONTROL_MODE_STANDBY = 0x00, - CONTROL_MODE_RC = 0x00, - CONTROL_MODE_CAN = 0x01, - CONTROL_MODE_UART = 0x02 -} ControlMode; - -// 0x130 typedef struct { ControlMode control_mode; - uint8_t fault_clear; - + bool clear_all_error; float linear; float angular; } MotionCommandMessage; -// 0x140 -typedef enum { - CONST_OFF = 0x00, - CONST_ON = 0x01, - BREATH = 0x02, - CUSTOM = 0x03 -} LightMode; - -typedef struct { - LightMode mode; - uint8_t custom_value; -} LightOperation; - typedef struct { bool enable_cmd_ctrl; LightOperation front_light; @@ -64,12 +44,6 @@ typedef struct { /**************** Feedback messages *****************/ -typedef enum { - VehicleStateNormal = 0x00, - VehicleStateEStop = 0x01, - VehicleStateException = 0x02 -} VehicleState; - #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) #define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) #define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) @@ -88,49 +62,19 @@ typedef struct { uint16_t error_code; } SystemStateMessage; -// 0x221 typedef struct { float linear_velocity; - float angular_velocity; // only valid for differential drivering - float lateral_velocity; - float steering_angle; // only valid for ackermann steering + float angular_velocity; } MotionStateMessage; -// 0x231 typedef LightCommandMessage LightStateMessage; -// 0x251 - 0x258 typedef struct { - uint8_t motor_id; - int16_t rpm; float current; - int32_t pulse_count; + int16_t rpm; + float temperature; } ActuatorStateMessage; -// 0x261 - 0x264 -#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) -#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) -#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04) -#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08) -#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10) -#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20) -#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) -#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) - -// 0x291 -typedef struct { - uint8_t motion_mode; - uint8_t mode_changing; -} MotionModeFeedbackMessage; - -typedef struct { - uint8_t motor_id; - float driver_voltage; - float driver_temperature; - int8_t motor_temperature; - uint8_t driver_state; -} ActuatorLSStateMessage; - ////////////////////////////////////////////////////// typedef enum { diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c new file mode 100644 index 0000000..b4b6117 --- /dev/null +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -0,0 +1,18 @@ +/* + * agilex_msg_parser.c + * + * Created on: Jul 09, 2021 22:04 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "agilex_protocol_v1.h" +#include "protocol_v1/agilex_msg_parser_v1.h" + +#include "stdio.h" +#include "string.h" + +bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg) {} +void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame) {} +uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {} diff --git a/src/protocol_v1/agilex_msg_parser_v1.h b/src/protocol_v1/agilex_msg_parser_v1.h new file mode 100644 index 0000000..0649113 --- /dev/null +++ b/src/protocol_v1/agilex_msg_parser_v1.h @@ -0,0 +1,41 @@ +/* + * agilex_msg_parser.h + * + * Created on: Jul 09, 2021 22:04 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_MSG_PARSER_H +#define AGILEX_MSG_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#ifdef __linux__ +#include +#else +struct can_frame { + uint32_t can_id; + uint8_t can_dlc; + uint8_t data[8] __attribute__((aligned(8))); +}; +#endif + +#include "protocol_v1/agilex_message_v1.h" + +bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg); +void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame); +uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc); + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_MSG_PARSER_H */ diff --git a/src/protocol_v1/agx_protocol_v1.h b/src/protocol_v1/agilex_protocol_v1.h similarity index 97% rename from src/protocol_v1/agx_protocol_v1.h rename to src/protocol_v1/agilex_protocol_v1.h index d367979..8a84097 100644 --- a/src/protocol_v1/agx_protocol_v1.h +++ b/src/protocol_v1/agilex_protocol_v1.h @@ -1,5 +1,5 @@ /* - * agx_protocol_v1.h + * agilex_protocol_v1.h * * Created on: Jul 09, 2021 20:34 * Description: @@ -7,8 +7,8 @@ * Copyright (c) 2021 Ruixiang Du (rdu) */ -#ifndef AGX_PROTOCOL_V1_H -#define AGX_PROTOCOL_V1_H +#ifndef AGILEX_PROTOCOL_V1_H +#define AGILEX_PROTOCOL_V1_H #ifdef __cplusplus extern "C" { @@ -194,4 +194,4 @@ typedef struct { } #endif -#endif /* AGX_PROTOCOL_V1_H */ +#endif /* AGILEX_PROTOCOL_V1_H */ diff --git a/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp b/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp new file mode 100644 index 0000000..981a5a5 --- /dev/null +++ b/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp @@ -0,0 +1,111 @@ +/* + * scout_protocol_v1_parser.cpp + * + * Created on: Jul 08, 2021 22:43 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp" + +namespace westonrobot { +// CAN support +bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) { + // ScoutMessage scout_msg; + // if ScoutMessage found, convert to AgxMessage + // if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) { + // switch (scout_msg.type) { + // case ScoutMotionStatusMsg: { + // break; + // } + // case ScoutLightStatusMsg: { + // break; + // } + // case ScoutSystemStatusMsg: { + // break; + // } + // case ScoutMotorDriverStatusMsg: { + // break; + // } + // } + // } + return false; +} + +void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) { + // ScoutMessage scout_msg; + // convert to ScoutMessage, then encode to can frame + // switch (msg->type) { + // case AgxMsgMotionCommand: { + // // scout_msg.type = ScoutMotionControlMsg; + // // scout_msg.body.motion_control_msg.data.cmd.control_mode = + // // CTRL_MODE_COMMANDED; + // // scout_msg.body.motion_control_msg.data.cmd.control_mode = + // // CTRL_MODE_CMD_CAN; + // // scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag = + // 0x00; + + // /* + // std::lock_guard guard(motion_cmd_mutex_); + // current_motion_cmd_.linear_velocity = static_cast( + // linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0); + // current_motion_cmd_.angular_velocity = static_cast( + // angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0); + // */ + // // scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = + // // current_motion_cmd_.linear_velocity; + // // scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = + // // current_motion_cmd_.angular_velocity; + // // scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0; + // // scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0; + // // scout_msg.body.motion_control_msg.data.cmd.count = count; + // // scout_msg.body.motion_control_msg.data.cmd.checksum = + // // CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, + // // scout_msg.body.motion_control_msg.data.raw, + // 8); + // break; + // } + // case AgxMsgLightCommand: { + // scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable = + // LIGHT_ENABLE_CTRL; + + // // scout_msg.body.light_control_msg.data.cmd.front_mode = + // // static_cast(current_light_cmd_.front_mode); + // // scout_msg.body.light_control_msg.data.cmd.front_custom = + // // current_light_cmd_.front_custom_value; + // // scout_msg.body.light_control_msg.data.cmd.rear_mode = + // // static_cast(current_light_cmd_.rear_mode); + // // scout_msg.body.light_control_msg.data.cmd.rear_custom = + // // current_light_cmd_.rear_custom_value; + // // scout_msg.body.light_control_msg.data.cmd.reserved0 = 0; + // // scout_msg.body.light_control_msg.data.cmd.count = count; + + // scout_msg.body.light_control_msg.data.cmd.checksum = + // CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, + // scout_msg.body.light_control_msg.data.raw, 8); + + // break; + // } + // } +} + +uint8_t ScoutProtocolV1Parser::CalculateChecksum(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; +} + +// UART support +bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc, + AgxMessage *msg) {} + +void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf, + uint8_t *len) {} + +uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {} +} // namespace westonrobot diff --git a/src/protocol_v1/scout/scout_can_parser.c b/src/protocol_v1/scout/scout_can_parser.c deleted file mode 100644 index 8f23aa4..0000000 --- a/src/protocol_v1/scout/scout_can_parser.c +++ /dev/null @@ -1,178 +0,0 @@ -/* - * scout_can_parser.c - * - * Created on: Aug 31, 2019 04:25 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "protocol_v1/scout/scout_can_parser.h" - -#include "string.h" - -static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); -static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame); - -bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *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: - { - msg->type = ScoutMotionStatusMsg; - // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; - memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_LIGHT_CONTROL_STATUS_ID: - { - msg->type = ScoutLightStatusMsg; - // msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_SYSTEM_STATUS_STATUS_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, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR1_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR2_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR3_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR4_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_CONTROL_CMD_ID: - { - msg->type = ScoutMotionControlMsg; - // msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID; - memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_LIGHT_CONTROL_CMD_ID: - { - msg->type = ScoutLightControlMsg; - // msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - default: - break; - } - - return true; -} - -void EncodeScoutMsgToCAN(const ScoutMessage *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_dlc = 8; - memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutLightStatusMsg: - { - tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutSystemStatusMsg: - { - tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) - tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) - tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) - tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) - tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutMotionControlMsg: - { - EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); - break; - } - case ScoutLightControlMsg: - { - EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); - break; - } - default: - break; - } - tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} - -void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame) -{ - tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID; - 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); -} - -void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame) -{ - tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID; - 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); -} - -uint8_t CalcScoutCANChecksum(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; -} \ No newline at end of file diff --git a/src/protocol_v1/scout/scout_can_parser.h b/src/protocol_v1/scout/scout_can_parser.h deleted file mode 100644 index 14ae740..0000000 --- a/src/protocol_v1/scout/scout_can_parser.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * scout_can_parser.h - * - * Created on: Aug 31, 2019 04:23 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_CAN_PARSER_H -#define SCOUT_CAN_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -#include "protocol_v1/scout/scout_protocol.h" - -#ifdef __linux__ -#include -#else -struct can_frame -{ - uint32_t can_id; - uint8_t can_dlc; - uint8_t data[8]__attribute__((aligned(8))); -}; -#endif - -bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg); -void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame); - -uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); - -#ifdef __cplusplus -} -#endif - -#endif /* SCOUT_CAN_PARSER_H */ diff --git a/src/protocol_v1/scout/scout_protocol.h b/src/protocol_v1/scout/scout_protocol.h deleted file mode 100644 index a296af1..0000000 --- a/src/protocol_v1/scout/scout_protocol.h +++ /dev/null @@ -1,254 +0,0 @@ -/* - * scout_protocol.h - * - * Created on: Aug 07, 2019 21:49 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_PROTOCOL_H -#define SCOUT_PROTOCOL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define SCOUT_CMD_BUF_LEN 32 -#define SCOUT_STATUS_BUF_LEN 32 -#define SCOUT_FRAME_SIZE 13 - -#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 UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) -#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) - -// CAN Definitions -#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130) -#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131) -#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140) -#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141) -#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) -#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200) -#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201) -#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202) -#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203) - -/*--------------------- Control/State Constants ------------------------*/ - -// Motion Control -// #define CTRL_MODE_REMOTE ((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) -#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02) -#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03) -#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04) -#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05) -#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06) -#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07) -#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08) - -// Light Control -#define LIGHT_DISABLE_CTRL ((uint8_t)0x00) -#define LIGHT_ENABLE_CTRL ((uint8_t)0x01) - -#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00) -#define LIGHT_MODE_CONST_ON ((uint8_t)0x01) -#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) - -#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 FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) -#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) -#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004) -#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008) -#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010) -#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020) -#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040) -#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080) - -/*-------------------- Control/Feedback Messages -----------------------*/ - -/* No padding in the struct */ -// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect -#pragma pack(push, 1) - -// Note: id could be different for UART and CAN protocol -// Motion Control -typedef struct { - union { - struct { - uint8_t control_mode; - uint8_t fault_clear_flag; - int8_t linear_velocity_cmd; - int8_t angular_velocity_cmd; - uint8_t reserved0; - uint8_t reserved1; - uint8_t count; - uint8_t checksum; - } cmd; - uint8_t raw[8]; - } data; -} MotionControlMessage; - -typedef struct { - union { - struct { - struct { - uint8_t high_byte; - uint8_t low_byte; - } linear_velocity; - struct { - uint8_t high_byte; - uint8_t low_byte; - } angular_velocity; - uint8_t reserved0; - uint8_t reserved1; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} MotionStatusMessage; - -// 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; - struct { - uint8_t high_byte; - uint8_t low_byte; - } fault_code; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} SystemStatusMessage; - -// Light Control -typedef struct { - union { - struct { - uint8_t light_ctrl_enable; - uint8_t front_mode; - uint8_t front_custom; - uint8_t rear_mode; - uint8_t rear_custom; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; - } cmd; - uint8_t raw[8]; - } data; -} LightControlMessage; - -typedef struct { - union { - struct { - uint8_t light_ctrl_enable; - uint8_t front_mode; - uint8_t front_custom; - uint8_t rear_mode; - uint8_t rear_custom; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} LightStatusMessage; - -// Motor Driver Feedback -typedef struct { - uint8_t motor_id; - union { - struct { - struct { - uint8_t high_byte; - uint8_t low_byte; - } current; - struct { - uint8_t high_byte; - uint8_t low_byte; - } rpm; - int8_t temperature; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} MotorDriverStatusMessage; - -// For convenience to access status/control message -typedef enum { - ScoutMsgNone = 0x00, - // status messages - ScoutMotionStatusMsg = 0x01, - ScoutLightStatusMsg = 0x02, - ScoutSystemStatusMsg = 0x03, - ScoutMotorDriverStatusMsg = 0x04, - // control messages - ScoutMotionControlMsg = 0x21, - ScoutLightControlMsg = 0x22 -} ScoutMsgType; - -typedef struct { - ScoutMsgType type; - union { - // status messages - MotionStatusMessage motion_status_msg; - LightStatusMessage light_status_msg; - SystemStatusMessage system_status_msg; - MotorDriverStatusMessage motor_driver_status_msg; - // control messages - MotionControlMessage motion_control_msg; - LightControlMessage light_control_msg; - } body; -} ScoutMessage; - -#pragma pack(pop) - -#ifdef __cplusplus -} -#endif - -#endif /* SCOUT_PROTOCOL_H */ diff --git a/src/protocol_v1/scout/scout_protocol_v1_parser.cpp b/src/protocol_v1/scout/scout_protocol_v1_parser.cpp deleted file mode 100644 index 108f6ed..0000000 --- a/src/protocol_v1/scout/scout_protocol_v1_parser.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * scout_protocol_v1_parser.cpp - * - * Created on: Jul 08, 2021 22:43 - * Description: - * - * Copyright (c) 2021 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp" - -#include "protocol_v1/scout/scout_can_parser.h" - -namespace westonrobot { -// CAN support -bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame, - AgxMessage *msg) { - ScoutMessage scout_msg; - // if ScoutMessage found, convert to AgxMessage - if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) { - switch (scout_msg.type) { - case ScoutMotionStatusMsg: { - break; - } - case ScoutLightStatusMsg: { - break; - } - case ScoutSystemStatusMsg: { - break; - } - case ScoutMotorDriverStatusMsg: { - break; - } - } - } - return false; -} - -void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, - struct can_frame *tx_frame) { - ScoutMessage scout_msg; - // convert to ScoutMessage, then encode to can frame - switch (msg->type) { - case AgxMsgMotionCommand: { - // scout_msg.type = ScoutMotionControlMsg; - // scout_msg.body.motion_control_msg.data.cmd.control_mode = - // CTRL_MODE_COMMANDED; - // scout_msg.body.motion_control_msg.data.cmd.control_mode = - // CTRL_MODE_CMD_CAN; - // scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag = 0x00; - - /* - std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = static_cast( - linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast( - angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0); - */ - // scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = - // current_motion_cmd_.linear_velocity; - // scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = - // current_motion_cmd_.angular_velocity; - // scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0; - // scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0; - // scout_msg.body.motion_control_msg.data.cmd.count = count; - // scout_msg.body.motion_control_msg.data.cmd.checksum = - // CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, - // scout_msg.body.motion_control_msg.data.raw, 8); - break; - } - case AgxMsgLightCommand: { - scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable = - LIGHT_ENABLE_CTRL; - - // scout_msg.body.light_control_msg.data.cmd.front_mode = - // static_cast(current_light_cmd_.front_mode); - // scout_msg.body.light_control_msg.data.cmd.front_custom = - // current_light_cmd_.front_custom_value; - // scout_msg.body.light_control_msg.data.cmd.rear_mode = - // static_cast(current_light_cmd_.rear_mode); - // scout_msg.body.light_control_msg.data.cmd.rear_custom = - // current_light_cmd_.rear_custom_value; - // scout_msg.body.light_control_msg.data.cmd.reserved0 = 0; - // scout_msg.body.light_control_msg.data.cmd.count = count; - - scout_msg.body.light_control_msg.data.cmd.checksum = - CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, - scout_msg.body.light_control_msg.data.raw, 8); - - break; - } - } -} - -uint8_t ScoutProtocolV1Parser::CalculateChecksum(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; -} - -// UART support -bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc, - AgxMessage *msg) {} - -void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf, - uint8_t *len) {} - -uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {} -} // namespace westonrobot diff --git a/src/protocol_v1/scout/scout_uart_parser.c b/src/protocol_v1/scout/scout_uart_parser.c deleted file mode 100644 index b861419..0000000 --- a/src/protocol_v1/scout/scout_uart_parser.c +++ /dev/null @@ -1,629 +0,0 @@ -/* - * scout_uart_parser.c - * - * Created on: Aug 14, 2019 12:02 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "protocol_v1/scout/scout_uart_parser.h" - -// #define USE_XOR_CHECKSUM - -// #define PRINT_CPP_DEBUG_INFO -// #define PRINT_JLINK_DEBUG_INFO - -#ifdef PRINT_CPP_DEBUG_INFO -#undef PRINT_JLINK_DEBUG_INFO -#endif - -#ifdef PRINT_CPP_DEBUG_INFO -#define -#elif (defined(PRINT_JLINK_DEBUG_INFO)) -#include "segger/jlink_rtt.h" -#endif - -typedef enum -{ - WAIT_FOR_SOF1 = 0, - WAIT_FOR_SOF2, - WAIT_FOR_FRAME_LEN, - WAIT_FOR_FRAME_TYPE, - WAIT_FOR_FRAME_ID, - WAIT_FOR_PAYLOAD, - WAIT_FOR_FRAME_COUNT, - WAIT_FOR_CHECKSUM -} ScoutSerialDecodeState; - -#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2) - -#define FRAME_SOF_LEN ((uint8_t)2) -#define FRAME_FIXED_FIELD_LEN ((uint8_t)4) - -#define FRAME_SOF1 ((uint8_t)0x5a) -#define FRAME_SOF2 ((uint8_t)0xa5) - -#define FRAME_TYPE_CONTROL ((uint8_t)0x55) -#define FRAME_TYPE_STATUS ((uint8_t)0xaa) - -#define FRAME_NONE_ID ((uint8_t)0x00) - -// frame buffer -static struct -{ - uint8_t frame_id; - uint8_t frame_type; - uint8_t frame_len; - uint8_t frame_cnt; - uint8_t frame_checksum; - uint8_t internal_checksum; - uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE]; - size_t payload_data_pos; -} uart_parsing_data; - -// statisctics -typedef struct -{ - uint32_t frame_parsed; - uint32_t frame_with_wrong_checksum; -} UARTParsingStats; - -static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123}; - -// internal functions -static bool ParseChar(uint8_t c, ScoutMessage *msg); -static uint8_t CalcBufferedFrameChecksum(); -static bool ConstructStatusMessage(ScoutMessage *msg); -static bool ConstructControlMessage(ScoutMessage *msg); - -static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len); -static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len); - -void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_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[5] = msg->body.motion_status_msg.data.status.linear_velocity.high_byte; - buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte; - buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte; - buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte; - buf[9] = 0; - buf[10] = 0; - buf[11] = msg->body.motion_status_msg.data.status.count; - break; - } - case ScoutLightStatusMsg: - { - buf[4] = UART_FRAME_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_mode; - buf[7] = msg->body.light_status_msg.data.status.front_custom; - buf[8] = msg->body.light_status_msg.data.status.rear_mode; - buf[9] = msg->body.light_status_msg.data.status.rear_custom; - buf[10] = 0; - buf[11] = msg->body.light_status_msg.data.status.count; - 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[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.high_byte; - buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte; - buf[11] = msg->body.system_status_msg.data.status.count; - break; - } - case ScoutMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) - buf[4] = UART_FRAME_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; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) - buf[4] = UART_FRAME_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[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; - buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte; - buf[9] = msg->body.motor_driver_status_msg.data.status.temperature; - buf[10] = 0; - buf[11] = msg->body.motor_driver_status_msg.data.status.count; - break; - } - case ScoutMotionControlMsg: - { - EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len); - break; - } - case ScoutLightControlMsg: - { - EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len); - break; - } - default: - break; - } - - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg) -{ - static ScoutMessage decoded_msg; - - bool result = ParseChar(c, &decoded_msg); - if (result) - *msg = decoded_msg; - return result; -} - -void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_TYPE_CONTROL; - buf[4] = UART_FRAME_MOTION_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.control_mode; - buf[6] = msg->data.cmd.fault_clear_flag; - buf[7] = msg->data.cmd.linear_velocity_cmd; - buf[8] = msg->data.cmd.angular_velocity_cmd; - buf[9] = 0x00; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_TYPE_CONTROL; - buf[4] = UART_FRAME_LIGHT_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.light_ctrl_enable; - buf[6] = msg->data.cmd.front_mode; - buf[7] = msg->data.cmd.front_custom; - buf[8] = msg->data.cmd.rear_mode; - buf[9] = msg->data.cmd.rear_custom; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool ParseChar(uint8_t c, ScoutMessage *msg) -{ - static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1; - - bool new_frame_parsed = false; - switch (decode_state) - { - case WAIT_FOR_SOF1: - { - if (c == FRAME_SOF1) - { - uart_parsing_data.frame_id = FRAME_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, PAYLOAD_BUFFER_SIZE); - - decode_state = WAIT_FOR_SOF2; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof1" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof1\n"); -#endif - } - break; - } - case WAIT_FOR_SOF2: - { - if (c == FRAME_SOF2) - { - decode_state = WAIT_FOR_FRAME_LEN; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof2\n"); -#endif - } - else - { - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "failed to find sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "failed to find sof2\n"); -#endif - } - break; - } - case WAIT_FOR_FRAME_LEN: - { - uart_parsing_data.frame_len = c; - decode_state = WAIT_FOR_FRAME_TYPE; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame len: " << std::hex << static_cast(frame_len) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame len: %d\n", frame_len); -#endif - break; - } - case WAIT_FOR_FRAME_TYPE: - { - switch (c) - { - case FRAME_TYPE_CONTROL: - { - uart_parsing_data.frame_type = FRAME_TYPE_CONTROL; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "control type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "control type frame received\n"); -#endif - break; - } - case FRAME_TYPE_STATUS: - { - uart_parsing_data.frame_type = FRAME_TYPE_STATUS; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "status type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "status type frame received\n"); -#endif - break; - } - default: - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - 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: - { - uart_parsing_data.frame_id = c; - decode_state = WAIT_FOR_PAYLOAD; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame id: " << std::hex << static_cast(frame_id) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame id: %d\n", frame_id); -#endif - break; - } - default: - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Unknown frame id" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "ERROR: Unknown frame id\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - case WAIT_FOR_PAYLOAD: - { - uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "1 byte added: " << std::hex << static_cast(c) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "1 byte added: %d\n", c); -#endif - if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN)) - decode_state = WAIT_FOR_FRAME_COUNT; - break; - } - case WAIT_FOR_FRAME_COUNT: - { - uart_parsing_data.frame_cnt = c; - decode_state = WAIT_FOR_CHECKSUM; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame count: " << std::hex << static_cast(frame_cnt) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt); -#endif - break; - } - case WAIT_FOR_CHECKSUM: - { - uart_parsing_data.frame_checksum = c; - uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum(); - new_frame_parsed = true; - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum); - JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum); -#endif - break; - } - default: - break; - } - - if (new_frame_parsed) - { - if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum) - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum correct" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum correct\n"); -#endif - if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS) - ConstructStatusMessage(msg); - else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL) - ConstructControlMessage(msg); - ++uart_parsing_stats.frame_parsed; - } - else - { - ++uart_parsing_stats.frame_with_wrong_checksum; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum is NOT correct" << std::endl; - std::cout << std::hex << static_cast(frame_id) << " , " << static_cast(frame_len) << " , " << static_cast(frame_cnt) << " , " << static_cast(frame_checksum) << " : " << std::dec << std::endl; - std::cout << "payload: "; - for (int i = 0; i < payload_data_pos; ++i) - std::cout << std::hex << static_cast(payload_buffer[i]) << std::dec << " "; - std::cout << std::endl; - std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum is NOT correct\n"); -#endif - } - } - - return new_frame_parsed; -} - -bool ConstructControlMessage(ScoutMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_MOTION_CONTROL_ID: - { - msg->type = ScoutMotionControlMsg; - msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0]; - msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1]; - msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2]; - msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3]; - msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4]; - msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5]; - msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt; - msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_LIGHT_CONTROL_ID: - { - msg->type = ScoutLightControlMsg; - msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_control_msg.data.cmd.front_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_control_msg.data.cmd.front_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_control_msg.data.cmd.rear_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_control_msg.data.cmd.rear_custom = uart_parsing_data.payload_buffer[4]; - msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5]; - msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt; - msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum; - break; - } - } - return true; -} - -bool ConstructStatusMessage(ScoutMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_SYSTEM_STATUS_ID: - { - msg->type = ScoutSystemStatusMsg; - msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0]; - msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1]; - msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4]; - msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5]; - msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_MOTION_STATUS_ID: - { - msg->type = ScoutMotionStatusMsg; - msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0]; - msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motion_status_msg.data.status.reserved0 = 0x00; - msg->body.motion_status_msg.data.status.reserved0 = 0x00; - msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_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 = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_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 = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_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 = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_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 = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_LIGHT_STATUS_ID: - { - msg->type = ScoutLightStatusMsg; - msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_status_msg.data.status.front_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_status_msg.data.status.front_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_status_msg.data.status.rear_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_status_msg.data.status.rear_custom = uart_parsing_data.payload_buffer[4]; - msg->body.light_status_msg.data.status.reserved0 = 0x00; - msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - } - return true; -} - -uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len) -{ - uint8_t checksum = 0; - -#ifdef USE_XOR_CHECKSUM - for (int i = 0; i < len; ++i) - checksum ^= buf[i]; -#else - for (int i = 0; i < len; ++i) - checksum += buf[i]; -#endif - - return checksum; -} - -uint8_t CalcBufferedFrameChecksum() -{ - uint8_t checksum = 0x00; - -#ifdef USE_XOR_CHECKSUM - checksum ^= FRAME_SOF1; - checksum ^= FRAME_SOF2; - checksum ^= uart_parsing_data.frame_len; - checksum ^= uart_parsing_data.frame_type; - checksum ^= uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum ^= uart_parsing_data.payload_buffer[i]; - checksum ^= uart_parsing_data.frame_cnt; -#else - checksum += FRAME_SOF1; - checksum += FRAME_SOF2; - checksum += uart_parsing_data.frame_len; - checksum += uart_parsing_data.frame_type; - checksum += uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum += uart_parsing_data.payload_buffer[i]; - checksum += uart_parsing_data.frame_cnt; -#endif - - return checksum; -} diff --git a/src/protocol_v1/scout/scout_uart_parser.h b/src/protocol_v1/scout/scout_uart_parser.h deleted file mode 100644 index 58f65a6..0000000 --- a/src/protocol_v1/scout/scout_uart_parser.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * scout_uart_parser.h - * - * Created on: Aug 14, 2019 12:01 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_UART_PARSER_H -#define SCOUT_UART_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -#include "protocol_v1/scout/scout_protocol.h" - -bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg); -void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len); - -uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len); - -#ifdef __cplusplus -} -#endif - -#endif /* SCOUT_UART_PARSER_H */ diff --git a/src/protocol_v2/agx_msg_parser.c b/src/protocol_v2/agilex_msg_parser_v2.c similarity index 99% rename from src/protocol_v2/agx_msg_parser.c rename to src/protocol_v2/agilex_msg_parser_v2.c index b05655f..b4446e4 100644 --- a/src/protocol_v2/agx_msg_parser.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -7,8 +7,8 @@ * Copyright (c) 2019 Weston Robot Pte. Ltd. */ -#include "agx_protocol_v2.h" -#include "protocol_v2/agilex_msg_parser.h" +#include "agilex_protocol_v2.h" +#include "protocol_v2/agilex_msg_parser_v2.h" #include "stdio.h" #include "string.h" diff --git a/src/protocol_v2/agilex_msg_parser.h b/src/protocol_v2/agilex_msg_parser_v2.h similarity index 100% rename from src/protocol_v2/agilex_msg_parser.h rename to src/protocol_v2/agilex_msg_parser_v2.h diff --git a/src/protocol_v2/agx_protocol_v2.h b/src/protocol_v2/agilex_protocol_v2.h similarity index 98% rename from src/protocol_v2/agx_protocol_v2.h rename to src/protocol_v2/agilex_protocol_v2.h index db7f149..7cc2f28 100644 --- a/src/protocol_v2/agx_protocol_v2.h +++ b/src/protocol_v2/agilex_protocol_v2.h @@ -1,5 +1,5 @@ /* - * agx_protocol_v2.h + * agilex_protocol_v2.h * * Created on: Nov 04, 2020 13:54 * Description: @@ -7,8 +7,8 @@ * Copyright (c) 2020 Weston Robot Pte. Ltd. */ -#ifndef AGX_PROTOCOL_V2_H -#define AGX_PROTOCOL_V2_H +#ifndef AGILEX_PROTOCOL_V2_H +#define AGILEX_PROTOCOL_V2_H #ifdef __cplusplus extern "C" { @@ -399,4 +399,4 @@ typedef struct { } #endif -#endif /* AGX_PROTOCOL_V2_H */ +#endif /* AGILEX_PROTOCOL_V2_H */ diff --git a/src/protocol_v2/protocol_v2_parser.cpp b/src/protocol_v2/parser_impl/protocol_v2_parser.cpp similarity index 94% rename from src/protocol_v2/protocol_v2_parser.cpp rename to src/protocol_v2/parser_impl/protocol_v2_parser.cpp index 9ead7bf..c85a8d4 100644 --- a/src/protocol_v2/protocol_v2_parser.cpp +++ b/src/protocol_v2/parser_impl/protocol_v2_parser.cpp @@ -8,7 +8,7 @@ */ #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" -#include "protocol_v2/agilex_msg_parser.h" +#include "protocol_v2/agilex_msg_parser_v2.h" namespace westonrobot { bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,