From 3c56ae3a0a4a1c0f58178a08197f3034ec00c574 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 8 Jul 2021 23:40:29 +0800 Subject: [PATCH] made scout_base to be class template --- CMakeLists.txt | 8 +- demo/scout_demo/scout_v2_demo.cpp | 2 +- demo/tracer_demo/tracer_demo.cpp | 2 +- .../robot_base}/agilex_base.hpp | 5 +- .../robot_base}/ranger_base.hpp | 0 .../ugv_sdk/details/robot_base/scout_base.hpp | 120 ++++ .../robot_base}/tracer_base.hpp | 6 +- .../ugv_sdk/interface/parser_interface.hpp | 12 +- include/ugv_sdk/mobile_base/scout_robot.hpp | 2 +- .../scout_base_v1.hpp} | 21 +- .../protocol_v1/scout_protocol_v1_parser.hpp | 32 + sample/tracer_demo.cpp | 2 +- src/mobile_base/scout_robot.cpp | 2 +- .../scout_can_parser.c} | 2 +- .../protocol_v1/scout/scout_can_parser.h | 2 +- .../protocol_v1/scout/scout_protocol.h | 0 .../scout/scout_protocol_v1_parser.cpp | 32 + src/protocol_v1/scout/scout_uart_parser.c | 629 ++++++++++++++++++ src/protocol_v1/scout/scout_uart_parser.h | 32 + src/protocol_v2/scout_base.cpp | 98 --- src/protocol_v2/tracer_base.cpp | 16 +- 21 files changed, 887 insertions(+), 138 deletions(-) rename include/ugv_sdk/{mobile_base => details/robot_base}/agilex_base.hpp (98%) rename include/ugv_sdk/{protocol_v2 => details/robot_base}/ranger_base.hpp (100%) create mode 100644 include/ugv_sdk/details/robot_base/scout_base.hpp rename include/ugv_sdk/{protocol_v2 => details/robot_base}/tracer_base.hpp (87%) rename include/ugv_sdk/{protocol_v2/scout_base_v2.hpp => protocol_v1/scout_base_v1.hpp} (67%) create mode 100644 include/ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp rename src/protocol_v1/{scout_can_parser_v1.c => scout/scout_can_parser.c} (99%) rename include/ugv_sdk/protocol_v1/scout_can_parser_v1.h => src/protocol_v1/scout/scout_can_parser.h (93%) rename include/ugv_sdk/protocol_v1/scout_protocol_v1.h => src/protocol_v1/scout/scout_protocol.h (100%) create mode 100644 src/protocol_v1/scout/scout_protocol_v1_parser.cpp create mode 100644 src/protocol_v1/scout/scout_uart_parser.c create mode 100644 src/protocol_v1/scout/scout_uart_parser.h delete mode 100644 src/protocol_v2/scout_base.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index da39283..d327f62 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,16 +81,18 @@ add_library(${PROJECT_NAME} ######################## ## protocol v2 support # parser - # src/protocol_v2/agilex_base.cpp src/protocol_v2/agx_msg_parser.c src/protocol_v2/protocol_v2_parser.cpp # robots - src/protocol_v2/scout_base.cpp + # src/protocol_v2/scout_base.cpp # src/protocol_v2/tracer_base.cpp # src/protocol_v2/ranger_base.cpp ######################## ## legacy protocol v1 support - src/protocol_v1/scout_can_parser_v1.c + src/protocol_v1/scout/scout_uart_parser.c + src/protocol_v1/scout/scout_can_parser.c + src/protocol_v1/scout/scout_protocol_v1_parser.cpp + ) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) diff --git a/demo/scout_demo/scout_v2_demo.cpp b/demo/scout_demo/scout_v2_demo.cpp index 0f1c358..92bfe62 100644 --- a/demo/scout_demo/scout_v2_demo.cpp +++ b/demo/scout_demo/scout_v2_demo.cpp @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" +#include "ugv_sdk/details/robot_base/scout_base.hpp" using namespace westonrobot; diff --git a/demo/tracer_demo/tracer_demo.cpp b/demo/tracer_demo/tracer_demo.cpp index e7b22cc..a738871 100644 --- a/demo/tracer_demo/tracer_demo.cpp +++ b/demo/tracer_demo/tracer_demo.cpp @@ -23,7 +23,7 @@ int main(int argc, char **argv) { return -1; } - TracerBase tracer; + TracerBaseV2 tracer; tracer.Connect(device_name); tracer.EnableCommandedMode(); diff --git a/include/ugv_sdk/mobile_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp similarity index 98% rename from include/ugv_sdk/mobile_base/agilex_base.hpp rename to include/ugv_sdk/details/robot_base/agilex_base.hpp index bca5a79..996c26a 100644 --- a/include/ugv_sdk/mobile_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -119,11 +119,8 @@ class AgilexBase : public RobotInterface { } protected: - std::mutex state_mutex_; - // std::mutex motion_cmd_mutex_; - MotionCommandMessage current_motion_cmd_; - ParserType parser_; + std::mutex state_mutex_; // communication interface bool can_connected_ = false; diff --git a/include/ugv_sdk/protocol_v2/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp similarity index 100% rename from include/ugv_sdk/protocol_v2/ranger_base.hpp rename to include/ugv_sdk/details/robot_base/ranger_base.hpp diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp new file mode 100644 index 0000000..a5501e8 --- /dev/null +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -0,0 +1,120 @@ +/* + * scout_base.hpp + * + * Created on: Dec 23, 2020 14:39 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_BASE_HPP +#define SCOUT_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/interface/scout_interface.hpp" +#include "ugv_sdk/details/robot_base/agilex_base.hpp" +#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp" + +namespace westonrobot { +template +class ScoutBase : public AgilexBase, public ScoutInterface { + public: + ScoutBase() : AgilexBase(){}; + ~ScoutBase() = default; + + // set up connection + void Connect(std::string can_name) override { + AgilexBase::ConnectPort( + can_name, std::bind(&ScoutBase::ParseCANFrame, this, + std::placeholders::_1)); + } + + void Connect(std::string uart_name, uint32_t baudrate) override { + // TODO + } + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel) override { + AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0); + } + + void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) override { + AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value); + } + + // get robot state + ScoutState GetRobotState() override { + std::lock_guard guard(AgilexBase::state_mutex_); + return scout_state_; + } + + void ResetRobotState() override { + // TODO + } + + private: + ScoutState scout_state_; + + void ParseCANFrame(can_frame *rx_frame) override { + AgxMessage status_msg; + DecodeCanFrame(rx_frame, &status_msg); + std::lock_guard guard(AgilexBase::state_mutex_); + UpdateScoutState(status_msg, scout_state_); + } + + void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) { + switch (status_msg.type) { + case AgxMsgSystemState: { + // std::cout << "system status feedback received" << std::endl; + state.system_state = status_msg.body.system_state_msg; + break; + } + case AgxMsgMotionState: { + // std::cout << "motion control feedback received" << std::endl; + state.motion_state = status_msg.body.motion_state_msg; + break; + } + case AgxMsgLightState: { + // std::cout << "light control feedback received" << std::endl; + state.light_state = status_msg.body.light_state_msg; + break; + } + case AgxMsgRcState: { + state.rc_state = status_msg.body.rc_state_msg; + break; + } + case AgxMsgActuatorHSState: { + // std::cout << "actuator hs feedback received" << std::endl; + state + .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] = + status_msg.body.actuator_hs_state_msg; + break; + } + case AgxMsgActuatorLSState: { + // std::cout << "actuator ls feedback received" << std::endl; + state + .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = + status_msg.body.actuator_ls_state_msg; + break; + } + /* sensor feedback */ + case AgxMsgOdometry: { + // std::cout << "Odometer msg feedback received" << std::endl; + state.odometry = status_msg.body.odometry_msg; + } + default: + break; + } + } +}; + + +using ScoutBaseV2 = ScoutBase; +} // namespace westonrobot + +#endif /* SCOUT_BASE_HPP */ diff --git a/include/ugv_sdk/protocol_v2/tracer_base.hpp b/include/ugv_sdk/details/robot_base/tracer_base.hpp similarity index 87% rename from include/ugv_sdk/protocol_v2/tracer_base.hpp rename to include/ugv_sdk/details/robot_base/tracer_base.hpp index 684ccac..84d897e 100644 --- a/include/ugv_sdk/protocol_v2/tracer_base.hpp +++ b/include/ugv_sdk/details/robot_base/tracer_base.hpp @@ -19,10 +19,10 @@ #include "ugv_sdk/protocol_v2/agilex_base.hpp" namespace westonrobot { -class TracerBase : public AgilexBase, public TracerInterface { +class TracerBaseV2 : public AgilexBase, public TracerInterface { public: - TracerBase() : AgilexBase(){}; - ~TracerBase() = default; + TracerBaseV2() : AgilexBase(){}; + ~TracerBaseV2() = default; // set up connection void Connect(std::string can_name) override; diff --git a/include/ugv_sdk/interface/parser_interface.hpp b/include/ugv_sdk/interface/parser_interface.hpp index bb4f693..36f280b 100644 --- a/include/ugv_sdk/interface/parser_interface.hpp +++ b/include/ugv_sdk/interface/parser_interface.hpp @@ -31,19 +31,21 @@ struct ParserInterface { virtual bool DecodeMessage(const struct can_frame *rx_frame, AgxMessage *msg) = 0; virtual void EncodeMessage(const AgxMessage *msg, - struct can_frame *tx_frame) = 0; + struct can_frame *tx_frame) = 0; virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) = 0; // UART support - virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg){ - // throw exception + virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) { + // throw exception + return false; }; virtual void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len){ // throw exception }; - virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len){ - // throw exception + virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) { + // throw exception + return 0; }; }; diff --git a/include/ugv_sdk/mobile_base/scout_robot.hpp b/include/ugv_sdk/mobile_base/scout_robot.hpp index 84901ca..022e3d2 100644 --- a/include/ugv_sdk/mobile_base/scout_robot.hpp +++ b/include/ugv_sdk/mobile_base/scout_robot.hpp @@ -24,7 +24,7 @@ class ScoutRobot : public RobotInterface, public ScoutInterface { void Connect(std::string can_name) override; void Connect(std::string uart_name, uint32_t baudrate) override; - void EnableCommandedMode(); + void EnableCommandedMode() override; void SetMotionCommand(double linear_vel, double angular_vel) override; void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, diff --git a/include/ugv_sdk/protocol_v2/scout_base_v2.hpp b/include/ugv_sdk/protocol_v1/scout_base_v1.hpp similarity index 67% rename from include/ugv_sdk/protocol_v2/scout_base_v2.hpp rename to include/ugv_sdk/protocol_v1/scout_base_v1.hpp index 4c0e908..e30e199 100644 --- a/include/ugv_sdk/protocol_v2/scout_base_v2.hpp +++ b/include/ugv_sdk/protocol_v1/scout_base_v1.hpp @@ -1,14 +1,14 @@ /* - * scout_base.hpp + * scout_base_v1.hpp * - * Created on: Dec 23, 2020 14:39 + * Created on: Jul 08, 2021 22:47 * Description: * - * Copyright (c) 2020 Ruixiang Du (rdu) + * Copyright (c) 2021 Ruixiang Du (rdu) */ -#ifndef SCOUT_BASE_HPP -#define SCOUT_BASE_HPP +#ifndef SCOUT_BASE_V1_HPP +#define SCOUT_BASE_V1_HPP #include #include @@ -18,13 +18,14 @@ #include "ugv_sdk/interface/scout_interface.hpp" #include "ugv_sdk/mobile_base/agilex_base.hpp" -#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp" +#include "ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp" namespace westonrobot { -class ScoutBaseV2 : public AgilexBase, public ScoutInterface { +class ScoutBaseV1 : public AgilexBase, + public ScoutInterface { public: - ScoutBaseV2() : AgilexBase(){}; - ~ScoutBaseV2() = default; + ScoutBaseV1() : AgilexBase(){}; + ~ScoutBaseV1() = default; // set up connection void Connect(std::string can_name) override; @@ -48,4 +49,4 @@ class ScoutBaseV2 : public AgilexBase, public ScoutInterface { }; } // namespace westonrobot -#endif /* SCOUT_BASE_HPP */ +#endif /* SCOUT_BASE_V1_HPP */ diff --git a/include/ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp b/include/ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp new file mode 100644 index 0000000..b040eaa --- /dev/null +++ b/include/ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp @@ -0,0 +1,32 @@ +/* + * scout_protocol_v1_parser.hpp + * + * Created on: Jul 08, 2021 22:30 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP +#define SCOUT_PROTOCOL_V1_PARSER_HPP + +#include "ugv_sdk/interface/parser_interface.hpp" + +namespace westonrobot { +class ScoutProtocolV1Parser : public ParserInterface { + public: + bool DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) override; + void EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) override; + uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override; + + // UART support + bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override; + void EncodeMessage(const AgxMessage *msg, uint8_t *buf, + uint8_t *len) override; + uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override; +}; +} // namespace westonrobot + +#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */ diff --git a/sample/tracer_demo.cpp b/sample/tracer_demo.cpp index 8bb5c9a..982ac23 100644 --- a/sample/tracer_demo.cpp +++ b/sample/tracer_demo.cpp @@ -23,7 +23,7 @@ int main(int argc, char **argv) { return -1; } - TracerBase tracer; + TracerBaseV2 tracer; tracer.Connect(device_name); tracer.EnableCommandedMode(); diff --git a/src/mobile_base/scout_robot.cpp b/src/mobile_base/scout_robot.cpp index bde260b..7aa9079 100644 --- a/src/mobile_base/scout_robot.cpp +++ b/src/mobile_base/scout_robot.cpp @@ -8,7 +8,7 @@ */ #include "ugv_sdk/mobile_base/scout_robot.hpp" -#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" +#include "ugv_sdk/details/robot_base/scout_base.hpp" namespace westonrobot { ScoutRobot::ScoutRobot(ProtocolType protocol) { diff --git a/src/protocol_v1/scout_can_parser_v1.c b/src/protocol_v1/scout/scout_can_parser.c similarity index 99% rename from src/protocol_v1/scout_can_parser_v1.c rename to src/protocol_v1/scout/scout_can_parser.c index 7df8640..8f23aa4 100644 --- a/src/protocol_v1/scout_can_parser_v1.c +++ b/src/protocol_v1/scout/scout_can_parser.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/protocol_v1/scout_can_parser_v1.h" +#include "protocol_v1/scout/scout_can_parser.h" #include "string.h" diff --git a/include/ugv_sdk/protocol_v1/scout_can_parser_v1.h b/src/protocol_v1/scout/scout_can_parser.h similarity index 93% rename from include/ugv_sdk/protocol_v1/scout_can_parser_v1.h rename to src/protocol_v1/scout/scout_can_parser.h index 7d7166d..14ae740 100644 --- a/include/ugv_sdk/protocol_v1/scout_can_parser_v1.h +++ b/src/protocol_v1/scout/scout_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/protocol_v1/scout_protocol_v1.h" +#include "protocol_v1/scout/scout_protocol.h" #ifdef __linux__ #include diff --git a/include/ugv_sdk/protocol_v1/scout_protocol_v1.h b/src/protocol_v1/scout/scout_protocol.h similarity index 100% rename from include/ugv_sdk/protocol_v1/scout_protocol_v1.h rename to src/protocol_v1/scout/scout_protocol.h diff --git a/src/protocol_v1/scout/scout_protocol_v1_parser.cpp b/src/protocol_v1/scout/scout_protocol_v1_parser.cpp new file mode 100644 index 0000000..f9760ff --- /dev/null +++ b/src/protocol_v1/scout/scout_protocol_v1_parser.cpp @@ -0,0 +1,32 @@ +/* + * scout_protocol_v1_parser.cpp + * + * Created on: Jul 08, 2021 22:43 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp" + +#include "protocol_v1/scout/scout_can_parser.h" + +namespace westonrobot { +bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) {} + +void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) {} + +uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data, + uint8_t dlc) {} + +// UART support +bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc, + AgxMessage *msg) {} + +void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf, + uint8_t *len) {} + +uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {} +} // namespace westonrobot diff --git a/src/protocol_v1/scout/scout_uart_parser.c b/src/protocol_v1/scout/scout_uart_parser.c new file mode 100644 index 0000000..e2d0afb --- /dev/null +++ b/src/protocol_v1/scout/scout_uart_parser.c @@ -0,0 +1,629 @@ +/* + * scout_uart_parser.c + * + * Created on: Aug 14, 2019 12:02 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "protocol_v1/scout/scout_uart_parser.h" + +// #define USE_XOR_CHECKSUM + +// #define PRINT_CPP_DEBUG_INFO +// #define PRINT_JLINK_DEBUG_INFO + +#ifdef PRINT_CPP_DEBUG_INFO +#undef PRINT_JLINK_DEBUG_INFO +#endif + +#ifdef PRINT_CPP_DEBUG_INFO +#define +#elif (defined(PRINT_JLINK_DEBUG_INFO)) +#include "segger/jlink_rtt.h" +#endif + +typedef enum +{ + WAIT_FOR_SOF1 = 0, + WAIT_FOR_SOF2, + WAIT_FOR_FRAME_LEN, + WAIT_FOR_FRAME_TYPE, + WAIT_FOR_FRAME_ID, + WAIT_FOR_PAYLOAD, + WAIT_FOR_FRAME_COUNT, + WAIT_FOR_CHECKSUM +} ScoutSerialDecodeState; + +#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2) + +#define FRAME_SOF_LEN ((uint8_t)2) +#define FRAME_FIXED_FIELD_LEN ((uint8_t)4) + +#define FRAME_SOF1 ((uint8_t)0x5a) +#define FRAME_SOF2 ((uint8_t)0xa5) + +#define FRAME_TYPE_CONTROL ((uint8_t)0x55) +#define FRAME_TYPE_STATUS ((uint8_t)0xaa) + +#define FRAME_NONE_ID ((uint8_t)0x00) + +// frame buffer +static struct +{ + uint8_t frame_id; + uint8_t frame_type; + uint8_t frame_len; + uint8_t frame_cnt; + uint8_t frame_checksum; + uint8_t internal_checksum; + uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE]; + size_t payload_data_pos; +} uart_parsing_data; + +// statisctics +typedef struct +{ + uint32_t frame_parsed; + uint32_t frame_with_wrong_checksum; +} UARTParsingStats; + +static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123}; + +// internal functions +static bool ParseChar(uint8_t c, ScoutMessage *msg); +static uint8_t CalcBufferedFrameChecksum(); +static bool ConstructStatusMessage(ScoutMessage *msg); +static bool ConstructControlMessage(ScoutMessage *msg); + +static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len); +static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len); + +void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len) +{ + // SOF + buf[0] = FRAME_SOF1; + buf[1] = FRAME_SOF2; + + // frame len, type, ID + buf[2] = 0x0a; + buf[3] = FRAME_TYPE_STATUS; + + switch (msg->type) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case ScoutMotionStatusMsg: + { + buf[4] = UART_FRAME_MOTION_STATUS_ID; + buf[5] = msg->body.motion_status_msg.data.status.linear_velocity.high_byte; + buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte; + buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte; + buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte; + buf[9] = 0; + buf[10] = 0; + buf[11] = msg->body.motion_status_msg.data.status.count; + break; + } + case ScoutLightStatusMsg: + { + buf[4] = UART_FRAME_LIGHT_STATUS_ID; + buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable; + buf[6] = msg->body.light_status_msg.data.status.front_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 ScoutSystemStatusMsg: + { + buf[4] = UART_FRAME_SYSTEM_STATUS_ID; + buf[5] = msg->body.system_status_msg.data.status.base_state; + buf[6] = msg->body.system_status_msg.data.status.control_mode; + buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte; + buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte; + buf[9] = msg->body.system_status_msg.data.status.fault_code.high_byte; + buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte; + buf[11] = msg->body.system_status_msg.data.status.count; + break; + } + case ScoutMotorDriverStatusMsg: + { + if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) + buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) + buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) + buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) + buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID; + buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte; + buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte; + buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte; + buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte; + buf[9] = msg->body.motor_driver_status_msg.data.status.temperature; + buf[10] = 0; + buf[11] = msg->body.motor_driver_status_msg.data.status.count; + break; + } + case ScoutMotionControlMsg: + { + EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len); + break; + } + case ScoutLightControlMsg: + { + EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len); + break; + } + default: + break; + } + + buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); + + // length: SOF + Frame + Checksum + *len = buf[2] + FRAME_SOF_LEN + 1; +} + +bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg) +{ + static ScoutMessage decoded_msg; + + bool result = ParseChar(c, &decoded_msg); + if (result) + *msg = decoded_msg; + return result; +} + +void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len) +{ + // SOF + buf[0] = FRAME_SOF1; + buf[1] = FRAME_SOF2; + + // frame len, type, ID + buf[2] = 0x0a; + buf[3] = FRAME_TYPE_CONTROL; + buf[4] = UART_FRAME_MOTION_CONTROL_ID; + + // frame payload + buf[5] = msg->data.cmd.control_mode; + buf[6] = msg->data.cmd.fault_clear_flag; + buf[7] = msg->data.cmd.linear_velocity_cmd; + buf[8] = msg->data.cmd.angular_velocity_cmd; + buf[9] = 0x00; + buf[10] = 0x00; + + // frame count, checksum + buf[11] = msg->data.cmd.count; + buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); + + // length: SOF + Frame + Checksum + *len = buf[2] + FRAME_SOF_LEN + 1; +} + +void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len) +{ + // SOF + buf[0] = FRAME_SOF1; + buf[1] = FRAME_SOF2; + + // frame len, type, ID + buf[2] = 0x0a; + buf[3] = FRAME_TYPE_CONTROL; + buf[4] = UART_FRAME_LIGHT_CONTROL_ID; + + // frame payload + buf[5] = msg->data.cmd.light_ctrl_enable; + buf[6] = msg->data.cmd.front_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] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); + + // length: SOF + Frame + Checksum + *len = buf[2] + FRAME_SOF_LEN + 1; +} + +bool ParseChar(uint8_t c, ScoutMessage *msg) +{ + static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1; + + bool new_frame_parsed = false; + switch (decode_state) + { + case WAIT_FOR_SOF1: + { + if (c == FRAME_SOF1) + { + uart_parsing_data.frame_id = FRAME_NONE_ID; + uart_parsing_data.frame_type = 0; + uart_parsing_data.frame_len = 0; + uart_parsing_data.frame_cnt = 0; + uart_parsing_data.frame_checksum = 0; + uart_parsing_data.internal_checksum = 0; + uart_parsing_data.payload_data_pos = 0; + memset(uart_parsing_data.payload_buffer, 0, PAYLOAD_BUFFER_SIZE); + + decode_state = WAIT_FOR_SOF2; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "found sof1" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "found sof1\n"); +#endif + } + break; + } + case WAIT_FOR_SOF2: + { + if (c == FRAME_SOF2) + { + decode_state = WAIT_FOR_FRAME_LEN; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "found sof2" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "found sof2\n"); +#endif + } + else + { + decode_state = WAIT_FOR_SOF1; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "failed to find sof2" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "failed to find sof2\n"); +#endif + } + break; + } + case WAIT_FOR_FRAME_LEN: + { + uart_parsing_data.frame_len = c; + decode_state = WAIT_FOR_FRAME_TYPE; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "frame len: " << std::hex << static_cast(frame_len) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkRTTPrintf(0, "frame len: %d\n", frame_len); +#endif + break; + } + case WAIT_FOR_FRAME_TYPE: + { + switch (c) + { + case FRAME_TYPE_CONTROL: + { + uart_parsing_data.frame_type = FRAME_TYPE_CONTROL; + decode_state = WAIT_FOR_FRAME_ID; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "control type frame received" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "control type frame received\n"); +#endif + break; + } + case FRAME_TYPE_STATUS: + { + uart_parsing_data.frame_type = FRAME_TYPE_STATUS; + decode_state = WAIT_FOR_FRAME_ID; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "status type frame received" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "status type frame received\n"); +#endif + break; + } + default: + { +#ifdef PRINT_CPP_DEBUG_INFO + std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n"); +#endif + decode_state = WAIT_FOR_SOF1; + } + } + break; + } + case WAIT_FOR_FRAME_ID: + { + switch (c) + { + case UART_FRAME_SYSTEM_STATUS_ID: + case UART_FRAME_MOTION_STATUS_ID: + case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: + case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: + case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: + case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: + case UART_FRAME_LIGHT_STATUS_ID: + { + uart_parsing_data.frame_id = c; + decode_state = WAIT_FOR_PAYLOAD; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "frame id: " << std::hex << static_cast(frame_id) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkRTTPrintf(0, "frame id: %d\n", frame_id); +#endif + break; + } + default: + { +#ifdef PRINT_CPP_DEBUG_INFO + std::cerr << "ERROR: Unknown frame id" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "ERROR: Unknown frame id\n"); +#endif + decode_state = WAIT_FOR_SOF1; + } + } + break; + } + case WAIT_FOR_PAYLOAD: + { + uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "1 byte added: " << std::hex << static_cast(c) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkRTTPrintf(0, "1 byte added: %d\n", c); +#endif + if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN)) + decode_state = WAIT_FOR_FRAME_COUNT; + break; + } + case WAIT_FOR_FRAME_COUNT: + { + uart_parsing_data.frame_cnt = c; + decode_state = WAIT_FOR_CHECKSUM; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "frame count: " << std::hex << static_cast(frame_cnt) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt); +#endif + break; + } + case WAIT_FOR_CHECKSUM: + { + uart_parsing_data.frame_checksum = c; + uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum(); + new_frame_parsed = true; + decode_state = WAIT_FOR_SOF1; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; + std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum); + JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum); +#endif + break; + } + default: + break; + } + + if (new_frame_parsed) + { + if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum) + { +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "checksum correct" << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "checksum correct\n"); +#endif + if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS) + ConstructStatusMessage(msg); + else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL) + ConstructControlMessage(msg); + ++uart_parsing_stats.frame_parsed; + } + else + { + ++uart_parsing_stats.frame_with_wrong_checksum; +#ifdef PRINT_CPP_DEBUG_INFO + std::cout << "checksum is NOT correct" << std::endl; + std::cout << std::hex << static_cast(frame_id) << " , " << static_cast(frame_len) << " , " << static_cast(frame_cnt) << " , " << static_cast(frame_checksum) << " : " << std::dec << std::endl; + std::cout << "payload: "; + for (int i = 0; i < payload_data_pos; ++i) + std::cout << std::hex << static_cast(payload_buffer[i]) << std::dec << " "; + std::cout << std::endl; + std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; + std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; +#elif (defined(PRINT_JLINK_DEBUG_INFO)) + JLinkWriteString(0, "checksum is NOT correct\n"); +#endif + } + } + + return new_frame_parsed; +} + +bool ConstructControlMessage(ScoutMessage *msg) +{ + if (msg == NULL) + return false; + + switch (uart_parsing_data.frame_id) + { + case UART_FRAME_MOTION_CONTROL_ID: + { + msg->type = ScoutMotionControlMsg; + msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0]; + msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1]; + msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2]; + msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3]; + msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4]; + msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5]; + msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt; + msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_LIGHT_CONTROL_ID: + { + msg->type = ScoutLightControlMsg; + msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; + msg->body.light_control_msg.data.cmd.front_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(ScoutMessage *msg) +{ + if (msg == NULL) + return false; + + switch (uart_parsing_data.frame_id) + { + case UART_FRAME_SYSTEM_STATUS_ID: + { + msg->type = ScoutSystemStatusMsg; + msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0]; + msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1]; + msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4]; + msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5]; + msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_MOTION_STATUS_ID: + { + msg->type = ScoutMotionStatusMsg; + msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motion_status_msg.data.status.reserved0 = 0x00; + msg->body.motion_status_msg.data.status.reserved0 = 0x00; + msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; + msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; + msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; + msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; + msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; + msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; + msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; + msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; + msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; + msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; + msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; + msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; + msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; + msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; + break; + } + case UART_FRAME_LIGHT_STATUS_ID: + { + msg->type = ScoutLightStatusMsg; + msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; + msg->body.light_status_msg.data.status.front_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 CalcScoutUARTChecksum(uint8_t *buf, uint8_t len) +{ + uint8_t checksum = 0; + +#ifdef USE_XOR_CHECKSUM + for (int i = 0; i < len; ++i) + checksum ^= buf[i]; +#else + for (int i = 0; i < len; ++i) + checksum += buf[i]; +#endif + + return checksum; +} + +uint8_t CalcBufferedFrameChecksum() +{ + uint8_t checksum = 0x00; + +#ifdef USE_XOR_CHECKSUM + checksum ^= FRAME_SOF1; + checksum ^= FRAME_SOF2; + checksum ^= uart_parsing_data.frame_len; + checksum ^= uart_parsing_data.frame_type; + checksum ^= uart_parsing_data.frame_id; + for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) + checksum ^= uart_parsing_data.payload_buffer[i]; + checksum ^= uart_parsing_data.frame_cnt; +#else + checksum += FRAME_SOF1; + checksum += FRAME_SOF2; + checksum += uart_parsing_data.frame_len; + checksum += uart_parsing_data.frame_type; + checksum += uart_parsing_data.frame_id; + for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) + checksum += uart_parsing_data.payload_buffer[i]; + checksum += uart_parsing_data.frame_cnt; +#endif + + return checksum; +} diff --git a/src/protocol_v1/scout/scout_uart_parser.h b/src/protocol_v1/scout/scout_uart_parser.h new file mode 100644 index 0000000..58f65a6 --- /dev/null +++ b/src/protocol_v1/scout/scout_uart_parser.h @@ -0,0 +1,32 @@ +/* + * scout_uart_parser.h + * + * Created on: Aug 14, 2019 12:01 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_UART_PARSER_H +#define SCOUT_UART_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#include "protocol_v1/scout/scout_protocol.h" + +bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg); +void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len); + +uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* SCOUT_UART_PARSER_H */ diff --git a/src/protocol_v2/scout_base.cpp b/src/protocol_v2/scout_base.cpp deleted file mode 100644 index 01e20a7..0000000 --- a/src/protocol_v2/scout_base.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * scout_base.cpp - * - * Created on: Jul 08, 2021 12:07 - * Description: - * - * Copyright (c) 2021 Weston Robot Pte. Ltd. - */ - -#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" - -namespace westonrobot { -void ScoutBaseV2::Connect(std::string dev_name) { - AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBaseV2::ParseCANFrame, this, - std::placeholders::_1)); -} - -void ScoutBaseV2::Connect(std::string uart_name, uint32_t baudrate) { - // TODO -} - -void ScoutBaseV2::SetMotionCommand(double linear_vel, double angular_vel) { - AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0); -} - -void ScoutBaseV2::SetLightCommand(LightMode f_mode, uint8_t f_value, - LightMode r_mode, uint8_t r_value) { - AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value); -} - -ScoutState ScoutBaseV2::GetRobotState() { - std::lock_guard guard(state_mutex_); - return scout_state_; -} - -void ScoutBaseV2::ParseCANFrame(can_frame *rx_frame) { - AgxMessage status_msg; - DecodeCanFrame(rx_frame, &status_msg); - std::lock_guard guard(state_mutex_); - UpdateScoutState(status_msg, scout_state_); -} - -void ScoutBaseV2::UpdateScoutState(const AgxMessage &status_msg, - ScoutState &state) { - switch (status_msg.type) { - case AgxMsgSystemState: { - // std::cout << "system status feedback received" << std::endl; - state.system_state = status_msg.body.system_state_msg; - break; - } - case AgxMsgMotionState: { - // std::cout << "motion control feedback received" << std::endl; - state.motion_state = status_msg.body.motion_state_msg; - break; - } - case AgxMsgLightState: { - // std::cout << "light control feedback received" << std::endl; - state.light_state = status_msg.body.light_state_msg; - break; - } - case AgxMsgRcState: { - state.rc_state = status_msg.body.rc_state_msg; - break; - } - case AgxMsgActuatorHSState: { - // std::cout << "actuator hs feedback received" << std::endl; - state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] = - status_msg.body.actuator_hs_state_msg; - break; - } - case AgxMsgActuatorLSState: { - // std::cout << "actuator ls feedback received" << std::endl; - state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = - status_msg.body.actuator_ls_state_msg; - break; - } - /* sensor feedback */ - case AgxMsgOdometry: { - // std::cout << "Odometer msg feedback received" << std::endl; - state.odometry = status_msg.body.odometry_msg; - } - default: - break; - } -} -} // namespace westonrobot diff --git a/src/protocol_v2/tracer_base.cpp b/src/protocol_v2/tracer_base.cpp index d234c2d..29751e4 100644 --- a/src/protocol_v2/tracer_base.cpp +++ b/src/protocol_v2/tracer_base.cpp @@ -15,39 +15,39 @@ #include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { -void TracerBase::Connect(std::string dev_name) { - AgilexBase::ConnectPort(dev_name, std::bind(&TracerBase::ParseCANFrame, this, +void TracerBaseV2::Connect(std::string dev_name) { + AgilexBase::ConnectPort(dev_name, std::bind(&TracerBaseV2::ParseCANFrame, this, std::placeholders::_1)); } -void TracerBase::Connect(std::string uart_name, uint32_t baudrate) { +void TracerBaseV2::Connect(std::string uart_name, uint32_t baudrate) { // TODO } -void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) { +void TracerBaseV2::SetMotionCommand(double linear_vel, double angular_vel) { AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0); } -void TracerBase::SetLightCommand(const TracerLightCmd &cmd) { +void TracerBaseV2::SetLightCommand(const TracerLightCmd &cmd) { if (cmd.cmd_ctrl_allowed) { AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value, LightMode::CONST_OFF, 0); } } -TracerState TracerBase::GetTracerState() { +TracerState TracerBaseV2::GetTracerState() { std::lock_guard guard(state_mutex_); return tracer_state_; } -void TracerBase::ParseCANFrame(can_frame *rx_frame) { +void TracerBaseV2::ParseCANFrame(can_frame *rx_frame) { AgxMessage status_msg; DecodeCanFrame(rx_frame, &status_msg); std::lock_guard guard(state_mutex_); UpdateTracerState(status_msg, tracer_state_); } -void TracerBase::UpdateTracerState(const AgxMessage &status_msg, +void TracerBaseV2::UpdateTracerState(const AgxMessage &status_msg, TracerState &state) { switch (status_msg.type) { case AgxMsgSystemState: {