From 87cd3665aa5fdf5e50e4fbcfb0a737c9e6ed1209 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 5 Nov 2020 19:12:51 +0800 Subject: [PATCH] saved work on protocol v2 for tracer --- ugv_sdk/CMakeLists.txt | 4 +- ugv_sdk/apps/CMakeLists.txt | 12 +- ugv_sdk/apps/tracer_demo/tracer_demo.cpp | 173 +++--- .../include/ugv_sdk/proto/agx_msg_parser.h | 42 +- .../include/ugv_sdk/proto/agx_protocol_v2.h | 454 ++++++-------- .../include/ugv_sdk/tracer/tracer_base.hpp | 108 ++-- .../ugv_sdk/tracer/tracer_can_parser.h | 42 -- .../include/ugv_sdk/tracer/tracer_protocol.h | 527 ---------------- .../include/ugv_sdk/tracer/tracer_types.hpp | 194 ++---- .../ugv_sdk/tracer/tracer_uart_parser.h | 24 - ugv_sdk/src/agx_msg_parser.c | 329 +++++----- ugv_sdk/src/tracer_base.cpp | 580 ++++++----------- ugv_sdk/src/tracer_can_parser.c | 140 ----- ugv_sdk/src/tracer_uart_parser.c | 587 ------------------ 14 files changed, 752 insertions(+), 2464 deletions(-) delete mode 100644 ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h delete mode 100644 ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h delete mode 100644 ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h delete mode 100644 ugv_sdk/src/tracer_can_parser.c delete mode 100644 ugv_sdk/src/tracer_uart_parser.c diff --git a/ugv_sdk/CMakeLists.txt b/ugv_sdk/CMakeLists.txt index eb85b52..9214ffd 100755 --- a/ugv_sdk/CMakeLists.txt +++ b/ugv_sdk/CMakeLists.txt @@ -90,7 +90,7 @@ add_library(${PROJECT_NAME} # src/scout_base.cpp # src/scout_can_parser.c # src/scout_uart_parser.c - # src/tracer_base.cpp + src/tracer_base.cpp # src/tracer_can_parser.c # src/tracer_uart_parser.c # src/bunker_base.cpp @@ -119,7 +119,7 @@ if(BUILD_WITHOUT_ROS) endif() # add app source directory - # add_subdirectory(apps) + add_subdirectory(apps) endif() # Build tests diff --git a/ugv_sdk/apps/CMakeLists.txt b/ugv_sdk/apps/CMakeLists.txt index 9263416..4747016 100755 --- a/ugv_sdk/apps/CMakeLists.txt +++ b/ugv_sdk/apps/CMakeLists.txt @@ -2,17 +2,17 @@ #find_package(LIBRARY_NAME REQUIRED) # tests -add_executable(app_scout_demo scout_demo/scout_demo.cpp) -target_link_libraries(app_scout_demo ugv_sdk) +# add_executable(app_scout_demo scout_demo/scout_demo.cpp) +# target_link_libraries(app_scout_demo ugv_sdk) # add_executable(app_scout_discharge scout_demo/scout_discharge.cpp) # target_link_libraries(app_scout_discharge ugv_sdk) -add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) -target_link_libraries(app_hunter_demo ugv_sdk) +# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) +# target_link_libraries(app_hunter_demo ugv_sdk) -# add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) -# target_link_libraries(app_tracer_demo ugv_sdk) +add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) +target_link_libraries(app_tracer_demo ugv_sdk) # if(BUILD_MONITOR) # add_subdirectory(scout_monitor) diff --git a/ugv_sdk/apps/tracer_demo/tracer_demo.cpp b/ugv_sdk/apps/tracer_demo/tracer_demo.cpp index 33aad3c..7fc8cd6 100644 --- a/ugv_sdk/apps/tracer_demo/tracer_demo.cpp +++ b/ugv_sdk/apps/tracer_demo/tracer_demo.cpp @@ -1,108 +1,97 @@ -/* +/* * demo_tracer_can.cpp - * + * * Created on: Jun 12, 2019 05:03 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "tracer_base/tracer_base.hpp" +#include "ugv_sdk/tracer/tracer_base.hpp" using namespace westonrobot; -int main(int argc, char **argv) -{ - std::string device_name; - int32_t baud_rate = 0; +int main(int argc, char **argv) { + std::string device_name; + int32_t baud_rate = 0; - if (argc == 2) - { - device_name = {argv[1]}; - std::cout << "Specified CAN: " << device_name << std::endl; - } - else - { - std::cout << "Usage: app_tracer_demo " << std::endl - << "Example 1: ./app_tracer_demo can0" << std::endl; - return -1; - } + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_tracer_demo " << std::endl + << "Example 1: ./app_tracer_demo can0" << std::endl; + return -1; + } - TracerBase tracer; - tracer.Connect(device_name); + TracerBase tracer; + tracer.Connect(device_name); - // light control - std::cout << "Light: const off" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, TracerLightCmd::LightMode::CONST_OFF, 0}); - sleep(3); - std::cout << "Light: const on" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, TracerLightCmd::LightMode::CONST_ON, 0}); - sleep(3); - std::cout << "Light: breath" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, TracerLightCmd::LightMode::BREATH, 0}); - sleep(3); - std::cout << "Light: custom 90-80" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, TracerLightCmd::LightMode::CUSTOM, 80}); - sleep(3); - std::cout << "Light: diabled cmd control" << std::endl; - tracer.DisableLightCmdControl(); + tracer.EnableCommandedMode(); - int count = 0; - while (true) - { - // motion control - if (count < 5) - { - std::cout << "Motor: 0.2, 0.0" << std::endl; - tracer.SetMotionCommand(0.2, 0.0); - } - else if (count < 10) - { - std::cout << "Motor: 0.8, 0.3" << std::endl; - tracer.SetMotionCommand(0.8, 0.3); - } - else if (count < 15) - { - std::cout << "Motor: 1.5, 0.5" << std::endl; - tracer.SetMotionCommand(1.5, 0.5); - } - else if (count < 20) - { - std::cout << "Motor: 1.0, 0.3" << std::endl; - tracer.SetMotionCommand(1.0, 0.3); - } - else if (count < 25) - { - std::cout << "Motor: 0.0, 0.0" << std::endl; - tracer.SetMotionCommand(0.0, 0.0); - } - else if (count < 30) - { - std::cout << "Motor: -0.5, -0.3" << std::endl; - tracer.SetMotionCommand(-0.5, -0.3); - } - else if (count < 35) - { - std::cout << "Motor: -1.0, -0.5" << std::endl; - tracer.SetMotionCommand(-1.0, -0.5); - } - else if (count < 40) - { - std::cout << "Motor: 0.0, 0.0," << std::endl; - tracer.SetMotionCommand(0.0, 0.0); - } + // light control + std::cout << "Light: const off" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, + TracerLightCmd::LightMode::CONST_OFF, 0}); + sleep(5); + std::cout << "Light: const on" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, + TracerLightCmd::LightMode::CONST_ON, 0}); + sleep(3); + std::cout << "Light: breath" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, + TracerLightCmd::LightMode::BREATH, 0}); + sleep(3); + std::cout << "Light: custom 90-80" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, + TracerLightCmd::LightMode::CUSTOM, 80}); + sleep(3); + std::cout << "Light: diabled cmd control" << std::endl; + tracer.DisableLightCmdControl(); - auto state = tracer.GetTracerState(); - std::cout << "-------------------------------" << std::endl; - std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl; - std::cout << "-------------------------------" << std::endl; + int count = 0; + while (true) { + // // motion control + // if (count < 5) { + // std::cout << "Motor: 0.2, 0.0" << std::endl; + // tracer.SetMotionCommand(0.2, 0.0); + // } else if (count < 10) { + // std::cout << "Motor: 0.8, 0.3" << std::endl; + // tracer.SetMotionCommand(0.8, 0.3); + // } else if (count < 15) { + // std::cout << "Motor: 1.5, 0.5" << std::endl; + // tracer.SetMotionCommand(1.5, 0.5); + // } else if (count < 20) { + // std::cout << "Motor: 1.0, 0.3" << std::endl; + // tracer.SetMotionCommand(1.0, 0.3); + // } else if (count < 25) { + // std::cout << "Motor: 0.0, 0.0" << std::endl; + // tracer.SetMotionCommand(0.0, 0.0); + // } else if (count < 30) { + // std::cout << "Motor: -0.5, -0.3" << std::endl; + // tracer.SetMotionCommand(-0.5, -0.3); + // } else if (count < 35) { + // std::cout << "Motor: -1.0, -0.5" << std::endl; + // tracer.SetMotionCommand(-1.0, -0.5); + // } else if (count < 40) { + // std::cout << "Motor: 0.0, 0.0," << std::endl; + // tracer.SetMotionCommand(0.0, 0.0); + // } - sleep(1); - ++count; - } + auto state = tracer.GetTracerState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " << static_cast(state.control_mode) + << " , base state: " << static_cast(state.base_state) + << std::endl; + std::cout << "battery voltage: " << state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " + << state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; - return 0; + sleep(1); + ++count; + } + + return 0; } \ No newline at end of file diff --git a/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h b/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h index 8e26735..d7b68f8 100644 --- a/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h +++ b/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h @@ -30,14 +30,48 @@ struct can_frame { }; #endif +#pragma pack(push, 1) +typedef enum { + AgxMsgUnkonwn = 0x00, + // command + AgxMsgMotionCommand = 0x01, + AgxMsgLightCommand = 0x02, + AgxMsgCtrlModeSelect = 0x03, + AgxMsgFaultByteReset = 0x04, + // state feedback + AgxMsgSystemState = 0x21, + AgxMsgMotionState = 0x22, + AgxMsgLightState = 0x23, + AgxMsgRcState = 0x24, + AgxMsgActuatorHSState = 0x25, + AgxMsgActuatorLSState = 0x26, + AgxMsgOdometry = 0x27 +} MsgType; + +typedef struct { + MsgType type; + union { + // command + MotionCommandMessage motion_command_msg; + LightCommandMessage light_command_msg; + CtrlModeSelectMessage ctrl_mode_select_msg; + StateResetMessage state_reset_msg; + // state feedback + SystemStateMessage system_state_msg; + MotionStateMessage motion_state_msg; + LightStateMessage light_state_msg; + RcStateMessage rc_state_msg; + ActuatorHSStateMessage actuator_hs_state_msg; + ActuatorLSStateMessage actuator_ls_state_msg; + OdometryMessage odometry_msg; + } body; +} AgxMessage; +#pragma pack(pop) + 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 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 diff --git a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h index 4b44a47..f8e64d9 100644 --- a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h +++ b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h @@ -10,98 +10,79 @@ #ifndef AGX_PROTOCOL_V2_H #define AGX_PROTOCOL_V2_H -// #ifdef __cplusplus -// extern "C" { -// #endif +#ifdef __cplusplus +extern "C" { +#endif #include /*---------------------------- 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) - -#define HUNTER_MOTOR1_ID ((uint8_t)0x00) -#define HUNTER_MOTOR2_ID ((uint8_t)0x01) -#define HUNTER_MOTOR3_ID ((uint8_t)0x02) - -#define TRACER_MOTOR1_ID ((uint8_t)0x00) -#define TRACER_MOTOR2_ID ((uint8_t)0x01) +#define ACTUATOR1_ID ((uint8_t)0x00) +#define ACTUATOR2_ID ((uint8_t)0x01) +#define ACTUATOR3_ID ((uint8_t)0x02) +#define ACTUATOR4_ID ((uint8_t)0x03) /*--------------------------- Message IDs ------------------------------*/ -// 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) +#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x231) +#define CAN_MSG_RC_STATE_ID ((uint32_t)0x241) -#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x111) -#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_ACTUATOR1_HS_STATE_ID ((uint32_t)0x251) +#define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252) +#define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253) +#define CAN_MSG_ACTUATOR4_HS_STATE_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) -#define CAN_MSG_MOTOR4_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x254) -#define CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID ((uint32_t)0x261) -#define CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID ((uint32_t)0x262) -#define CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID ((uint32_t)0x263) -#define CAN_MSG_MOTOR4_LOW_DRIVER_STATUS_ID ((uint32_t)0x264) -#define CAN_MSG_ODOMETER_ID ((uint32_t)0x311) +#define CAN_MSG_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261) +#define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262) +#define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263) +#define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264) + +#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) + +#define CAN_MSG_CTRL_MODE_SELECT_ID ((uint32_t)0x421) +#define CAN_MSG_STATE_RESET_ID ((uint32_t)0x441) /*--------------------- Control/State Constants ------------------------*/ -// Motion Control -#define CTRL_MODE_RC ((uint8_t)0x00) -#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) -#define CTRL_MODE_CMD_UART ((uint8_t)0x02) - -#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 State #define VEHICLE_STATE_NORMAL ((uint8_t)0x00) #define VEHICLE_STATE_ESTOP ((uint8_t)0x01) #define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02) -#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_CLR_ALL ((uint8_t)0x00) +#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x01) +#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x02) +#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x04) +#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x08) -#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) +// Motion Control +#define CTRL_MODE_RC ((uint8_t)0x00) +#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) +#define CTRL_MODE_CMD_UART ((uint8_t)0x02) + +// Light Control +#define LIGHT_CTRL_DISABLE ((uint8_t)0x00) +#define LIGHT_CTRL_ENABLE ((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) + +// Actuator State +#define BATTERY_VOLTAGE_LOW ((uint8_t)0x00) +#define MOTOR_OVERHEAT ((uint8_t)0x01) +#define MOTOR_DRIVER_OVERLOAD ((uint8_t)0x02) +#define MOTOR_DRIVER_OVERHEAT ((uint8_t)0x03) +#define MOTOR_SENSOR_FAULT ((uint8_t)0x04) +#define MOTOR_DRIVER_FAULT ((uint8_t)0x05) +#define MOTOR_DRIVER_ENABLED ((uint8_t)0x06) +#define MOTOR_DRIVER_RESERVED0 ((uint8_t)0x07) /*-------------------- Control/Feedback Messages -----------------------*/ @@ -109,7 +90,73 @@ // reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect #pragma pack(push, 1) -// System Status Feedback +// Control messages +typedef union { + struct { + struct { + int8_t high_byte; + int8_t low_byte; + } linear_velocity; + struct { + uint8_t high_byte; + uint8_t low_byte; + } angular_velocity; + struct { + uint8_t high_byte; + uint8_t low_byte; + } lateral_velocity; + struct { + uint8_t high_byte; + uint8_t low_byte; + } steering_angle; + } cmd; + uint8_t raw[8]; +} MotionCommandMessage; + +typedef union { + struct { + uint8_t light_ctrl_enabled; + uint8_t front_light_mode; + uint8_t front_light_custom; + uint8_t rear_light_mode; + uint8_t rear_light_custom; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; + + } cmd; + uint8_t raw[8]; +} LightCommandMessage; + +typedef union { + struct { + uint8_t control_mode; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; + } cmd; + uint8_t raw[8]; +} CtrlModeSelectMessage; + +typedef union { + struct { + uint8_t fault_byte; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; + } cmd; + uint8_t raw[8]; +} StateResetMessage; + +// State feedback messages typedef union { struct { uint8_t vehicle_state; @@ -122,145 +169,78 @@ typedef union { uint8_t reserved0; uint8_t reserved1; uint8_t count; - } status; + } state; uint8_t raw[8]; } SystemStateMessage; -// Motion Control typedef 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 reserved2; + uint8_t reserved3; + } state; + uint8_t raw[8]; +} MotionStateMessage; + +typedef union { + struct { + uint8_t light_ctrl_enabled; + uint8_t front_light_mode; + uint8_t front_light_custom; + uint8_t rear_light_mode; + uint8_t rear_light_custom; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; + } state; + uint8_t raw[8]; +} LightStateMessage; + +typedef union { + struct { + uint8_t sws; + uint8_t right_stick_left_right; + uint8_t right_stick_up_down; + uint8_t left_stick_left_right; + uint8_t left_stick_up_down; + uint8_t var_a; + uint8_t reserved0; + uint8_t count; + } state; + uint8_t raw[8]; +} RcStateMessage; + +typedef struct { + uint8_t motor_id; union { struct { struct { + uint8_t high_byte; + uint8_t low_byte; + } rpm; + struct { + uint8_t high_byte; + uint8_t low_byte; + } current; + struct { + int8_t msb; int8_t high_byte; int8_t low_byte; - } linear_velocity; - struct { - uint8_t high_byte; - uint8_t low_byte; - } angular_velocity; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - } cmd; + int8_t lsb; + } pulse_count; + } state; 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 reserved2; - uint8_t reserved3; - } cmd; - uint8_t raw[8]; - } data; -} MotionStatusMessage; - -typedef struct { - union { - struct { - uint8_t control_mode; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t reserved4; - uint8_t reserved5; - uint8_t reserved6; - } cmd; - uint8_t raw[8]; - } data; -} ModSelectMessage; - -// Light Control -typedef struct { - union { - struct { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t rear_light_mode; - uint8_t rear_light_custom; - uint8_t reserved0; - uint8_t reserved1; - uint8_t count; - - } cmd; - uint8_t raw[8]; - } data; -} LightControlMessage; - -typedef struct { - union { - struct { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t rear_light_mode; - uint8_t rear_light_custom; - uint8_t reserved0; - uint8_t reserved1; - uint8_t count; - } 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; - -typedef struct { - uint8_t motor_id; - union { - struct { - struct { - uint8_t high_byte; - uint8_t low_byte; - } rpm; - struct { - uint8_t high_byte; - uint8_t low_byte; - } current; - struct { - int8_t heighest; - int8_t sec_heighest; - int8_t sec_lowest; - int8_t lowest; - } moter_pose; - } status; - uint8_t raw[8]; - } data; -} MotorDriverHeightSpeedStatusMessage; +} ActuatorHSStateMessage; typedef struct { uint8_t motor_id; @@ -278,71 +258,33 @@ typedef struct { uint8_t driver_state; uint8_t reserved0; uint8_t reserved1; - } status; + } state; uint8_t raw[8]; } data; -} MotorDriverLowSpeedStatusMessage; +} ActuatorLSStateMessage; -typedef struct { - uint8_t motor_id; - union { +typedef union { + struct { struct { - struct { - uint8_t heighest; - uint8_t sec_heighest; - uint8_t sec_lowest; - uint8_t lowest; - } left; - struct { - uint8_t heighest; - uint8_t sec_heighest; - uint8_t sec_lowest; - uint8_t lowest; - } right; - } status; - uint8_t raw[8]; - } data; -} OdomterMessage; - -// For convenience to access status/control message -typedef enum { - ScoutMsgNone = 0x00, - // status messages - ScoutMotionStatusMsg = 0x01, - ScoutLightStatusMsg = 0x02, - ScoutSystemStatusMsg = 0x03, - ScoutMotorDriverStatusMsg = 0x04, - ScoutMotorDriverHeightSpeedStatusMsg = 0x05, - ScoutMotorDriverLowSpeedStatusMsg = 0x06, - ScoutodometerMsg = 0x07, - // control messages - ScoutMotionControlMsg = 0x21, - ScoutLightControlMsg = 0x22, - ScoutControlModeMsg = 0x23 -} ScoutMsgType; - -typedef struct { - ScoutMsgType type; - union { - // status messages - MotionStatusMessage motion_status_msg; - LightStatusMessage light_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; - OdomterMessage odom_msg; - // control messages - MotionControlMessage motion_control_msg; - LightControlMessage light_control_msg; - ModSelectMessage mode_cmd_msg; - } body; -} AgxMessage; + uint8_t msb; + uint8_t high_byte; + uint8_t low_byte; + uint8_t lsb; + } left_wheel; + struct { + uint8_t msb; + uint8_t high_byte; + uint8_t low_byte; + uint8_t lsb; + } right_wheel; + } state; + uint8_t raw[8]; +} OdometryMessage; #pragma pack(pop) -// #ifdef __cplusplus -// } -// #endif +#ifdef __cplusplus +} +#endif #endif /* AGX_PROTOCOL_V2_H */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp index 7795d3c..0e8acee 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp @@ -1,11 +1,11 @@ -/* +/* * tracer_base.hpp - * + * * Created on: Apr 14, 2020 10:21 - * Description: - * + * Description: + * * Copyright (c) 2020 Ruixiang Du (rdu) - */ + */ #ifndef TRACER_BASE_HPP #define TRACER_BASE_HPP @@ -15,83 +15,53 @@ #include #include -//#include "wrp_sdk/asyncio/async_can.hpp" -//#include "wrp_sdk/asyncio/async_serial.hpp" #include "ugv_sdk/mobile_base.hpp" - -#include "ugv_sdk/tracer/tracer_protocol.h" -#include "ugv_sdk/tracer/tracer_can_parser.h" -#include "ugv_sdk/tracer/tracer_uart_parser.h" +#include "ugv_sdk/proto/agx_msg_parser.h" #include "ugv_sdk/tracer/tracer_types.hpp" +namespace westonrobot { +class TracerBase : public MobileBase { + public: + TracerBase() : MobileBase(){}; + ~TracerBase() = default; -namespace westonrobot -{ -class TracerBase :public MobileBase -{ -public: - TracerBase() : MobileBase(){}; - ~TracerBase()= default; + // get robot state + TracerState GetTracerState(); - // get robot state - TracerState GetTracerState(); - UartTracerState GetUartTracerState(); - // motion control - void SetMotionCommand(double linear_vel, double angular_vel, - TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT); + void EnableCommandedMode(); - // light control - void SetLightCommand(TracerLightCmd cmd); - void DisableLightCmdControl(); + // motion control + void SetMotionCommand(double linear_vel, double angular_vel); + // light control + void SetLightCommand(TracerLightCmd cmd); + void DisableLightCmdControl(); + private: + // cmd/status update related variables + std::mutex tracer_state_mutex_; + std::mutex uart_tracer_state_mutex_; + std::mutex motion_cmd_mutex_; + std::mutex light_cmd_mutex_; + std::mutex mode_cmd_mutex_; -private: + TracerState tracer_state_; + TracerMotionCmd current_motion_cmd_; + TracerLightCmd current_light_cmd_; - // CAN priority higher than serial if both connected -// bool can_connected_ = false; -// bool serial_connected_ = false; + bool light_ctrl_enabled_ = false; + bool light_ctrl_requested_ = false; - // serial port related variables - uint8_t tx_cmd_len_; - uint8_t tx_buffer_[TRACER_CMD_BUF_LEN]; + void SendMotionCmd(uint8_t count); + void SendLightCmd(uint8_t count); - // cmd/status update related variables - //std::thread cmd_thread_; - std::mutex tracer_state_mutex_; - std::mutex uart_tracer_state_mutex_; - std::mutex motion_cmd_mutex_; - std::mutex light_cmd_mutex_; - std::mutex mode_cmd_mutex_; + void SendRobotCmd() override; + void ParseCANFrame(can_frame *rx_frame) override; - TracerState tracer_state_; - UartTracerState uart_tracer_state_; - TracerMotionCmd current_motion_cmd_; - UartTracerMotionCmd uart_current_motion_cmd_; - TracerLightCmd current_light_cmd_; - -// int32_t cmd_thread_period_ms_ = 10; -// bool cmd_thread_started_ = false; - - bool light_ctrl_enabled_ = false; - bool light_ctrl_requested_ = false; - - void SendMotionCmd(uint8_t count); - void SendRobotCmd() override; - void SendLightCmd(uint8_t count); - void SendControlCmd(); - void ParseCANFrame(can_frame *rx_frame) override; - void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received); - // override{}; - - void NewStatusMsgReceivedCallback(const TracerMessage &msg); - void UartNewStatusMsgReceivedCallback(const UartTracerMessage &msg); - - -public: - static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state); - static void UartUpdateTracerState(const UartTracerMessage &status_msg, UartTracerState &state); + void NewStatusMsgReceivedCallback(const AgxMessage &msg); + static void UpdateTracerState(const AgxMessage &status_msg, + TracerState &state); }; -} // namespace westonrobot +} // namespace westonrobot #endif /* TRACER_BASE_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h deleted file mode 100644 index 608e302..0000000 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * tracer_can_parser.h - * - * Created on: Apr 14, 2020 10:52 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef TRACER_CAN_PARSER_H -#define TRACER_CAN_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -#include "ugv_sdk/tracer/tracer_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 DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg); -void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame); - -uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); - -#ifdef __cplusplus -} -#endif - -#endif /* TRACER_CAN_PARSER_H */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h deleted file mode 100644 index aab086a..0000000 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h +++ /dev/null @@ -1,527 +0,0 @@ -/* - * tracer_protocol.h - * - * Created on: Apr 14, 2020 10:34 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef TRACER_PROTOCOL_H -#define TRACER_PROTOCOL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define TRACER_CMD_BUF_LEN 32 -#define TRACER_STATUS_BUF_LEN 32 -#define TRACER_FRAME_SIZE 13 - -#define TRACER_MOTOR1_ID ((uint8_t)0x00) -#define TRACER_MOTOR2_ID ((uint8_t)0x01) - -// 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_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_CMD_ID ((uint32_t)0x111) -#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x221) -#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_COMTROL_MODE_ID ((uint32_t)0x421) -#define CAN_MSG_ODOMETER_ID ((uint32_t)0x311) - -/*--------------------- 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_FRONT_STEER_ENCODER_F ((uint16_t)0x0200) -#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x0400) -#define FAULT_HIGH_BYTE_RESERVED1 ((uint16_t)0x0800) -#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x1000) -#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x2000) -#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x4000) -#define FAULT_HIGH_BYTE_RESERVED5 ((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_RESERVED1 ((uint16_t)0x0010) -#define FAULT_RESERVED2 ((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(can) -typedef struct { - union - { - struct - { - struct - { - int8_t H_byte; - int8_t L_byte; - }linear_velocity; - struct - { - int8_t H_byte; - int8_t L_byte; - }angular_velocity; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - } cmd; - uint8_t raw[8]; - } data; -} MotionCmdMessage; - -// Motion Control(uart) -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; -} UartMotionControlMessage; - -typedef struct { - union - { - struct - { - uint8_t control_mode; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t reserved4; - uint8_t reserved5; - uint8_t reserved6; - } cmd; - uint8_t raw[8]; - } data; -} ModSelectMessage; - -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 reserved2; - uint8_t reserved3; - } status; - uint8_t raw[8]; - } data; -} MotionStatusMessage; - -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; -} UartMotionStatusMessage; - -// 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 count; - //uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} SystemStatusMessage; - -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; -} UartSystemStatusMessage; - -// Light Control -typedef struct { - union - { - struct - { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t count; - - } cmd; - uint8_t raw[8]; - } data; -} LightControlMessage; - -typedef struct { - union - { - struct - { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t rear_light_mode; - uint8_t rear_light_custom; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; - } cmd; - uint8_t raw[8]; - } data; -} UartLightControlMessage; - -typedef struct { - union - { - struct - { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t count; - } status; - uint8_t raw[8]; - } data; -} LightStatusMessage; - -typedef struct { - union - { - struct - { - uint8_t light_ctrl_enable; - uint8_t front_light_mode; - uint8_t front_light_custom; - uint8_t rear_light_mode; - uint8_t rear_light_custom; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} UartLightStatusMessage; - -// 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; - -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; -} UartMotorDriverStatusMessage; - -typedef struct -{ - uint8_t motor_id; - union { - struct - { - struct - { - uint8_t high_byte; - uint8_t low_byte; - } rpm; - int8_t reserved0; - int8_t reserved1; - int8_t reserved2; - int8_t reserved3; - int8_t reserved4; - int8_t reserved5; - } status; - uint8_t raw[8]; - } data; -} MotorHeightSpeedStatusMessage; - -typedef struct -{ - uint8_t motor_id; - union { - struct - { - int8_t reserved0; - int8_t reserved1; - int8_t reserved2; - int8_t reserved3; - int8_t reserved4; - int8_t driver_state; - int8_t reserved5; - int8_t reserved6; - } status; - uint8_t raw[8]; - } data; -} MotorLowSpeedStatusMessage; - -typedef struct -{ - uint8_t motor_id; - union { - struct - { - struct - { - uint8_t highest; - uint8_t sec_highest; - uint8_t sec_lowest; - uint8_t lowest; - }leftodometer; - struct - { - uint8_t highest; - uint8_t sec_highest; - uint8_t sec_lowest; - uint8_t lowest; - }rightodometer; - } status; - uint8_t raw[8]; - } data; -} OdometerMessage; - -// For convenience to access status/control message -typedef enum -{ - TracerMsgNone = 0x00, - // status messages - TracerMotionStatusMsg = 0x01, - TracerLightStatusMsg = 0x02, - TracerSystemStatusMsg = 0x03, - TracerMotorDriverStatusMsg = 0x04, - TracerOdometerMsg = 0x05, - TracerHeighSpeedMsg = 0x06, - TracerLowSpeedMsg = 0x07, - - // control messages - TracerMotionCmdMsg = 0x21, - TracerLightControlMsg = 0x22, - TracerModeControlMsg = 0x23 -} TracerMsgType; - -typedef enum -{ - UartTracerMsgNone = 0x00, - // status messages - UartTracerMotionStatusMsg = 0x01, - UartTracerLightStatusMsg = 0x02, - UartTracerSystemStatusMsg = 0x03, - UartTracerMotorDriverStatusMsg = 0x04, - // control messages - UartTracerMotionControlMsg = 0x21, - UartTracerLightControlMsg = 0x22 -} UartTracerMsgType; - -typedef struct -{ - TracerMsgType type; - union { - // status messages - MotionStatusMessage motion_status_msg; - LightStatusMessage light_status_msg; - SystemStatusMessage system_status_msg; - MotorDriverStatusMessage motor_driver_status_msg; - OdometerMessage odom_msg; - MotorHeightSpeedStatusMessage motor_heigh_speed_msg; - MotorLowSpeedStatusMessage motor_low_speed_msg; - - // control messages - MotionCmdMessage motion_cmd_msg; - LightControlMessage light_control_msg; - ModSelectMessage mode_cmd_msg; - - } body; -} TracerMessage; - -typedef struct -{ - UartTracerMsgType type; - union { - // status messages - UartMotionStatusMessage motion_status_msg; - UartLightStatusMessage light_status_msg; - UartSystemStatusMessage system_status_msg; - UartMotorDriverStatusMessage motor_driver_status_msg; - // control messages - UartMotionControlMessage motion_control_msg; - UartLightControlMessage light_control_msg; - } body; -} UartTracerMessage; - -#pragma pack(pop) - -#ifdef __cplusplus -} -#endif - -#endif /* TRACER_PROTOCOL_H */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp index 7c76ff2..475a8b7 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp @@ -1,11 +1,11 @@ -/* +/* * tracer_types.hpp - * + * * Created on: Apr 14, 2020 10:22 - * Description: - * + * Description: + * * Copyright (c) 2020 Ruixiang Du (rdu) - */ + */ #ifndef TRACER_TYPES_HPP #define TRACER_TYPES_HPP @@ -13,168 +13,80 @@ #include #include -namespace westonrobot -{ -struct TracerState -{ - struct MotorState - { - //double current = 0; // in A - double rpm = 0; - //double temperature = 0; - }; +namespace westonrobot { +struct TracerState { + struct ActuatorState { + double motor_current = 0; // in A + uint16_t motor_rpm = 0; + uint16_t motor_pulses = 0; + double motor_temperature = 0; - struct LightState - { - uint8_t mode = 0; - uint8_t custom_value = 0; - }; - - // base state - uint8_t base_state = 0; - uint8_t control_mode = 0; - uint8_t fault_code = 0; - double battery_voltage = 0.0; - - // motor state - MotorState motor_states[2]; - - // light state - bool light_control_enabled = false; - LightState front_light_state; - - // motion state - double linear_velocity = 0; - double angular_velocity = 0; - - //odometer state - double left_odomter = 0; - double right_odomter = 0; -}; -struct UartTracerState -{ - enum MotorID - { - FRONT_RIGHT = 0, - FRONT_LEFT = 1, - REAR_LEFT = 2, - REAR_RIGHT = 3 + double driver_voltage = 0; + double driver_temperature = 0; + uint8_t driver_state = 0; }; - struct MotorState - { - double current = 9; // in A - double rpm = 0; - double temperature = 9; - }; - - struct LightState - { - uint8_t mode = 0; - uint8_t custom_value = 0; + struct LightState { + uint8_t mode = 0; + uint8_t custom_value = 0; }; // base state uint8_t base_state = 0; uint8_t control_mode = 0; - uint16_t fault_code = 0; + uint8_t fault_code = 0; double battery_voltage = 0.0; - // motor state - static constexpr uint8_t motor_num = 2; - MotorState motor_states[motor_num]; + // actuator state + ActuatorState actuator_states[2]; // light state bool light_control_enabled = false; LightState front_light_state; - LightState rear_light_state; // motion state double linear_velocity = 0; double angular_velocity = 0; + + // odometer state + double left_odometry = 0; + double right_odometry = 0; }; -struct TracerMotionCmd -{ - enum class FaultClearFlag - { - NO_FAULT = 0x00, - BAT_UNDER_VOL = 0x01, - BAT_OVER_VOL = 0x02, - MOTOR1_COMM = 0x03, - MOTOR2_COMM = 0x04, - MOTOR3_COMM = 0x05, - MOTOR4_COMM = 0x06, - MOTOR_DRV_OVERHEAT = 0x07, - MOTOR_OVERCURRENT = 0x08 - }; +struct TracerMotionCmd { + TracerMotionCmd(double linear = 0.0, double angular = 0.0) + : linear_velocity(linear), angular_velocity(angular) {} - TracerMotionCmd(int8_t linear_H = 0, int8_t angular_H = 0,int8_t linear_L = 0, int8_t angular_L = 0, - FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) - : linear_velocity_H(linear_H), angular_velocity_H(angular_H),linear_velocity_L(linear_L), angular_velocity_L(angular_L), - fault_clear_flag(fault_clr_flag) {} + double linear_velocity; + double angular_velocity; - int8_t linear_velocity_H; - int8_t linear_velocity_L; - int8_t angular_velocity_H; - int8_t angular_velocity_L; - FaultClearFlag fault_clear_flag; - - static constexpr double max_linear_velocity = 1.5; // 1.5 m/s - static constexpr double min_linear_velocity = -1.5; // -1.5 m/s - static constexpr double max_angular_velocity = 0.7853; // 0.5235 rad/s - static constexpr double min_angular_velocity = -0.7853; // -0.5235 rad/s + static constexpr double max_linear_velocity = 1.8; // m/s + static constexpr double min_linear_velocity = -1.8; + static constexpr double max_angular_velocity = 1.0; // rad/s + static constexpr double min_angular_velocity = -1.0; }; -struct UartTracerMotionCmd -{ - enum class FaultClearFlag - { - NO_FAULT = 0x00, - BAT_UNDER_VOL = 0x01, - BAT_OVER_VOL = 0x02, - MOTOR1_COMM = 0x03, - MOTOR2_COMM = 0x04, - MOTOR3_COMM = 0x05, - MOTOR4_COMM = 0x06, - MOTOR_DRV_OVERHEAT = 0x07, - MOTOR_OVERCURRENT = 0x08 - }; +struct TracerLightCmd { + enum class LightMode { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 + }; - UartTracerMotionCmd(int8_t linear = 0, int8_t angular = 0, - FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) - : linear_velocity(linear), angular_velocity(angular), - fault_clear_flag(fault_clr_flag) {} + TracerLightCmd() = default; + TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) + : front_mode(f_mode), + front_custom_value(f_value), + rear_mode(r_mode), + rear_custom_value(r_value) {} - int8_t linear_velocity; - int8_t angular_velocity; - FaultClearFlag fault_clear_flag = FaultClearFlag::NO_FAULT; - - static constexpr double max_linear_velocity = 1.5; // 1.5 m/s - static constexpr double min_linear_velocity = -1.5; // -1.5 m/s - static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s - static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s + LightMode front_mode; + uint8_t front_custom_value; + LightMode rear_mode; + uint8_t rear_custom_value; }; - -struct TracerLightCmd -{ - enum class LightMode - { - CONST_OFF = 0x00, - CONST_ON = 0x01, - BREATH = 0x02, - CUSTOM = 0x03 - }; - - TracerLightCmd() = default; - TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), - rear_mode(r_mode), rear_custom_value(r_value) {} - - LightMode front_mode; - uint8_t front_custom_value; - LightMode rear_mode; - uint8_t rear_custom_value; -}; -} // namespace westonrobot +} // namespace westonrobot #endif /* TRACER_TYPES_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h deleted file mode 100644 index b612aa3..0000000 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h +++ /dev/null @@ -1,24 +0,0 @@ - -#ifndef TRACE_UART_PARSER_H -#define TRACE_UART_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -#include "ugv_sdk/tracer/tracer_protocol.h" - -bool DecodeTracerMsgFromUART(uint8_t c, UartTracerMessage *msg); -void EncodeTracerMsgToUART(const UartTracerMessage *msg, uint8_t *buf, uint8_t *len); - -uint8_t CalcTracerUARTChecksum(uint8_t *buf, uint8_t len); - -#ifdef __cplusplus -} -#endif - -#endif /* TRACE_UART_PARSER_H */ diff --git a/ugv_sdk/src/agx_msg_parser.c b/ugv_sdk/src/agx_msg_parser.c index 600c077..ecaae82 100644 --- a/ugv_sdk/src/agx_msg_parser.c +++ b/ugv_sdk/src/agx_msg_parser.c @@ -11,134 +11,86 @@ #include "string.h" -// #define USE_XOR_CHECKSUM - -// #define PRINT_CPP_DEBUG_INFO -// #define PRINT_JLINK_DEBUG_INFO - -#ifdef PRINT_CPP_DEBUG_INFO -#define < iostream> -#elif (defined(PRINT_JLINK_DEBUG_INFO)) -#include "segger/jlink_rtt.h" -#endif - -static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, - struct can_frame *tx_frame); -static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, - struct can_frame *tx_frame); -static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, - struct can_frame *tx_frame); - bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) { - msg->type = ScoutMsgNone; + msg->type = AgxMsgUnkonwn; switch (rx_frame->can_id) { - // in the current implementation, both MsgType and can_frame include 8 * - // uint8_t - case CAN_MSG_MOTION_STATE_ID: { - msg->type = ScoutMotionStatusMsg; - // msg->motion_status_msg.id = CAN_MSG_MOTION_STATE_ID; - memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, + // command frame + case CAN_MSG_MOTION_COMMAND_ID: { + msg->type = AgxMsgMotionCommand; + memcpy(msg->body.motion_command_msg.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, + case CAN_MSG_LIGHT_COMMAND_ID: { + msg->type = AgxMsgLightCommand; + memcpy(msg->body.light_command_msg.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); break; } + case CAN_MSG_RC_STATE_ID: { + msg->type = AgxMsgRcState; + memcpy(msg->body.rc_state_msg.raw, rx_frame->data, + rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_CTRL_MODE_SELECT_ID: { + msg->type = AgxMsgCtrlModeSelect; + memcpy(msg->body.ctrl_mode_select_msg.raw, rx_frame->data, + rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_STATE_RESET_ID: { + msg->type = AgxMsgFaultByteReset; + memcpy(msg->body.state_reset_msg.raw, rx_frame->data, + rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + // state feedback frame case CAN_MSG_SYSTEM_STATE_ID: { - msg->type = ScoutSystemStatusMsg; - // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATE_ID; - memcpy(msg->body.system_status_msg.raw, rx_frame->data, + msg->type = AgxMsgSystemState; + memcpy(msg->body.system_state_msg.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, + case CAN_MSG_MOTION_STATE_ID: { + msg->type = AgxMsgMotionState; + memcpy(msg->body.motion_state_msg.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, + case CAN_MSG_LIGHT_STATE_ID: { + msg->type = AgxMsgLightState; + memcpy(msg->body.light_state_msg.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); break; } - case CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverLowSpeedStatusMsg; - msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR1_ID; - memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverLowSpeedStatusMsg; - msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR2_ID; - memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverLowSpeedStatusMsg; - msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR3_ID; - memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR4_LOW_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverLowSpeedStatusMsg; - msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR4_ID; - memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverHeightSpeedStatusMsg; - msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR1_ID; - memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverHeightSpeedStatusMsg; - msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR2_ID; - memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverHeightSpeedStatusMsg; - msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR3_ID; - memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR4_HEIGHT_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverHeightSpeedStatusMsg; - msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR4_ID; - memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw, - rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_ODOMETER_ID: { - msg->type = ScoutodometerMsg; - // msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - memcpy(msg->body.odom_msg.data.raw, rx_frame->data, + case CAN_MSG_ODOMETRY_ID: { + msg->type = AgxMsgOdometry; + memcpy(msg->body.odometry_msg.raw, rx_frame->data, + rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR1_HS_STATE_ID: + case CAN_MSG_ACTUATOR2_HS_STATE_ID: + case CAN_MSG_ACTUATOR3_HS_STATE_ID: + case CAN_MSG_ACTUATOR4_HS_STATE_ID: { + msg->type = AgxMsgActuatorLSState; + msg->body.actuator_hs_state_msg.motor_id = + (uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID); + memcpy(msg->body.actuator_hs_state_msg.data.raw, rx_frame->data, + rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR1_LS_STATE_ID: + case CAN_MSG_ACTUATOR2_LS_STATE_ID: + case CAN_MSG_ACTUATOR3_LS_STATE_ID: + case CAN_MSG_ACTUATOR4_LS_STATE_ID: { + msg->type = AgxMsgActuatorLSState; + msg->body.actuator_ls_state_msg.motor_id = + (uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID); + memcpy(msg->body.actuator_ls_state_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - // printf("%x\t",msg->body.odom_msg.data.status.left.heighest); - // printf("%x\t",msg->body.odom_msg.data.status.left.sec_heighest); - // printf("%x\t",msg->body.odom_msg.data.status.left.sec_lowest); - // printf("%x\r\n",msg->body.odom_msg.data.status.left.lowest); - // printf("%x\t",msg->body.odom_msg.data.raw); - // printf("%x\r\n",msg->body.odom_msg.data.status); break; } default: @@ -150,91 +102,118 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) { 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_STATE_ID; + // command frame + case AgxMsgMotionCommand: { + tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID; tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, + memcpy(tx_frame->data, msg->body.motion_command_msg.raw, tx_frame->can_dlc); + // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, + // tx_frame->can_dlc); break; } - case ScoutLightStatusMsg: { - tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + case AgxMsgLightCommand: { + tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID; tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, + memcpy(tx_frame->data, msg->body.light_command_msg.raw, tx_frame->can_dlc); + // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, + // tx_frame->can_dlc); break; } - case ScoutSystemStatusMsg: { + case AgxMsgCtrlModeSelect: { + tx_frame->can_id = CAN_MSG_CTRL_MODE_SELECT_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw, + tx_frame->can_dlc); + // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, + // tx_frame->can_dlc); + break; + } + case AgxMsgFaultByteReset: { + tx_frame->can_id = CAN_MSG_STATE_RESET_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc); + tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, + tx_frame->can_dlc); + break; + } + // state feedback frame + case AgxMsgSystemState: { tx_frame->can_id = CAN_MSG_SYSTEM_STATE_ID; tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.system_status_msg.raw, + memcpy(tx_frame->data, msg->body.system_state_msg.raw, tx_frame->can_dlc); + break; + } + case AgxMsgMotionState: { + tx_frame->can_id = CAN_MSG_MOTION_STATE_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.motion_state_msg.raw, tx_frame->can_dlc); + break; + } + case AgxMsgLightState: { + tx_frame->can_id = CAN_MSG_LIGHT_STATE_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.light_state_msg.raw, tx_frame->can_dlc); + break; + } + case AgxMsgOdometry: { + tx_frame->can_id = CAN_MSG_ODOMETRY_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.odometry_msg.raw, tx_frame->can_dlc); + break; + } + case AgxMsgActuatorHSState: { + switch (msg->body.actuator_hs_state_msg.motor_id) { + case ACTUATOR1_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR1_HS_STATE_ID; + break; + } + case ACTUATOR2_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR2_HS_STATE_ID; + break; + } + case ACTUATOR3_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR3_HS_STATE_ID; + break; + } + case ACTUATOR4_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR4_HS_STATE_ID; + break; + } + } + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.actuator_hs_state_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; - } - // TODO check correctiveness - // case ScoutControlModeMsg: { - // EncodeScoutnControlModeMsgToCAN(&(msg->body.motion_control_msg), - // tx_frame); - // break; - // } - case ScoutLightControlMsg: { - EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); + case AgxMsgActuatorLSState: { + switch (msg->body.actuator_ls_state_msg.motor_id) { + case ACTUATOR1_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR1_LS_STATE_ID; + break; + } + case ACTUATOR2_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR2_LS_STATE_ID; + break; + } + case ACTUATOR3_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR3_LS_STATE_ID; + break; + } + case ACTUATOR4_ID: { + tx_frame->can_id = CAN_MSG_ACTUATOR4_LS_STATE_ID; + break; + } + } + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.actuator_ls_state_msg.data.raw, + tx_frame->can_dlc); break; } default: break; } - // tx_frame->data[7] = CalcCanFrameChecksum(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] = - CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} -void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, - struct can_frame *tx_frame) { - tx_frame->can_id = CAN_MSG_SELECT_CONTROL_MODE_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - tx_frame->data[7] = - CalcCanFrameChecksum(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] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); } diff --git a/ugv_sdk/src/tracer_base.cpp b/ugv_sdk/src/tracer_base.cpp index 092c548..6a9296f 100644 --- a/ugv_sdk/src/tracer_base.cpp +++ b/ugv_sdk/src/tracer_base.cpp @@ -12,454 +12,236 @@ #include "stopwatch.hpp" -namespace westonrobot -{ +namespace westonrobot { void TracerBase::SendRobotCmd() { static uint8_t cmd_count = 0; static uint8_t light_cmd_count = 0; - if(can_connected_) SendControlCmd(); - SendMotionCmd(cmd_count++); - if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); -} - - -void TracerBase::SendMotionCmd(uint8_t count) -{ - if(can_connected_) - { - // motion control message - TracerMessage m_msg; - m_msg.type = TracerMotionCmdMsg; - - //SendControlCmd(); - motion_cmd_mutex_.lock(); - m_msg.body.motion_cmd_msg.data.cmd.linear_velocity.H_byte = current_motion_cmd_.linear_velocity_H; - m_msg.body.motion_cmd_msg.data.cmd.linear_velocity.L_byte = current_motion_cmd_.linear_velocity_L; - m_msg.body.motion_cmd_msg.data.cmd.angular_velocity.H_byte = current_motion_cmd_.angular_velocity_H; - m_msg.body.motion_cmd_msg.data.cmd.angular_velocity.L_byte = current_motion_cmd_.angular_velocity_L; - motion_cmd_mutex_.unlock(); - m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0; - m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0; - m_msg.body.motion_cmd_msg.data.cmd.reserved2 = 0; - m_msg.body.motion_cmd_msg.data.cmd.reserved3 = 0; - - if (can_connected_) - { - // send to can bus - can_frame m_frame; - EncodeTracerMsgToCAN(&m_msg, &m_frame); - can_if_->SendFrame(m_frame); - } -// else -// { -// // TODO -// // send to serial port -// // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); -// // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); -// } -// if (can_connected_) -// m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8); -// serial_connected_: checksum will be calculated later when packed into a complete serial frame - - } - else if (serial_connected_){ - UartTracerMessage m_msg; - m_msg.type = UartTracerMotionControlMsg; - m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; - - //SendControlCmd(); - motion_cmd_mutex_.lock(); - m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = - static_cast(uart_current_motion_cmd_.fault_clear_flag); - m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = - uart_current_motion_cmd_.linear_velocity; - m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = - uart_current_motion_cmd_.angular_velocity; - motion_cmd_mutex_.unlock(); - - m_msg.body.motion_control_msg.data.cmd.reserved0 = 0; - m_msg.body.motion_control_msg.data.cmd.reserved1 = 0; - m_msg.body.motion_control_msg.data.cmd.count = count; -// serial_connected_: checksum will be calculated later when packed into a -// complete serial frame - EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); - serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); - } -} - -void TracerBase::SendLightCmd(uint8_t count) -{ - UartTracerMessage l_msg; - l_msg.type = UartTracerLightControlMsg; - - light_cmd_mutex_.lock(); - if (light_ctrl_enabled_) { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - static_cast(current_light_cmd_.front_mode); - l_msg.body.light_control_msg.data.cmd.front_light_custom = - current_light_cmd_.front_custom_value; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - static_cast(current_light_cmd_.rear_mode); - l_msg.body.light_control_msg.data.cmd.rear_light_custom = - current_light_cmd_.rear_custom_value; - } else { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = - LIGHT_DISABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; - } - light_ctrl_requested_ = false; - light_cmd_mutex_.unlock(); - - l_msg.body.light_control_msg.data.cmd.reserved0 = 0; - l_msg.body.light_control_msg.data.cmd.count = count; - - // serial_connected_: checksum will be calculated later when packed into a - // complete serial frame - // send to serial port - EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); - serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); - - /*TracerMessage l_msg; - l_msg.type = TracerLightControlMsg; - - light_cmd_mutex_.lock(); - if (light_ctrl_enabled_) - { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast(current_light_cmd_.front_mode); - l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value; - //l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast(current_light_cmd_.rear_mode); - //l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value; - - // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , " - // << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl; - // std::cout << "light cmd generated" << std::endl; - } - else - { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; - //l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF; - //l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; - } - light_ctrl_requested_ = false; - light_cmd_mutex_.unlock(); - - l_msg.body.light_control_msg.data.cmd.reserved0 = 0; - l_msg.body.light_control_msg.data.cmd.count = count; - - if (can_connected_) - // l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8); - // serial_connected_: checksum will be calculated later when packed into a complete serial frame - - if (can_connected_) - { - // send to can bus - can_frame l_frame; - EncodeTracerMsgToCAN(&l_msg, &l_frame); - - can_if_->SendFrame(l_frame); - } + if (can_connected_) { + SendMotionCmd(cmd_count++); + SendLightCmd(light_cmd_count++); + // if (light_ctrl_requested_) + // SendLightCmd(light_cmd_count++); // else - // { - // // send to serial port - // EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); - // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); - // } - - // std::cout << "cmd: " << static_cast(l_msg.data.cmd.front_light_mode) << " , " << static_cast(l_msg.data.cmd.front_light_custom) << " , " - // << static_cast(l_msg.data.cmd.rear_light_mode) << " , " << static_cast(l_msg.data.cmd.rear_light_custom) << std::endl; - // std::cout << "can: "; - // for (int i = 0; i < 8; ++i) - // std::cout << static_cast(l_frame.data[i]) << " "; - // std::cout << "uart: "; - // for (int i = 0; i < tx_cmd_len_; ++i) - // std::cout << static_cast(tx_buffer_[i]) << " "; - // std::cout << std::endl;*/ + // std::cout << "not updating light cmd" << std::endl; + } } -void TracerBase::SendControlCmd() -{ - TracerMessage c_msg; - c_msg.type = TracerModeControlMsg; - - mode_cmd_mutex_.lock(); - c_msg.body.mode_cmd_msg.data.cmd.control_mode=0x01; - mode_cmd_mutex_.unlock(); - c_msg.body.mode_cmd_msg.data.cmd.reserved0=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved1=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved2=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved3=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved4=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved5=0; - c_msg.body.mode_cmd_msg.data.cmd.reserved6=0; - if (can_connected_) - { - // send to can bus - can_frame c_frame; - EncodeTracerMsgToCAN(&c_msg, &c_frame); - can_if_->SendFrame(c_frame); - } - else - { - // TODO - // send to serial port - // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); - // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); - } +void TracerBase::EnableCommandedMode() { + AgxMessage c_msg; + c_msg.type = AgxMsgCtrlModeSelect; + memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8); + c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN; + // send to can bus + can_frame c_frame; + EncodeCanFrame(&c_msg, &c_frame); + can_if_->SendFrame(c_frame); } -TracerState TracerBase::GetTracerState() -{ - std::lock_guard guard(tracer_state_mutex_); - return tracer_state_; +void TracerBase::SendMotionCmd(uint8_t count) { + // motion control message + AgxMessage m_msg; + m_msg.type = AgxMsgMotionCommand; + memset(m_msg.body.motion_command_msg.raw, 0, 8); + + motion_cmd_mutex_.lock(); + int16_t linear_cmd = + static_cast(current_motion_cmd_.linear_velocity) * 1000; + int16_t angular_cmd = + static_cast(current_motion_cmd_.angular_velocity) * 1000; + motion_cmd_mutex_.unlock(); + + // SendControlCmd(); + m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte = + (static_cast(linear_cmd) >> 8) & 0x00ff; + m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte = + (static_cast(linear_cmd) >> 0) & 0x00ff; + m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte = + (static_cast(angular_cmd) >> 8) & 0x00ff; + m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte = + (static_cast(angular_cmd) >> 0) & 0x00ff; + + // send to can bus + can_frame m_frame; + EncodeCanFrame(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); } -UartTracerState TracerBase::GetUartTracerState() -{ - std::lock_guard guard(uart_tracer_state_mutex_); - return uart_tracer_state_; +void TracerBase::SendLightCmd(uint8_t count) { + AgxMessage l_msg; + l_msg.type = AgxMsgLightCommand; + memset(l_msg.body.light_command_msg.raw, 0, 8); + + light_cmd_mutex_.lock(); + if (light_ctrl_enabled_) { + l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE; + + l_msg.body.light_command_msg.cmd.front_light_mode = + static_cast(current_light_cmd_.front_mode); + l_msg.body.light_command_msg.cmd.front_light_custom = + current_light_cmd_.front_custom_value; + l_msg.body.light_command_msg.cmd.rear_light_mode = + static_cast(current_light_cmd_.rear_mode); + l_msg.body.light_command_msg.cmd.rear_light_custom = + current_light_cmd_.rear_custom_value; + } else { + l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE; + } + light_ctrl_requested_ = false; + light_cmd_mutex_.unlock(); + + l_msg.body.light_command_msg.cmd.count = count; + + // send to can bus + can_frame l_frame; + EncodeCanFrame(&l_msg, &l_frame); + can_if_->SendFrame(l_frame); } -void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag) -{ - // make sure cmd thread is started before attempting to send commands - if (!cmd_thread_started_) StartCmdThread(); - if(can_connected_) - { - if (linear_vel < TracerMotionCmd::min_linear_velocity) - linear_vel = TracerMotionCmd::min_linear_velocity; - if (linear_vel > TracerMotionCmd::max_linear_velocity) - linear_vel = TracerMotionCmd::max_linear_velocity; - if (angular_vel < TracerMotionCmd::min_angular_velocity) - angular_vel = TracerMotionCmd::min_angular_velocity; - if (angular_vel > TracerMotionCmd::max_angular_velocity) - angular_vel = TracerMotionCmd::max_angular_velocity; - - std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity_H = static_cast(linear_vel*1000)>>8; - current_motion_cmd_.linear_velocity_L = static_cast(linear_vel*1000)&0xff; - current_motion_cmd_.angular_velocity_H = static_cast(angular_vel*1000)>>8; - current_motion_cmd_.angular_velocity_L = static_cast(angular_vel*1000)&0xff; - current_motion_cmd_.fault_clear_flag = fault_clr_flag; - } - else - { - if (linear_vel < UartTracerMotionCmd::min_linear_velocity) - linear_vel = UartTracerMotionCmd::min_linear_velocity; - if (linear_vel > UartTracerMotionCmd::max_linear_velocity) - linear_vel = UartTracerMotionCmd::max_linear_velocity; - if (angular_vel < UartTracerMotionCmd::min_angular_velocity) - angular_vel = UartTracerMotionCmd::min_angular_velocity; - if (angular_vel > UartTracerMotionCmd::max_angular_velocity) - angular_vel = UartTracerMotionCmd::max_angular_velocity; - - std::lock_guard guard(motion_cmd_mutex_); - uart_current_motion_cmd_.linear_velocity = static_cast( - linear_vel / UartTracerMotionCmd::max_linear_velocity * 100.0); - uart_current_motion_cmd_.angular_velocity = static_cast( - angular_vel / UartTracerMotionCmd::max_angular_velocity * 100.0); - //uart_current_motion_cmd_.fault_clear_flag = FaultClearFlag::NO_FAULT; - //std::cout<<"linear_vel:"< guard(tracer_state_mutex_); + return tracer_state_; } -void TracerBase::SetLightCommand(TracerLightCmd cmd) -{ - if (!cmd_thread_started_) - StartCmdThread(); +void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) { + // make sure cmd thread is started before attempting to send commands + if (!cmd_thread_started_) StartCmdThread(); - std::lock_guard guard(light_cmd_mutex_); - current_light_cmd_ = cmd; - light_ctrl_enabled_ = true; - light_ctrl_requested_ = true; - FeedCmdTimeoutWatchdog(); + if (linear_vel < TracerMotionCmd::min_linear_velocity) + linear_vel = TracerMotionCmd::min_linear_velocity; + if (linear_vel > TracerMotionCmd::max_linear_velocity) + linear_vel = TracerMotionCmd::max_linear_velocity; + if (angular_vel < TracerMotionCmd::min_angular_velocity) + angular_vel = TracerMotionCmd::min_angular_velocity; + if (angular_vel > TracerMotionCmd::max_angular_velocity) + angular_vel = TracerMotionCmd::max_angular_velocity; + + std::lock_guard guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = linear_vel; + current_motion_cmd_.angular_velocity = angular_vel; + + FeedCmdTimeoutWatchdog(); } -void TracerBase::DisableLightCmdControl() -{ - std::lock_guard guard(light_cmd_mutex_); - light_ctrl_enabled_ = false; - light_ctrl_requested_ = true; +void TracerBase::SetLightCommand(TracerLightCmd cmd) { + if (!cmd_thread_started_) StartCmdThread(); + + std::lock_guard guard(light_cmd_mutex_); + current_light_cmd_ = cmd; + light_ctrl_enabled_ = true; + light_ctrl_requested_ = true; } -void TracerBase::ParseCANFrame(can_frame *rx_frame) -{ - // otherwise, update robot state with new frame - TracerMessage status_msg; - DecodeTracerMsgFromCAN(rx_frame, &status_msg);//assigned a value to status_msg from can include status_msg->type - NewStatusMsgReceivedCallback(status_msg); +void TracerBase::DisableLightCmdControl() { + std::lock_guard guard(light_cmd_mutex_); + light_ctrl_enabled_ = false; + light_ctrl_requested_ = true; } -void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) -{ - // std::cout << "bytes received from serial: " << bytes_received << std::endl; - // serial_parser_.PrintStatistics(); - // serial_parser_.ParseBuffer(buf, bytes_received); - - UartTracerMessage status_msg; - for (int i = 0; i < bytes_received; ++i) - { - if (DecodeTracerMsgFromUART(buf[i], &status_msg)) - UartNewStatusMsgReceivedCallback(status_msg); - } +void TracerBase::ParseCANFrame(can_frame *rx_frame) { + AgxMessage status_msg; + DecodeCanFrame(rx_frame, &status_msg); + NewStatusMsgReceivedCallback(status_msg); } -void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg) -{ - // std::cout << "new status msg received" << std::endl; - std::lock_guard guard(tracer_state_mutex_); - UpdateTracerState(msg, tracer_state_); +void TracerBase::NewStatusMsgReceivedCallback(const AgxMessage &msg) { + // std::cout << "new status msg received" << std::endl; + std::lock_guard guard(tracer_state_mutex_); + UpdateTracerState(msg, tracer_state_); } -void TracerBase::UartNewStatusMsgReceivedCallback(const UartTracerMessage &msg) -{ - // std::cout << "new status msg received" << std::endl; - std::lock_guard guard(tracer_state_mutex_); - UartUpdateTracerState(msg, uart_tracer_state_); -} - -void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state) -{ - switch (status_msg.type) - { - case TracerMotionStatusMsg: - { - // std::cout << "motion control feedback received" << std::endl; - const MotionStatusMessage &msg = status_msg.body.motion_status_msg; - state.linear_velocity = static_cast(static_cast(msg.data.status.linear_velocity.low_byte) | static_cast(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0; - state.angular_velocity = static_cast(static_cast(msg.data.status.angular_velocity.low_byte) | static_cast(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0; - break; - } - case TracerLightStatusMsg: - { - // std::cout << "light control feedback received" << std::endl; - const LightStatusMessage &msg = status_msg.body.light_status_msg; - if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) - state.light_control_enabled = false; - else - state.light_control_enabled = true; - state.front_light_state.mode = msg.data.status.front_light_mode; - state.front_light_state.custom_value = msg.data.status.front_light_custom; - //state.rear_light_state.mode = msg.data.status.rear_light_mode; - //state.rear_light_state.custom_value = msg.data.status.rear_light_custom; - break; - } - case TracerSystemStatusMsg: - { - // std::cout << "system status feedback received" << std::endl; - const SystemStatusMessage &msg = status_msg.body.system_status_msg; - state.control_mode = msg.data.status.control_mode; - state.base_state = msg.data.status.base_state; - state.battery_voltage = (static_cast(msg.data.status.battery_voltage.low_byte) | static_cast(msg.data.status.battery_voltage.high_byte) << 8) / 10.0; - state.fault_code = msg.data.status.fault_code; - break; - } - case TracerMotorDriverStatusMsg: - { - // std::cout << "motor 1 driver feedback received" << std::endl; - //const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg; - const MotorHeightSpeedStatusMessage &msg = status_msg.body.motor_heigh_speed_msg; - for (int i = 0; i < 2; ++i) - { - //state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].current = (static_cast(msg.data.status.current.low_byte) | static_cast(msg.data.status.current.high_byte) << 8) / 10.0; - state.motor_states[status_msg.body.motor_heigh_speed_msg.motor_id].rpm = static_cast(static_cast(msg.data.status.rpm.low_byte) | static_cast(msg.data.status.rpm.high_byte) << 8); - //state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature; - } - break; - } - case TracerOdometerMsg: - { - // std::cout << "Odometer msg feedback received" << std::endl; - const OdometerMessage &msg = status_msg.body.odom_msg; - state.right_odomter=static_cast((static_cast(msg.data.status.rightodometer.lowest))|(static_cast(msg.data.status.rightodometer.sec_lowest)<<8)|(static_cast(msg.data.status.rightodometer.sec_highest)<<16)|(static_cast(msg.data.status.rightodometer.highest)<<24)); - state.left_odomter=static_cast((static_cast(msg.data.status.leftodometer.lowest))|(static_cast(msg.data.status.leftodometer.sec_lowest)<<8)|(static_cast(msg.data.status.leftodometer.sec_highest)<<16)|(static_cast(msg.data.status.leftodometer.highest)<<24)); - - } - } -} - -void TracerBase::UartUpdateTracerState(const UartTracerMessage &status_msg, UartTracerState &state) -{ +void TracerBase::UpdateTracerState(const AgxMessage &status_msg, + TracerState &state) { switch (status_msg.type) { - case UartTracerMotionStatusMsg: { + case AgxMsgSystemState: { + // std::cout << "system status feedback received" << std::endl; + const SystemStateMessage &msg = status_msg.body.system_state_msg; + state.control_mode = msg.state.control_mode; + state.base_state = msg.state.vehicle_state; + state.battery_voltage = + (static_cast(msg.state.battery_voltage.low_byte) | + static_cast(msg.state.battery_voltage.high_byte) << 8) / + 10.0; + state.fault_code = msg.state.fault_code; + break; + } + case AgxMsgMotionState: { // std::cout << "motion control feedback received" << std::endl; - const UartMotionStatusMessage &msg = status_msg.body.motion_status_msg; + const MotionStateMessage &msg = status_msg.body.motion_state_msg; state.linear_velocity = static_cast( - static_cast(msg.data.status.linear_velocity.low_byte) | - static_cast(msg.data.status.linear_velocity.high_byte) - << 8) / + static_cast(msg.state.linear_velocity.low_byte) | + static_cast(msg.state.linear_velocity.high_byte) << 8) / 1000.0; state.angular_velocity = static_cast( - static_cast(msg.data.status.angular_velocity.low_byte) | - static_cast(msg.data.status.angular_velocity.high_byte) + static_cast(msg.state.angular_velocity.low_byte) | + static_cast(msg.state.angular_velocity.high_byte) << 8) / 1000.0; break; } - case UartTracerLightStatusMsg: { + case AgxMsgLightState: { // std::cout << "light control feedback received" << std::endl; - const UartLightStatusMessage &msg = status_msg.body.light_status_msg; - if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) + const LightStateMessage &msg = status_msg.body.light_state_msg; + if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE) state.light_control_enabled = false; else state.light_control_enabled = true; - state.front_light_state.mode = msg.data.status.front_light_mode; - state.front_light_state.custom_value = msg.data.status.front_light_custom; - state.rear_light_state.mode = msg.data.status.rear_light_mode; - state.rear_light_state.custom_value = msg.data.status.rear_light_custom; + state.front_light_state.mode = msg.state.front_light_mode; + state.front_light_state.custom_value = msg.state.front_light_custom; break; } - case UartTracerSystemStatusMsg: { - // std::cout << "system status feedback received" << std::endl; - const UartSystemStatusMessage &msg = status_msg.body.system_status_msg; - state.control_mode = msg.data.status.control_mode; - state.base_state = msg.data.status.base_state; - state.battery_voltage = - (static_cast(msg.data.status.battery_voltage.low_byte) | - static_cast(msg.data.status.battery_voltage.high_byte) - << 8) / + case AgxMsgActuatorHSState: { + // std::cout << "actuator hs feedback received" << std::endl; + const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg; + state.actuator_states[msg.motor_id].motor_current = + (static_cast(msg.data.state.current.low_byte) | + static_cast(msg.data.state.current.high_byte) << 8) / 10.0; - state.fault_code = - (static_cast(msg.data.status.fault_code.low_byte) | - static_cast(msg.data.status.fault_code.high_byte) << 8); + state.actuator_states[msg.motor_id].motor_rpm = static_cast( + static_cast(msg.data.state.rpm.low_byte) | + static_cast(msg.data.state.rpm.high_byte) << 8); + state.actuator_states[msg.motor_id].motor_pulses = static_cast( + static_cast(msg.data.state.pulse_count.low_byte) | + static_cast(msg.data.state.pulse_count.high_byte) << 8); break; } - case UartTracerMotorDriverStatusMsg: { - // std::cout << "motor 1 driver feedback received" << std::endl; - const UartMotorDriverStatusMessage &msg = - status_msg.body.motor_driver_status_msg; - for (int i = 0; i < UartTracerState::motor_num; ++i) { - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .current = - (static_cast(msg.data.status.current.low_byte) | - static_cast(msg.data.status.current.high_byte) << 8) / + case AgxMsgActuatorLSState: { + // std::cout << "actuator ls feedback received" << std::endl; + const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg; + for (int i = 0; i < 2; ++i) { + state.actuator_states[msg.motor_id].driver_voltage = + (static_cast(msg.data.state.driver_voltage.low_byte) | + static_cast(msg.data.state.driver_voltage.high_byte) + << 8) / 10.0; - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .rpm = static_cast( - static_cast(msg.data.status.rpm.low_byte) | - static_cast(msg.data.status.rpm.high_byte) << 8); - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .temperature = msg.data.status.temperature; + state.actuator_states[msg.motor_id] + .driver_temperature = static_cast( + static_cast(msg.data.state.driver_temperature.low_byte) | + static_cast(msg.data.state.driver_temperature.high_byte) + << 8); + state.actuator_states[msg.motor_id].motor_temperature = + msg.data.state.motor_temperature; + state.actuator_states[msg.motor_id].driver_state = + msg.data.state.driver_state; } break; } - default:break; + case AgxMsgOdometry: { + // std::cout << "Odometer msg feedback received" << std::endl; + const OdometryMessage &msg = status_msg.body.odometry_msg; + state.right_odometry = static_cast( + (static_cast(msg.state.right_wheel.lsb)) | + (static_cast(msg.state.right_wheel.low_byte) << 8) | + (static_cast(msg.state.right_wheel.high_byte) << 16) | + (static_cast(msg.state.right_wheel.msb) << 24)); + state.left_odometry = static_cast( + (static_cast(msg.state.left_wheel.lsb)) | + (static_cast(msg.state.left_wheel.low_byte) << 8) | + (static_cast(msg.state.left_wheel.high_byte) << 16) | + (static_cast(msg.state.left_wheel.msb) << 24)); + } } } -} // namespace westonrobot +} // namespace westonrobot diff --git a/ugv_sdk/src/tracer_can_parser.c b/ugv_sdk/src/tracer_can_parser.c deleted file mode 100644 index 89f7dfa..0000000 --- a/ugv_sdk/src/tracer_can_parser.c +++ /dev/null @@ -1,140 +0,0 @@ -/* - * tracer_can_parser.c - * - * Created on: Apr 14, 2020 10:35 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/tracer/tracer_can_parser.h" - -#include "string.h" - -static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame); -static void EncodeTracerModeControlMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame); - -bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg) -{ - msg->type = TracerMsgNone; - - switch (rx_frame->can_id) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_STATUS_ID: - { - msg->type = TracerMotionStatusMsg; - memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_SYSTEM_STATUS_STATUS_ID: - { - msg->type = TracerSystemStatusMsg; - 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 = TracerMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR1_ID; - msg->body.motor_heigh_speed_msg.motor_id=TRACER_MOTOR1_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - memcpy(msg->body.motor_heigh_speed_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR2_DRIVER_STATUS_ID: - { - msg->type = TracerMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_ID; - msg->body.motor_heigh_speed_msg.motor_id = TRACER_MOTOR2_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - memcpy(msg->body.motor_heigh_speed_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_CMD_ID: - { - msg->type = TracerMotionCmdMsg; - memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_ODOMETER_ID: - { - msg->type = TracerOdometerMsg; - memcpy(msg->body.odom_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - default: - break; - } - - return true; -} - -void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame) -{ - switch (msg->type) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case TracerMotionStatusMsg: - { - tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case TracerSystemStatusMsg: - { - 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 TracerMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID) - tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID) - tx_frame->can_id = CAN_MSG_MOTOR2_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 TracerMotionCmdMsg: - { - EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame); - break; - } - case TracerModeControlMsg: - { - EncodeTracerModeControlMsgToCAN(&(msg->body.mode_cmd_msg), tx_frame); - break; - } - default: - break; - } - //tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} - -void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame) -{ - tx_frame->can_id = CAN_MSG_MOTION_CMD_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - //tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} -void EncodeTracerModeControlMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame) -{ - tx_frame->can_id = CAN_MSG_COMTROL_MODE_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - //tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} -uint8_t CalcTracerCANChecksum(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; -} diff --git a/ugv_sdk/src/tracer_uart_parser.c b/ugv_sdk/src/tracer_uart_parser.c deleted file mode 100644 index d497dc9..0000000 --- a/ugv_sdk/src/tracer_uart_parser.c +++ /dev/null @@ -1,587 +0,0 @@ -#include "ugv_sdk/tracer/tracer_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 -} TracerSerialDecodeState; - -#define PAYLOAD_BUFFER_SIZE (TRACER_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, UartTracerMessage *msg); -static uint8_t CalcBufferedFrameChecksum(); -static bool ConstructStatusMessage(UartTracerMessage *msg); -static bool ConstructControlMessage(UartTracerMessage *msg); - -static void EncodeMotionControlMsgToUART(const UartMotionControlMessage *msg, uint8_t *buf, uint8_t *len); -static void EncodeLightControlMsgToUART(const UartLightControlMessage *msg, uint8_t *buf, uint8_t *len); - -void EncodeTracerMsgToUART(const UartTracerMessage *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 UartTracerMotionStatusMsg: - { - 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 UartTracerLightStatusMsg: - { - 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_light_mode; - buf[7] = msg->body.light_status_msg.data.status.front_light_custom; - buf[8] = msg->body.light_status_msg.data.status.rear_light_mode; - buf[9] = msg->body.light_status_msg.data.status.rear_light_custom; - buf[10] = 0; - buf[11] = msg->body.light_status_msg.data.status.count; - break; - } - case UartTracerSystemStatusMsg: - { - 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 UartTracerMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID) - buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID) - buf[4] = UART_FRAME_MOTOR2_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 UartTracerMotionControlMsg: - { - EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len); - break; - } - case UartTracerLightControlMsg: - { - EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len); - break; - } - default: - break; - } - - buf[12] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool DecodeTracerMsgFromUART(uint8_t c, UartTracerMessage *msg) -{ - static UartTracerMessage decoded_msg; - - bool result = ParseChar(c, &decoded_msg); - if (result) - *msg = decoded_msg; - return result; -} - -void EncodeMotionControlMsgToUART(const UartMotionControlMessage *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] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -void EncodeLightControlMsgToUART(const UartLightControlMessage *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_light_mode; - buf[7] = msg->data.cmd.front_light_custom; - buf[8] = msg->data.cmd.rear_light_mode; - buf[9] = msg->data.cmd.rear_light_custom; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool ParseChar(uint8_t c, UartTracerMessage *msg) -{ - static TracerSerialDecodeState 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_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(UartTracerMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_MOTION_CONTROL_ID: - { - msg->type = UartTracerMotionControlMsg; - 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 = UartTracerLightControlMsg; - msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_control_msg.data.cmd.rear_light_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(UartTracerMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_SYSTEM_STATUS_ID: - { - msg->type = UartTracerSystemStatusMsg; - 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 = UartTracerMotionStatusMsg; - 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 = UartTracerMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = TRACER_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 = UartTracerMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = TRACER_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_LIGHT_STATUS_ID: - { - msg->type = UartTracerLightStatusMsg; - msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_status_msg.data.status.rear_light_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 CalcTracerUARTChecksum(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; -}