From 4d4b5baf73b6ac501844b56b470de2b68ce2e9f1 Mon Sep 17 00:00:00 2001 From: agilexrobotics <53333772+agilexrobotics@users.noreply.github.com> Date: Tue, 3 Nov 2020 16:32:51 +0800 Subject: [PATCH] V2.x (#5) * added support of scout and hunter for protocol v2.0 --- ugv_sdk/CMakeLists.txt | 5 + .../include/ugv_sdk/bunker/bunker_base.hpp | 67 ++ .../ugv_sdk/bunker/bunker_can_parser.h | 42 ++ .../bunker_protocol.h} | 47 +- .../bunker_types.hpp} | 24 +- .../{hunter => hunter_v2}/hunter_base.hpp | 12 +- .../{hunter => hunter_v2}/hunter_can_parser.h | 2 +- .../{hunter => hunter_v2}/hunter_protocol.h | 223 ++++--- .../{hunter => hunter_v2}/hunter_types.hpp | 30 +- .../{scout => scout_v2}/scout_base.hpp | 14 +- .../{scout => scout_v2}/scout_can_parser.h | 2 +- .../include/ugv_sdk/scout_v2/scout_protocol.h | 393 ++++++++++++ .../include/ugv_sdk/scout_v2/scout_types.hpp | 140 +++++ .../{scout => scout_v2}/scout_uart_parser.h | 2 +- .../include/ugv_sdk/tracer/tracer_base.hpp | 72 +-- .../ugv_sdk/tracer/tracer_can_parser.h | 2 +- .../include/ugv_sdk/tracer/tracer_protocol.h | 300 ++++++++- .../include/ugv_sdk/tracer/tracer_types.hpp | 99 ++- .../ugv_sdk/tracer/tracer_uart_parser.h | 24 + ugv_sdk/src/bunker_base.cpp | 200 ++++++ ugv_sdk/src/bunker_can_parser.c | 161 +++++ ugv_sdk/src/hunter_base.cpp | 162 +++-- ugv_sdk/src/hunter_can_parser.c | 118 ++-- ugv_sdk/src/scout_base.cpp | 185 ++++-- ugv_sdk/src/scout_can_parser.c | 150 +++-- ugv_sdk/src/scout_uart_parser.c | 70 +-- ugv_sdk/src/tracer_base.cpp | 441 ++++++++----- ugv_sdk/src/tracer_can_parser.c | 32 +- ugv_sdk/src/tracer_uart_parser.c | 587 ++++++++++++++++++ 29 files changed, 2980 insertions(+), 626 deletions(-) create mode 100644 ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp create mode 100644 ugv_sdk/include/ugv_sdk/bunker/bunker_can_parser.h rename ugv_sdk/include/ugv_sdk/{scout/scout_protocol.h => bunker/bunker_protocol.h} (89%) rename ugv_sdk/include/ugv_sdk/{scout/scout_types.hpp => bunker/bunker_types.hpp} (79%) rename ugv_sdk/include/ugv_sdk/{hunter => hunter_v2}/hunter_base.hpp (79%) rename ugv_sdk/include/ugv_sdk/{hunter => hunter_v2}/hunter_can_parser.h (94%) rename ugv_sdk/include/ugv_sdk/{hunter => hunter_v2}/hunter_protocol.h (64%) rename ugv_sdk/include/ugv_sdk/{hunter => hunter_v2}/hunter_types.hpp (56%) rename ugv_sdk/include/ugv_sdk/{scout => scout_v2}/scout_base.hpp (86%) rename ugv_sdk/include/ugv_sdk/{scout => scout_v2}/scout_can_parser.h (94%) create mode 100644 ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h create mode 100644 ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp rename ugv_sdk/include/ugv_sdk/{scout => scout_v2}/scout_uart_parser.h (92%) create mode 100644 ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h create mode 100644 ugv_sdk/src/bunker_base.cpp create mode 100644 ugv_sdk/src/bunker_can_parser.c create mode 100644 ugv_sdk/src/tracer_uart_parser.c diff --git a/ugv_sdk/CMakeLists.txt b/ugv_sdk/CMakeLists.txt index 53872a1..6c45733 100755 --- a/ugv_sdk/CMakeLists.txt +++ b/ugv_sdk/CMakeLists.txt @@ -88,6 +88,11 @@ add_library(${PROJECT_NAME} src/scout_base.cpp src/scout_can_parser.c src/scout_uart_parser.c + src/tracer_base.cpp + src/tracer_can_parser.c + src/tracer_uart_parser.c + src/bunker_base.cpp + src/bunker_can_parser.c ) if(BUILD_WITHOUT_ROS) target_link_libraries(${PROJECT_NAME} wrp_io Threads::Threads) diff --git a/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp b/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp new file mode 100644 index 0000000..ff79963 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp @@ -0,0 +1,67 @@ +/* + * bunker_base.hpp + * + * Created on: Jun 04, 2019 01:22 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef BUNKER_BASE_HPP +#define BUNKER_BASE_HPP + +#include +#include +#include +#include +#include + +#include "ugv_sdk/mobile_base.hpp" + +#include "ugv_sdk/bunker/bunker_protocol.h" +#include "ugv_sdk/bunker/bunker_can_parser.h" +#include "ugv_sdk/bunker/bunker_types.hpp" + +namespace westonrobot { +class BunkerBase : public MobileBase { + public: + BunkerBase() : MobileBase(){}; + ~BunkerBase() = default; + + public: + // motion control + void SetMotionCommand(double linear_vel, double angular_vel, + BunkerMotionCmd::FaultClearFlag fault_clr_flag = + BunkerMotionCmd::FaultClearFlag::NO_FAULT); + + // get robot state + BunkerState GetBunkerState(); + + private: + + + // cmd/status update related variables + std::mutex bunker_state_mutex_; + std::mutex motion_cmd_mutex_; + + BunkerState bunker_state_; + BunkerMotionCmd current_motion_cmd_; + + + + // internal functions + void SendRobotCmd() override; + void ParseCANFrame(can_frame *rx_frame) override; + void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received) override{}; + + void SendMotionCmd(uint8_t count); + void NewStatusMsgReceivedCallback(const BunkerMessage &msg); + + public: + static void UpdateBunkerState(const BunkerMessage &status_msg, + BunkerState &state); +}; +} // namespace westonrobot + +#endif /* BUNKER_BASE_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/bunker/bunker_can_parser.h b/ugv_sdk/include/ugv_sdk/bunker/bunker_can_parser.h new file mode 100644 index 0000000..e480c41 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/bunker/bunker_can_parser.h @@ -0,0 +1,42 @@ +/* + * bunker_can_parser.h + * + * Created on: Aug 31, 2019 04:23 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef BUNKER_CAN_PARSER_H +#define BUNKER_CAN_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "ugv_sdk/bunker/bunker_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 DecodeBunkerMsgFromCAN(const struct can_frame *rx_frame, BunkerMessage *msg); +void EncodeBunkerMsgToCAN(const BunkerMessage *msg, struct can_frame *tx_frame); + +uint8_t CalcBunkerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); + +#ifdef __cplusplus +} +#endif + +#endif /* BUNKER_CAN_PARSER_H */ diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h b/ugv_sdk/include/ugv_sdk/bunker/bunker_protocol.h similarity index 89% rename from ugv_sdk/include/ugv_sdk/scout/scout_protocol.h rename to ugv_sdk/include/ugv_sdk/bunker/bunker_protocol.h index bb0d32b..b44339c 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h +++ b/ugv_sdk/include/ugv_sdk/bunker/bunker_protocol.h @@ -1,14 +1,13 @@ /* - * scout_protocol.h + * bunker_protocol.h * * Created on: Aug 07, 2019 21:49 * Description: * * Copyright (c) 2019 Ruixiang Du (rdu) */ - -#ifndef SCOUT_PROTOCOL_H -#define SCOUT_PROTOCOL_H +#ifndef BUNKER_PROTOCOL_H +#define BUNKER_PROTOCOL_H #ifdef __cplusplus extern "C" { @@ -16,14 +15,14 @@ extern "C" { #include -#define SCOUT_CMD_BUF_LEN 32 -#define SCOUT_STATUS_BUF_LEN 32 -#define SCOUT_FRAME_SIZE 13 +#define BUNKER_CMD_BUF_LEN 32 +#define BUNKER_STATUS_BUF_LEN 32 +#define BUNKER_FRAME_SIZE 13 -#define SCOUT_MOTOR1_ID ((uint8_t)0x00) -#define SCOUT_MOTOR2_ID ((uint8_t)0x01) -#define SCOUT_MOTOR3_ID ((uint8_t)0x02) -#define SCOUT_MOTOR4_ID ((uint8_t)0x03) +#define BUNKER_MOTOR1_ID ((uint8_t)0x00) +#define BUNKER_MOTOR2_ID ((uint8_t)0x01) +//#define BUNKER_MOTOR3_ID ((uint8_t)0x02) +//#define BUNKER_MOTOR4_ID ((uint8_t)0x03) // UART Definitions #define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) @@ -45,8 +44,8 @@ extern "C" { #define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) #define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200) #define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201) -#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202) -#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203) +//#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202) +//#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203) /*--------------------- Control/State Constants ------------------------*/ @@ -240,20 +239,20 @@ typedef struct // For convenience to access status/control message typedef enum { - ScoutMsgNone = 0x00, + BunkerMsgNone = 0x00, // status messages - ScoutMotionStatusMsg = 0x01, - ScoutLightStatusMsg = 0x02, - ScoutSystemStatusMsg = 0x03, - ScoutMotorDriverStatusMsg = 0x04, + BunkerMotionStatusMsg = 0x01, + BunkerLightStatusMsg = 0x02, + BunkerSystemStatusMsg = 0x03, + BunkerMotorDriverStatusMsg = 0x04, // control messages - ScoutMotionControlMsg = 0x21, - ScoutLightControlMsg = 0x22 -} ScoutMsgType; + BunkerMotionControlMsg = 0x21, + BunkerLightControlMsg = 0x22 +} BunkerMsgType; typedef struct { - ScoutMsgType type; + BunkerMsgType type; union { // status messages MotionStatusMessage motion_status_msg; @@ -264,7 +263,7 @@ typedef struct MotionControlMessage motion_control_msg; LightControlMessage light_control_msg; } body; -} ScoutMessage; +} BunkerMessage; #pragma pack(pop) @@ -272,4 +271,4 @@ typedef struct } #endif -#endif /* SCOUT_PROTOCOL_H */ +#endif /* BUNKER_PROTOCOL_H */ diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp b/ugv_sdk/include/ugv_sdk/bunker/bunker_types.hpp similarity index 79% rename from ugv_sdk/include/ugv_sdk/scout/scout_types.hpp rename to ugv_sdk/include/ugv_sdk/bunker/bunker_types.hpp index 9fefcec..d5b4c1b 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp +++ b/ugv_sdk/include/ugv_sdk/bunker/bunker_types.hpp @@ -1,5 +1,5 @@ /* - * scout_state.hpp + * bunker_state.hpp * * Created on: Jun 11, 2019 08:48 * Description: @@ -7,15 +7,15 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#ifndef SCOUT_STATE_HPP -#define SCOUT_STATE_HPP +#ifndef BUNKER_STATE_HPP +#define BUNKER_STATE_HPP #include #include namespace westonrobot { -struct ScoutState +struct BunkerState { enum MotorID { @@ -58,7 +58,7 @@ struct ScoutState double angular_velocity = 0; }; -struct ScoutMotionCmd +struct BunkerMotionCmd { enum class FaultClearFlag { @@ -73,7 +73,7 @@ struct ScoutMotionCmd MOTOR_OVERCURRENT = 0x08 }; - ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0, + BunkerMotionCmd(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) {} @@ -84,11 +84,11 @@ struct ScoutMotionCmd 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 + static constexpr double max_angular_velocity = 1.0; // 3.1415 rad/s + static constexpr double min_angular_velocity = -1.0; // -3.1415 rad/s }; -struct ScoutLightCmd +struct BunkerLightCmd { enum class LightMode { @@ -98,8 +98,8 @@ struct ScoutLightCmd CUSTOM = 0x03 }; - ScoutLightCmd() = default; - ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), + BunkerLightCmd() = default; + BunkerLightCmd(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; @@ -109,4 +109,4 @@ struct ScoutLightCmd }; } // namespace westonrobot -#endif /* SCOUT_STATE_HPP */ +#endif /* BUNKER_STATE_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp similarity index 79% rename from ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp rename to ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp index a946a97..68d0faa 100644 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp +++ b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp @@ -17,9 +17,9 @@ #include "ugv_sdk/mobile_base.hpp" -#include "ugv_sdk/hunter/hunter_protocol.h" -#include "ugv_sdk/hunter/hunter_can_parser.h" -#include "ugv_sdk/hunter/hunter_types.hpp" +#include "ugv_sdk/hunter_v2/hunter_protocol.h" +#include "ugv_sdk/hunter_v2/hunter_can_parser.h" +#include "ugv_sdk/hunter_v2/hunter_types.hpp" namespace westonrobot { class HunterBase : public MobileBase { @@ -31,7 +31,7 @@ class HunterBase : public MobileBase { HunterState GetHunterState(); // motion control - void SetMotionCommand(double linear_vel, double steering_angle, + void SetMotionCommand(double linear_vel, double angular_vel, double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT); @@ -39,6 +39,8 @@ class HunterBase : public MobileBase { // cmd/status update related variables std::mutex hunter_state_mutex_; std::mutex motion_cmd_mutex_; + std::mutex mode_cmd_mutex_; + std::mutex pack_mode_cmd_mutex_; HunterState hunter_state_; HunterMotionCmd current_motion_cmd_; @@ -50,6 +52,8 @@ class HunterBase : public MobileBase { size_t bytes_received) override{}; void SendMotionCmd(uint8_t count); + void SendModeCtl(); + void SetParkMode(); void NewStatusMsgReceivedCallback(const HunterMessage &msg); public: diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h similarity index 94% rename from ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h rename to ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h index f887be3..2c77ddd 100644 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h +++ b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/hunter/hunter_protocol.h" +#include "ugv_sdk/hunter_v2/hunter_protocol.h" #ifdef __linux__ #include diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h similarity index 64% rename from ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h rename to ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h index 6dd0339..d8b417c 100644 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h +++ b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h @@ -25,14 +25,17 @@ extern "C" { #define HUNTER_MOTOR3_ID ((uint8_t)0x02) // CAN Definitions -#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130) -#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131) -#define CAN_MSG_CONFIG_CMD_ID ((uint32_t)0x210) -#define CAN_MSG_CONFIG_STATUS_ID ((uint32_t)0x211) -#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) -#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x201) -#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202) -#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x203) +#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x111) +#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x221) +#define CAN_MSG_SELECT_CONTROL_MODE_ID ((uint32_t)0x421) +#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x211) +#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_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_PARK_CONTROL_ID ((uint32_t)0x131) /*--------------------- Control/State Constants ------------------------*/ @@ -41,7 +44,6 @@ extern "C" { #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) @@ -87,25 +89,7 @@ extern "C" { // Note: id could be different for UART and CAN protocol -// Motion Control -typedef struct { - union - { - struct - { - uint8_t control_mode; - uint8_t fault_clear_flag; - int8_t linear_velocity_cmd; - int8_t angular_velocity_cmd; - uint8_t reserved0; - uint8_t reserved1; - uint8_t count; - uint8_t checksum; - } cmd; - uint8_t raw[8]; - } data; -} MotionCmdMessage; - +// Motion Status Feedback typedef struct { union { @@ -116,15 +100,15 @@ typedef struct { uint8_t high_byte; uint8_t low_byte; } linear_velocity; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; 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; @@ -148,58 +132,25 @@ typedef struct { uint8_t high_byte; uint8_t low_byte; } fault_code; + uint8_t park_mode; uint8_t count; - uint8_t checksum; } status; uint8_t raw[8]; } data; } SystemStatusMessage; -// System Configuration +// Motor Driver Height Speed Status Feedback typedef struct { - union - { - struct - { - uint8_t set_zero_steering; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t reserved4; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} ConfigCmdMessage; - -// System Configuration Status Feedback -typedef struct { - union - { - struct - { - uint8_t set_zero_steering; - uint8_t reserved0; - uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; - uint8_t reserved4; - uint8_t count; - uint8_t checksum; - } status; - uint8_t raw[8]; - } data; -} ConfigStatusMessage; - -// Motor Driver Feedback -typedef struct -{ uint8_t motor_id; - union { + union + { struct { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } rpm; struct { uint8_t high_byte; @@ -207,17 +158,105 @@ typedef struct } current; struct { - uint8_t high_byte; - uint8_t low_byte; - } rpm; - int8_t temperature; - uint8_t reserved0; - uint8_t count; - uint8_t checksum; + uint8_t heighest; + uint8_t sec_heighest; + uint8_t sec_lowest; + uint8_t lowest; + } moter_pose; } status; uint8_t raw[8]; } data; -} MotorDriverStatusMessage; +} MotorDriverHeightSpeedStatusMessage; + +// Motor Driver Low Speed Status Feedback +typedef struct { + uint8_t motor_id; + union + { + struct + { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } driver_voltage; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } driver_temperature; + uint8_t motor_temperature; + uint8_t driver_status; + uint8_t reserved0; + uint8_t reserved1; + } status; + uint8_t raw[8]; + } data; +} MotorDriverLowSpeedStatusMessage; + +// Motion Control +typedef struct { + union + { + struct + { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } linear_velocity_cmd; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } angular_velocity_cmd; + } cmd; + uint8_t raw[8]; + } data; +} MotionControlMessage; + +// Parking Mode Control +typedef struct { + union + { + struct + { + uint8_t packing_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; +} ParkControlMessage; + +// Motion Mode Control +typedef struct { + union + { + struct + { + uint8_t mode_control; + 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; // For convenience to access status/control message typedef enum @@ -225,12 +264,13 @@ typedef enum HunterMsgNone = 0x00, // status messages HunterMotionStatusMsg = 0x01, - HunterSystemStatusMsg = 0x03, - HunterMotorDriverStatusMsg = 0x04, - HunterConfigStatusMsg = 0x05, + HunterSystemStatusMsg = 0x02, + HunterMotorDriverHeightSpeedStatusMsg = 0x03, + HunterMotorDriverLowSpeedStatusMsg = 0x04, // control messages - HunterMotionCmdMsg = 0x21, - HunterConfigCmdMsg = 0x22 + HunterMotionControlMsg = 0x21, + HunterParkControlMsg = 0x22, + HunterControlModeMsg = 0x23 } HunterMsgType; typedef struct @@ -240,11 +280,12 @@ typedef struct // status messages MotionStatusMessage motion_status_msg; SystemStatusMessage system_status_msg; - ConfigStatusMessage config_status_msg; - MotorDriverStatusMessage motor_driver_status_msg; + MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg; + MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg; // control messages - MotionCmdMessage motion_cmd_msg; - ConfigCmdMessage config_cmd_msg; + MotionControlMessage motion_control_msg; + ParkControlMessage park_control_msg; + ModSelectMessage mode_cmd_msg; } body; } HunterMessage; diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp similarity index 56% rename from ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp rename to ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp index c4a487d..953ae8c 100644 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp +++ b/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp @@ -17,16 +17,24 @@ namespace westonrobot { struct HunterState { - struct MotorState + struct MotorHeightSpeedState { double current = 0; // in A double rpm = 0; - double temperature = 0; + double motor_pose = 0; + }; + struct MotorLowSpeedState + { + double driver_voltage = 0; + double driver_temperature = 0; + double motor_temperature = 0; + uint8_t driver_state = 0; }; // base state uint8_t base_state = 0; uint8_t control_mode = 0; + uint8_t park_mode = 1; uint16_t fault_code = 0; double battery_voltage = 0.0; @@ -34,7 +42,8 @@ struct HunterState // motor state static constexpr uint8_t motor_num = 3; - MotorState motor_states[motor_num]; + MotorHeightSpeedState motor_H_state[motor_num]; + MotorLowSpeedState motor_L_state[motor_num]; // motion state double linear_velocity = 0; @@ -56,19 +65,22 @@ struct HunterMotionCmd MOTOR_OVERCURRENT = 0x08 }; - HunterMotionCmd(int8_t linear = 0, int8_t angular = 0, + HunterMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0, int8_t angular_height_byte = 0, int8_t angular_low_byte = 0, FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) - : linear_velocity(linear), angular_velocity(angular), + : linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte), fault_clear_flag(fault_clr_flag) {} - int8_t linear_velocity; - int8_t angular_velocity; + int8_t linear_velocity_height_byte; + int8_t linear_velocity_low_byte; + int8_t angular_velocity_height_byte; + int8_t angular_velocity_low_byte; 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_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree - static constexpr double min_steering_angle = -0.4622; // -0.4622 rad + static constexpr double max_steering_angle = 0.576; // 0.576 rad ~= 30.00 degree + static constexpr double min_steering_angle = -0.576; // -0.576 rad + }; } // namespace westonrobot diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp b/ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp similarity index 86% rename from ugv_sdk/include/ugv_sdk/scout/scout_base.hpp rename to ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp index 81763a7..7f914ea 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp +++ b/ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp @@ -18,10 +18,10 @@ #include "ugv_sdk/mobile_base.hpp" -#include "ugv_sdk/scout/scout_protocol.h" -#include "ugv_sdk/scout/scout_can_parser.h" -#include "ugv_sdk/scout/scout_uart_parser.h" -#include "ugv_sdk/scout/scout_types.hpp" +#include "ugv_sdk/scout_v2/scout_protocol.h" +#include "ugv_sdk/scout_v2/scout_can_parser.h" +#include "ugv_sdk/scout_v2/scout_uart_parser.h" +#include "ugv_sdk/scout_v2/scout_types.hpp" namespace westonrobot { class ScoutBase : public MobileBase { @@ -48,14 +48,19 @@ class ScoutBase : public MobileBase { uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN]; // cmd/status update related variables + std::thread cmd_thread_; std::mutex scout_state_mutex_; std::mutex motion_cmd_mutex_; std::mutex light_cmd_mutex_; + std::mutex mode_cmd_mutex_; ScoutState scout_state_; ScoutMotionCmd current_motion_cmd_; ScoutLightCmd current_light_cmd_; + + + bool light_ctrl_enabled_ = false; bool light_ctrl_requested_ = false; @@ -67,6 +72,7 @@ class ScoutBase : public MobileBase { void SendMotionCmd(uint8_t count); void SendLightCmd(uint8_t count); + void SendModeCtl(); void NewStatusMsgReceivedCallback(const ScoutMessage &msg); public: diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h b/ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h similarity index 94% rename from ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h rename to ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h index 0e44f25..17f6244 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h +++ b/ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/scout/scout_protocol.h" +#include "ugv_sdk/scout_v2/scout_protocol.h" #ifdef __linux__ #include diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h b/ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h new file mode 100644 index 0000000..7e37444 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h @@ -0,0 +1,393 @@ +/* + * scout_protocol.h + * + * Created on: Aug 07, 2019 21:49 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_PROTOCOL_H +#define SCOUT_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define SCOUT_CMD_BUF_LEN 32 +#define SCOUT_STATUS_BUF_LEN 32 +#define SCOUT_FRAME_SIZE 13 + +#define SCOUT_MOTOR1_ID ((uint8_t)0x00) +#define SCOUT_MOTOR2_ID ((uint8_t)0x01) +#define SCOUT_MOTOR3_ID ((uint8_t)0x02) +#define SCOUT_MOTOR4_ID ((uint8_t)0x03) + +// UART Definitions +#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) +#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02) +#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03) +#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04) +#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05) +#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06) +#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07) + +#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) +#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) + +// CAN Definitions +#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x111) +#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x221) +#define CAN_MSG_SELECT_CONTROL_MODE_ID ((uint32_t)0x421) +#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x121) +#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x231) +#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x211) +//#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x251) +//#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x252) +//#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x253) +//#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x254) +#define CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x251) +#define CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x252) +#define CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x253) +#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) + +/*--------------------- Control/State Constants ------------------------*/ + +// Motion Control +#define CTRL_MODE_REMOTE ((uint8_t)0x00) +#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) +#define CTRL_MODE_CMD_UART ((uint8_t)0x02) +#define CTRL_MODE_COMMANDED ((uint8_t)0x03) + +#define FAULT_CLR_NONE ((uint8_t)0x00) +#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01) +#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02) +#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03) +#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04) +#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05) +#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06) +#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07) +#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08) + +// Light Control +#define LIGHT_DISABLE_CTRL ((uint8_t)0x00) +#define LIGHT_ENABLE_CTRL ((uint8_t)0x01) + +#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00) +#define LIGHT_MODE_CONST_ON ((uint8_t)0x01) +#define LIGHT_MODE_BREATH ((uint8_t)0x02) +#define LIGHT_MODE_CUSTOM ((uint8_t)0x03) + +// System Status Feedback +#define BASE_STATE_NORMAL ((uint8_t)0x00) +#define BASE_STATE_ESTOP ((uint8_t)0x01) +#define BASE_STATE_EXCEPTION ((uint8_t)0x02) + +#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100) +#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200) +#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400) +#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800) +#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000) +#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000) +#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000) +#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000) + +#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) +#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) +#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004) +#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008) +#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010) +#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020) +#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040) +#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080) + +/*-------------------- Control/Feedback Messages -----------------------*/ + +/* No padding in the struct */ +// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect +#pragma pack(push, 1) + +// Note: id could be different for UART and CAN protocol + +// Motion Control +typedef struct { + union + { + struct + { + struct + { + 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; + 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; + +// System Status Feedback +typedef struct { + union + { + struct + { + uint8_t base_state; + uint8_t control_mode; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } battery_voltage; + uint8_t fault_code; + uint8_t reserved0; + uint8_t reserved1; + uint8_t checksum; + } status; + uint8_t raw[8]; + } data; +} SystemStatusMessage; + +// Light Control +typedef struct { + union + { + 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; + +typedef struct +{ + uint8_t motor_id; + union { + struct + { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } driver_voltage; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } driver_temperature; + int8_t motor_temperature; + uint8_t driver_state; + uint8_t reserved0; + uint8_t reserved1; + } status; + uint8_t raw[8]; + } data; +} MotorDriverLowSpeedStatusMessage; + +typedef struct +{ + uint8_t motor_id; + union { + 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; + SystemStatusMessage 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; +} ScoutMessage; + +#pragma pack(pop) + +#ifdef __cplusplus +} +#endif + +#endif /* SCOUT_PROTOCOL_H */ diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp b/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp new file mode 100644 index 0000000..f39d030 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp @@ -0,0 +1,140 @@ +/* + * scout_state.hpp + * + * Created on: Jun 11, 2019 08:48 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_STATE_HPP +#define SCOUT_STATE_HPP + +#include +#include + +namespace westonrobot +{ +struct ScoutState +{ + enum MotorID + { + FRONT_RIGHT = 0, + FRONT_LEFT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 + }; + + struct MotorState + { + double current = 0; // in A + double rpm = 0; + double temperature = 0; + double motor_pose=0; + }; + struct DriverState + { + double driver_voltage = 0; + double driver_temperature = 0; + uint8_t driver_state = 0; + }; + struct MotorHeightSpeedState + { + double current = 0; // in A + double rpm = 0; + double motor_pose = 0; + }; + struct MotorLowSpeedState + { + double driver_voltage = 0; + double driver_temperature = 0; + double motor_temperature = 0; + uint8_t driver_state = 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 + static constexpr uint8_t motor_num = 4; +// MotorState motor_states[motor_num]; +// DriverState driver_states[motor_num]; + MotorHeightSpeedState motor_H_state[motor_num]; + MotorLowSpeedState motor_L_state[motor_num]; + + // 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_odomter=0; + double right_odomter=0; +}; + +struct ScoutMotionCmd +{ + 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 + }; + + ScoutMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0,int8_t angular_height_byte = 0,int8_t angular_low_byte = 0, + FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) + : linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte), + fault_clear_flag(fault_clr_flag) {} + + int8_t linear_velocity_height_byte; + int8_t linear_velocity_low_byte; + int8_t angular_velocity_height_byte; + int8_t angular_velocity_low_byte; + 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.5235; // 0.5235 rad/s + static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s +}; + +struct ScoutLightCmd +{ + enum class LightMode + { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 + }; + + ScoutLightCmd() = default; + ScoutLightCmd(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 + +#endif /* SCOUT_STATE_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h b/ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h similarity index 92% rename from ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h rename to ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h index 26fb95b..1524087 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h +++ b/ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h @@ -18,7 +18,7 @@ extern "C" { #include #include -#include "ugv_sdk/scout/scout_protocol.h" +#include "ugv_sdk/scout_v2/scout_protocol.h" bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg); void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len); diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp index d64acca..7795d3c 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp @@ -14,38 +14,28 @@ #include #include #include -#include -#include "async_io/async_can.hpp" -#include "async_io/async_serial.hpp" +//#include "wrp_sdk/asyncio/async_can.hpp" +//#include "wrp_sdk/asyncio/async_serial.hpp" +#include "ugv_sdk/mobile_base.hpp" -#include "tracer_protocol/tracer_protocol.h" -#include "tracer_protocol/tracer_can_parser.h" +#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/tracer/tracer_types.hpp" -#include "tracer_base/tracer_types.hpp" namespace westonrobot { -class TracerBase +class TracerBase :public MobileBase { public: - TracerBase() = default; - ~TracerBase(); - - // do not allow copy - TracerBase(const TracerBase &tracer) = delete; - TracerBase &operator=(const TracerBase &tracer) = delete; - -public: - // connect to roboot from CAN - void Connect(std::string dev_name); - - // disconnect from roboot, only valid for serial port - void Disconnect(); - - // cmd thread runs at 100Hz (10ms) by default - void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; }; + TracerBase() : MobileBase(){}; + ~TracerBase()= default; + // 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); @@ -54,55 +44,53 @@ public: void SetLightCommand(TracerLightCmd cmd); void DisableLightCmdControl(); - // get robot state - TracerState GetTracerState(); + private: - // hardware communication interface - std::shared_ptr can_if_; - std::shared_ptr serial_if_; // CAN priority higher than serial if both connected - bool can_connected_ = false; - bool serial_connected_ = false; +// bool can_connected_ = false; +// bool serial_connected_ = false; // serial port related variables uint8_t tx_cmd_len_; uint8_t tx_buffer_[TRACER_CMD_BUF_LEN]; // cmd/status update related variables - std::thread cmd_thread_; + //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_; 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; +// int32_t cmd_thread_period_ms_ = 10; +// bool cmd_thread_started_ = false; bool light_ctrl_enabled_ = false; bool light_ctrl_requested_ = false; - // internal functions - void ConfigureCANBus(const std::string &can_if_name = "can1"); - void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); - - void StartCmdThread(); - void ControlLoop(int32_t period_ms); - void SendMotionCmd(uint8_t count); + void SendRobotCmd() override; void SendLightCmd(uint8_t count); - - void ParseCANFrame(can_frame *rx_frame); + 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); }; } // namespace westonrobot diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h index ecc8401..608e302 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "tracer_protocol/tracer_protocol.h" +#include "ugv_sdk/tracer/tracer_protocol.h" #ifdef __linux__ #include diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h index de5e0bf..aab086a 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h @@ -23,14 +23,26 @@ extern "C" { #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)0x130) -#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131) -#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140) -#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141) -#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) -#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x201) -#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202) +#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 ------------------------*/ @@ -90,7 +102,32 @@ extern "C" { // Note: id could be different for UART and CAN protocol -// Motion Control +// 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 { @@ -107,7 +144,49 @@ typedef struct { } cmd; uint8_t raw[8]; } data; -} MotionCmdMessage; +} 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 @@ -131,10 +210,32 @@ typedef struct { } status; uint8_t raw[8]; } data; -} MotionStatusMessage; +} 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 @@ -156,7 +257,7 @@ typedef struct { } status; uint8_t raw[8]; } data; -} SystemStatusMessage; +} UartSystemStatusMessage; // Light Control typedef struct { @@ -167,11 +268,12 @@ typedef 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 reserved2; + uint8_t reserved3; uint8_t count; - uint8_t checksum; + } cmd; uint8_t raw[8]; } data; @@ -190,11 +292,47 @@ typedef struct { 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 { @@ -221,6 +359,97 @@ typedef struct } 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 { @@ -230,12 +459,30 @@ typedef enum TracerLightStatusMsg = 0x02, TracerSystemStatusMsg = 0x03, TracerMotorDriverStatusMsg = 0x04, + TracerOdometerMsg = 0x05, + TracerHeighSpeedMsg = 0x06, + TracerLowSpeedMsg = 0x07, + // control messages TracerMotionCmdMsg = 0x21, - TracerLightControlMsg = 0x22 + TracerLightControlMsg = 0x22, + TracerModeControlMsg = 0x23 } TracerMsgType; -typedef struct +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 { @@ -244,12 +491,33 @@ typedef struct 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 diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp index 964c860..7c76ff2 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp @@ -17,19 +17,11 @@ namespace westonrobot { struct TracerState { - enum MotorID - { - FRONT_RIGHT = 0, - FRONT_LEFT = 1, - REAR_LEFT = 2, - REAR_RIGHT = 3 - }; - struct MotorState { - double current = 0; // in A + //double current = 0; // in A double rpm = 0; - double temperature = 0; + //double temperature = 0; }; struct LightState @@ -41,20 +33,65 @@ struct TracerState // 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 - MotorState motor_states[4]; + MotorState motor_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_odomter = 0; + double right_odomter = 0; +}; +struct UartTracerState +{ + enum MotorID + { + FRONT_RIGHT = 0, + FRONT_LEFT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 + }; + + struct MotorState + { + double current = 9; // in A + double rpm = 0; + double temperature = 9; + }; + + 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; + double battery_voltage = 0.0; + + // motor state + static constexpr uint8_t motor_num = 2; + MotorState motor_states[motor_num]; + + // 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; }; struct TracerMotionCmd @@ -72,14 +109,46 @@ struct TracerMotionCmd MOTOR_OVERCURRENT = 0x08 }; - TracerMotionCmd(int8_t linear = 0, int8_t angular = 0, + 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) {} + + 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 +}; + +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 + }; + + 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) {} int8_t linear_velocity; int8_t angular_velocity; - FaultClearFlag fault_clear_flag; + 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 diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h b/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h new file mode 100644 index 0000000..b612aa3 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h @@ -0,0 +1,24 @@ + +#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/bunker_base.cpp b/ugv_sdk/src/bunker_base.cpp new file mode 100644 index 0000000..349f00f --- /dev/null +++ b/ugv_sdk/src/bunker_base.cpp @@ -0,0 +1,200 @@ +#include "ugv_sdk/bunker/bunker_base.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stopwatch.hpp" + +namespace westonrobot { +void BunkerBase::SendRobotCmd() { + static uint8_t cmd_count = 0; + static uint8_t light_cmd_count = 0; + SendMotionCmd(cmd_count++); +} + +void BunkerBase::SendMotionCmd(uint8_t count) { + // motion control message + BunkerMessage m_msg; + m_msg.type = BunkerMotionControlMsg; + + if (can_connected_) + m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; + else if (serial_connected_) + m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; + + motion_cmd_mutex_.lock(); + m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = + static_cast(current_motion_cmd_.fault_clear_flag); + m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = + current_motion_cmd_.linear_velocity; + m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = + 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; + + if (can_connected_) + m_msg.body.motion_control_msg.data.cmd.checksum = + CalcBunkerCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, + m_msg.body.motion_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 m_frame; + EncodeBunkerMsgToCAN(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); + } else { + // send to serial port +// EncodeBunkerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); +// serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); + } +} + + + +BunkerState BunkerBase::GetBunkerState() { + std::lock_guard guard(bunker_state_mutex_); + return bunker_state_; +} + +void BunkerBase::SetMotionCommand( + double linear_vel, double angular_vel, + BunkerMotionCmd::FaultClearFlag fault_clr_flag) { + // make sure cmd thread is started before attempting to send commands + if (!cmd_thread_started_) StartCmdThread(); + + if (linear_vel < BunkerMotionCmd::min_linear_velocity) + linear_vel = BunkerMotionCmd::min_linear_velocity; + if (linear_vel > BunkerMotionCmd::max_linear_velocity) + linear_vel = BunkerMotionCmd::max_linear_velocity; + if (angular_vel < BunkerMotionCmd::min_angular_velocity) + angular_vel = BunkerMotionCmd::min_angular_velocity; + if (angular_vel > BunkerMotionCmd::max_angular_velocity) + angular_vel = BunkerMotionCmd::max_angular_velocity; + //std::cout< guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = static_cast( + linear_vel / BunkerMotionCmd::max_linear_velocity * 100.0); + current_motion_cmd_.angular_velocity = static_cast( + angular_vel / BunkerMotionCmd::max_angular_velocity * 100.0); + current_motion_cmd_.fault_clear_flag = fault_clr_flag; + FeedCmdTimeoutWatchdog(); +} + + + +void BunkerBase::ParseCANFrame(can_frame *rx_frame) { + // validate checksum, discard frame if fails + if (!rx_frame->data[7] == CalcBunkerCANChecksum(rx_frame->can_id, + rx_frame->data, + rx_frame->can_dlc)) { + std::cerr << "ERROR: checksum mismatch, discard frame with id " + << rx_frame->can_id << std::endl; + return; + } + + // otherwise, update robot state with new frame + BunkerMessage status_msg; + DecodeBunkerMsgFromCAN(rx_frame, &status_msg); + NewStatusMsgReceivedCallback(status_msg); +} + +//void BunkerBase::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); +// BunkerMessage status_msg; +// for (int i = 0; i < bytes_received; ++i) { +// if (DecodeBunkerMsgFromUART(buf[i], &status_msg)) +// NewStatusMsgReceivedCallback(status_msg); +// } +//} + +void BunkerBase::NewStatusMsgReceivedCallback(const BunkerMessage &msg) { + // std::cout << "new status msg received" << std::endl; + std::lock_guard guard(bunker_state_mutex_); + UpdateBunkerState(msg, bunker_state_); +} + +void BunkerBase::UpdateBunkerState(const BunkerMessage &status_msg, + BunkerState &state) { + switch (status_msg.type) { + case BunkerMotionStatusMsg: { + // 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 BunkerLightStatusMsg: { + // 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 BunkerSystemStatusMsg: { + // 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 = + (static_cast(msg.data.status.fault_code.low_byte) | + static_cast(msg.data.status.fault_code.high_byte) << 8); + break; + } + case BunkerMotorDriverStatusMsg: { + // std::cout << "motor 1 driver feedback received" << std::endl; + const MotorDriverStatusMessage &msg = + status_msg.body.motor_driver_status_msg; + for (int i = 0; i < BunkerState::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) / + 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; + } + break; + } + } +} +} // namespace westonrobot diff --git a/ugv_sdk/src/bunker_can_parser.c b/ugv_sdk/src/bunker_can_parser.c new file mode 100644 index 0000000..6d83caf --- /dev/null +++ b/ugv_sdk/src/bunker_can_parser.c @@ -0,0 +1,161 @@ +/* + * bunker_can_parser.c + * + * Created on: Aug 31, 2019 04:25 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/bunker/bunker_can_parser.h" + +#include "string.h" + +static void EncodeBunkerMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); +static void EncodeBunkerLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame); + +bool DecodeBunkerMsgFromCAN(const struct can_frame *rx_frame, BunkerMessage *msg) +{ + msg->type = BunkerMsgNone; + + switch (rx_frame->can_id) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case CAN_MSG_MOTION_CONTROL_STATUS_ID: + { + msg->type = BunkerMotionStatusMsg; + // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; + memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_LIGHT_CONTROL_STATUS_ID: + { + msg->type = BunkerLightStatusMsg; + // msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_SYSTEM_STATUS_STATUS_ID: + { + msg->type = BunkerSystemStatusMsg; + // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; + memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR1_DRIVER_STATUS_ID: + { + msg->type = BunkerMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = BUNKER_MOTOR1_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR2_DRIVER_STATUS_ID: + { + msg->type = BunkerMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = BUNKER_MOTOR2_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTION_CONTROL_CMD_ID: + { + msg->type = BunkerMotionControlMsg; + // msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID; + memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_LIGHT_CONTROL_CMD_ID: + { + msg->type = BunkerLightControlMsg; + // msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + default: + break; + } + + return true; +} + +void EncodeBunkerMsgToCAN(const BunkerMessage *msg, struct can_frame *tx_frame) +{ + switch (msg->type) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case BunkerMotionStatusMsg: + { + tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case BunkerLightStatusMsg: + { + tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case BunkerSystemStatusMsg: + { + 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 BunkerMotorDriverStatusMsg: + { + if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR1_ID) + tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR2_ID) + tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; +// else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR3_ID) +// tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; +// else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_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 BunkerMotionControlMsg: + { + EncodeBunkerMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); + break; + } + case BunkerLightControlMsg: + { + EncodeBunkerLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); + break; + } + default: + break; + } + tx_frame->data[7] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +void EncodeBunkerMotionControlMsgToCAN(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] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +void EncodeBunkerLightControlMsgToCAN(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] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +uint8_t CalcBunkerCANChecksum(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/hunter_base.cpp b/ugv_sdk/src/hunter_base.cpp index 8e8301f..eee90fe 100644 --- a/ugv_sdk/src/hunter_base.cpp +++ b/ugv_sdk/src/hunter_base.cpp @@ -1,4 +1,4 @@ -#include "ugv_sdk/hunter/hunter_base.hpp" +#include "ugv_sdk/hunter_v2/hunter_base.hpp" #include #include @@ -16,35 +16,35 @@ namespace westonrobot { void HunterBase::SendRobotCmd() { static uint8_t cmd_count = 0; + SendModeCtl(); + SetParkMode(); SendMotionCmd(cmd_count++); } void HunterBase::SendMotionCmd(uint8_t count) { // motion control message HunterMessage m_msg; - m_msg.type = HunterMotionCmdMsg; - - if (can_connected_) - m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; + m_msg.type = HunterMotionControlMsg; + /*if (can_connected_) + m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; else if (serial_connected_) - m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; - + m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/; motion_cmd_mutex_.lock(); - m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = - static_cast(current_motion_cmd_.fault_clear_flag); - m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = - current_motion_cmd_.linear_velocity; - m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = - current_motion_cmd_.angular_velocity; + m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.high_byte = + current_motion_cmd_.linear_velocity_height_byte; + m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.low_byte = + current_motion_cmd_.linear_velocity_low_byte; + m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.high_byte = + current_motion_cmd_.angular_velocity_height_byte; + m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.low_byte = + current_motion_cmd_.angular_velocity_low_byte; 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.count = count; + 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.reserved2 = 0; + m_msg.body.motion_control_msg.data.cmd.reserved3 = 0; - if (can_connected_) - m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcHunterCANChecksum( - CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8); // send to can bus if (can_connected_) { @@ -60,7 +60,7 @@ HunterState HunterBase::GetHunterState() { } void HunterBase::SetMotionCommand( - double linear_vel, double steering_angle, + double linear_vel, double angular_vel,double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag) { // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); @@ -75,26 +75,68 @@ void HunterBase::SetMotionCommand( steering_angle = HunterMotionCmd::max_steering_angle; std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = static_cast( - linear_vel / HunterMotionCmd::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast( - steering_angle / HunterMotionCmd::max_steering_angle * 100.0); + current_motion_cmd_.linear_velocity_height_byte = static_cast(linear_vel*1000)>>8; + current_motion_cmd_.linear_velocity_low_byte = static_cast(linear_vel*1000)&0xff; + current_motion_cmd_.angular_velocity_height_byte = static_cast(angular_vel*1000)>>8; + current_motion_cmd_.angular_velocity_low_byte = static_cast(angular_vel*1000)&0xff; current_motion_cmd_.fault_clear_flag = fault_clr_flag; FeedCmdTimeoutWatchdog(); } -void HunterBase::ParseCANFrame(can_frame *rx_frame) { - // validate checksum, discard frame if fails - if (!rx_frame->data[7] == CalcHunterCANChecksum(rx_frame->can_id, - rx_frame->data, - rx_frame->can_dlc)) { - std::cerr << "ERROR: checksum mismatch, discard frame with id " - << rx_frame->can_id << std::endl; - return; - } +void HunterBase::SendModeCtl(){ + HunterMessage m_msg; + m_msg.type = HunterControlModeMsg; + mode_cmd_mutex_.lock(); + m_msg.body.mode_cmd_msg.data.cmd.mode_control=0x01; + mode_cmd_mutex_.unlock(); + m_msg.body.mode_cmd_msg.data.cmd.reserved0=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved1=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved2=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved3=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved4=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved5=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved6=0; + if (can_connected_) { + // send to can bus + can_frame m_frame; + EncodeHunterMsgToCAN(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); + }else {} +} - // otherwise, update robot state with new frame +void HunterBase::SetParkMode(){ + HunterMessage m_msg; + m_msg.type = HunterParkControlMsg; + bool flag = current_motion_cmd_.linear_velocity_height_byte || + current_motion_cmd_.linear_velocity_low_byte; + if(flag) + { + pack_mode_cmd_mutex_.lock(); + m_msg.body.park_control_msg.data.cmd.packing_mode=0x00; + pack_mode_cmd_mutex_.unlock(); + }else { + pack_mode_cmd_mutex_.lock(); + m_msg.body.park_control_msg.data.cmd.packing_mode=0x01; + pack_mode_cmd_mutex_.unlock(); + } + m_msg.body.park_control_msg.data.cmd.reserved0=0; + m_msg.body.park_control_msg.data.cmd.reserved1=0; + m_msg.body.park_control_msg.data.cmd.reserved2=0; + m_msg.body.park_control_msg.data.cmd.reserved3=0; + m_msg.body.park_control_msg.data.cmd.reserved4=0; + m_msg.body.park_control_msg.data.cmd.reserved5=0; + m_msg.body.park_control_msg.data.cmd.reserved6=0; + if (can_connected_) { + // send to can bus + can_frame m_frame; + EncodeHunterMsgToCAN(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); + }else {} +} + +void HunterBase::ParseCANFrame(can_frame *rx_frame) { + // update robot state with new frame HunterMessage status_msg; DecodeHunterMsgFromCAN(rx_frame, &status_msg); NewStatusMsgReceivedCallback(status_msg); @@ -130,6 +172,7 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, // 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.park_mode = msg.data.status.park_mode; state.base_state = msg.data.status.base_state; state.battery_voltage = (static_cast(msg.data.status.battery_voltage.low_byte) | @@ -141,29 +184,46 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, static_cast(msg.data.status.fault_code.high_byte) << 8); break; } - case HunterMotorDriverStatusMsg: { - // std::cout << "motor 1 driver feedback received" << std::endl; - const MotorDriverStatusMessage &msg = - status_msg.body.motor_driver_status_msg; + case HunterMotorDriverHeightSpeedStatusMsg: { + // std::cout << "motor driver height speed feedback received" << std::endl; + const MotorDriverHeightSpeedStatusMessage &msg = + status_msg.body.motor_driver_height_speed_status_msg; for (int i = 0; i < HunterState::motor_num; ++i) { - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .current = - (static_cast(msg.data.status.current.low_byte) | + state.motor_H_state[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_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.motor_H_state[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_H_state[msg.motor_id].motor_pose + = static_cast( + static_cast(msg.data.status.moter_pose.lowest) | + static_cast(msg.data.status.moter_pose.sec_lowest) << 8 | + static_cast(msg.data.status.moter_pose.sec_heighest)<< 16| + static_cast(msg.data.status.moter_pose.heighest)<< 24 ); } break; } - case HunterConfigStatusMsg: { - const ConfigStatusMessage &msg = status_msg.body.config_status_msg; - state.set_zero_steering = msg.data.status.set_zero_steering; - break; + case HunterMotorDriverLowSpeedStatusMsg: { + // std::cout << "motor driver low speed feedback received" << std::endl; + const MotorDriverLowSpeedStatusMessage &msg = + status_msg.body.motor_driver_low_speed_status_msg; + for (int i = 0; i < HunterState::motor_num; ++i) { + state.motor_L_state[msg.motor_id].driver_voltage + =(static_cast(msg.data.status.driver_voltage.low_byte) | + static_cast(msg.data.status.driver_voltage.high_byte) << 8) / + 10.0; + state.motor_L_state[msg.motor_id].driver_temperature + = static_cast( + static_cast(msg.data.status.driver_temperature.low_byte) | + static_cast(msg.data.status.driver_temperature.high_byte) << 8); + state.motor_L_state[msg.motor_id].motor_temperature + = msg.data.status.motor_temperature; + state.motor_L_state[msg.motor_id].driver_state + = msg.data.status.driver_status; + } } default: break; diff --git a/ugv_sdk/src/hunter_can_parser.c b/ugv_sdk/src/hunter_can_parser.c index 0c3fe56..cc77f9f 100644 --- a/ugv_sdk/src/hunter_can_parser.c +++ b/ugv_sdk/src/hunter_can_parser.c @@ -7,11 +7,12 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/hunter/hunter_can_parser.h" - +#include "ugv_sdk/hunter_v2/hunter_can_parser.h" #include "string.h" -static void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame); +static void EncodeHunterMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); +static void EncodeHunterControlModeMsgToCAN(const ModSelectMessage *msg,struct can_frame *tx_frame); +static void EncodeHunterControlParkMsgToCAN(const ParkControlMessage *msg,struct can_frame *tx_frame); bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg) { @@ -20,7 +21,7 @@ bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg switch (rx_frame->can_id) { // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_STATUS_ID: + case CAN_MSG_MOTION_CONTROL_STATUS_ID: { msg->type = HunterMotionStatusMsg; memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); @@ -32,46 +33,66 @@ bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); break; } - case CAN_MSG_CONFIG_STATUS_ID: + case CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID: { - msg->type = HunterSystemStatusMsg; - memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverHeightSpeedStatusMsg; + msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR1_DRIVER_STATUS_ID: + case CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID: { - msg->type = HunterMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR1_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverHeightSpeedStatusMsg; + msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR2_DRIVER_STATUS_ID: + case CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID: { - msg->type = HunterMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR2_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverHeightSpeedStatusMsg; + msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR3_DRIVER_STATUS_ID: + case CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID: { - msg->type = HunterMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR3_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverLowSpeedStatusMsg; + msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_MOTOR1_ID; + memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); break; } - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_CMD_ID: + case CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID: { - msg->type = HunterMotionCmdMsg; - memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverLowSpeedStatusMsg; + msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_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_CONFIG_CMD_ID: + case CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID: { - msg->type = HunterConfigCmdMsg; - memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + msg->type = HunterMotorDriverLowSpeedStatusMsg; + msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_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_MOTION_CONTROL_CMD_ID: + { + msg->type = HunterMotionControlMsg; + memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_PARK_CONTROL_ID: + { + msg->type = HunterParkControlMsg; + memcpy(msg->body.park_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_SELECT_CONTROL_MODE_ID: + { + msg->type = HunterControlModeMsg; + memcpy(msg->body.mode_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } default: break; } @@ -86,7 +107,7 @@ void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame) // in the current implementation, both MsgType and can_frame include 8 * uint8_t case HunterMotionStatusMsg: { - tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID; + tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); break; @@ -98,37 +119,44 @@ void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame) memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc); break; } - case HunterMotorDriverStatusMsg: + case HunterMotionControlMsg: { - if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR1_ID) - tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR2_ID) - tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR3_ID) - tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; - // else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_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); + EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); break; } - case HunterMotionCmdMsg: + case HunterControlModeMsg: { - EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame); + EncodeHunterControlModeMsgToCAN(&(msg->body.mode_cmd_msg), tx_frame); + break; + } + case HunterParkControlMsg: + { + EncodeHunterControlParkMsgToCAN(&(msg->body.park_control_msg), tx_frame); + break; } default: break; } - tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); } -void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame) +void EncodeHunterMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame) { - tx_frame->can_id = CAN_MSG_MOTION_CMD_ID; + 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); +} +void EncodeHunterControlModeMsgToCAN(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); +} +void EncodeHunterControlParkMsgToCAN(const ParkControlMessage *msg,struct can_frame *tx_frame) +{ + tx_frame->can_id = CAN_MSG_PARK_CONTROL_ID; tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); } uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) @@ -138,4 +166,4 @@ uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; return checksum; -} \ No newline at end of file +} diff --git a/ugv_sdk/src/scout_base.cpp b/ugv_sdk/src/scout_base.cpp index bebc5ee..05b70ca 100644 --- a/ugv_sdk/src/scout_base.cpp +++ b/ugv_sdk/src/scout_base.cpp @@ -1,5 +1,5 @@ -#include "ugv_sdk/scout/scout_base.hpp" - +#include "ugv_sdk/scout_v2/scout_base.hpp" +#include #include #include #include @@ -16,6 +16,7 @@ namespace westonrobot { void ScoutBase::SendRobotCmd() { static uint8_t cmd_count = 0; static uint8_t light_cmd_count = 0; + SendModeCtl(); SendMotionCmd(cmd_count++); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); } @@ -25,49 +26,68 @@ void ScoutBase::SendMotionCmd(uint8_t count) { ScoutMessage m_msg; m_msg.type = ScoutMotionControlMsg; - if (can_connected_) - m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; - else if (serial_connected_) - m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; +// if (can_connected_) +// m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; +// else if (serial_connected_) +// m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; motion_cmd_mutex_.lock(); - m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = - static_cast(current_motion_cmd_.fault_clear_flag); - m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = - current_motion_cmd_.linear_velocity; - m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = - current_motion_cmd_.angular_velocity; + m_msg.body.motion_control_msg.data.cmd.linear_velocity.high_byte = + current_motion_cmd_.linear_velocity_height_byte; + m_msg.body.motion_control_msg.data.cmd.linear_velocity.low_byte = + current_motion_cmd_.linear_velocity_low_byte; + m_msg.body.motion_control_msg.data.cmd.angular_velocity.high_byte = + current_motion_cmd_.angular_velocity_height_byte; + m_msg.body.motion_control_msg.data.cmd.angular_velocity.low_byte = + current_motion_cmd_.angular_velocity_low_byte; 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; - - if (can_connected_) - m_msg.body.motion_control_msg.data.cmd.checksum = - CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, - m_msg.body.motion_control_msg.data.raw, 8); - // serial_connected_: checksum will be calculated later when packed into a - // complete serial frame + m_msg.body.motion_control_msg.data.cmd.reserved2 = 0; + m_msg.body.motion_control_msg.data.cmd.reserved3 = 0; if (can_connected_) { // send to can bus can_frame m_frame; EncodeScoutMsgToCAN(&m_msg, &m_frame); can_if_->SendFrame(m_frame); - // std::cout << "CAN cmd sent" << std::endl; } else { // send to serial port EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); - // std::cout << "serial cmd sent" << std::endl; } } +void ScoutBase::SendModeCtl(){ + ScoutMessage m_msg; + m_msg.type = ScoutControlModeMsg; + mode_cmd_mutex_.lock(); + m_msg.body.mode_cmd_msg.data.cmd.control_mode=0x01; + mode_cmd_mutex_.unlock(); + m_msg.body.mode_cmd_msg.data.cmd.reserved0=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved1=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved2=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved3=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved4=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved5=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved6=0; + if (can_connected_) { + // send to can bus + can_frame m_frame; + EncodeScoutMsgToCAN(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); + } else { + // send to serial port + EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); + serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); + } +} void ScoutBase::SendLightCmd(uint8_t count) { ScoutMessage l_msg; l_msg.type = ScoutLightControlMsg; + light_cmd_mutex_.lock(); if (light_ctrl_enabled_) { l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; @@ -103,9 +123,9 @@ void ScoutBase::SendLightCmd(uint8_t count) { 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 = CalcScoutCANChecksum( - CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8); +// if (can_connected_) +// l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum( +// 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 @@ -155,13 +175,11 @@ void ScoutBase::SetMotionCommand( angular_vel = ScoutMotionCmd::max_angular_velocity; std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = static_cast( - linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast( - angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); + current_motion_cmd_.linear_velocity_height_byte = static_cast(linear_vel*1000)>>8; + current_motion_cmd_.linear_velocity_low_byte = static_cast(linear_vel*1000)&0xff; + current_motion_cmd_.angular_velocity_height_byte = static_cast(angular_vel*1000)>>8; + current_motion_cmd_.angular_velocity_low_byte = static_cast(angular_vel*1000)&0xff; current_motion_cmd_.fault_clear_flag = fault_clr_flag; - - FeedCmdTimeoutWatchdog(); } void ScoutBase::SetLightCommand(ScoutLightCmd cmd) { @@ -181,17 +199,21 @@ void ScoutBase::DisableLightCmdControl() { void ScoutBase::ParseCANFrame(can_frame *rx_frame) { // validate checksum, discard frame if fails - if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, - rx_frame->data, - rx_frame->can_dlc)) { - std::cerr << "ERROR: checksum mismatch, discard frame with id " - << rx_frame->can_id << std::endl; - return; - } +// if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, +// rx_frame->data, +// rx_frame->can_dlc)) { +// std::cerr << "ERROR: checksum mismatch, discard frame with id " +// << rx_frame->can_id << std::endl; +// return; +// } // otherwise, update robot state with new frame ScoutMessage status_msg; DecodeScoutMsgFromCAN(rx_frame, &status_msg); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.heighest); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_heighest); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_lowest); +// printf("%x\r\n",status_msg.body.odom_msg.data.status.left.lowest); NewStatusMsgReceivedCallback(status_msg); } @@ -219,18 +241,8 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, case ScoutMotionStatusMsg: { // 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; + state.linear_velocity =static_cast(static_cast(msg.data.cmd.linear_velocity.low_byte) |static_cast(msg.data.cmd.linear_velocity.high_byte)<< 8)/1000.0; + state.angular_velocity =static_cast(static_cast(msg.data.cmd.angular_velocity.low_byte) |static_cast(msg.data.cmd.angular_velocity.high_byte)<< 8)/1000.0; break; } case ScoutLightStatusMsg: { @@ -256,30 +268,79 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, static_cast(msg.data.status.battery_voltage.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.fault_code =msg.data.status.fault_code; break; } - case ScoutMotorDriverStatusMsg: { +// case ScoutMotorDriverStatusMsg: { +// // std::cout << "motor 1 driver feedback received" << std::endl; +// const MotorDriverStatusMessage &msg = +// status_msg.body.motor_driver_status_msg; +// for (int i = 0; i < ScoutState::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) / +// 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; +// } +// break; +// } + case ScoutMotorDriverHeightSpeedStatusMsg: { // std::cout << "motor 1 driver feedback received" << std::endl; - const MotorDriverStatusMessage &msg = - status_msg.body.motor_driver_status_msg; + const MotorDriverHeightSpeedStatusMessage &msg =status_msg.body.motor_driver_height_speed_status_msg; for (int i = 0; i < ScoutState::motor_num; ++i) { - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] + state.motor_H_state[status_msg.body.motor_driver_height_speed_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_driver_status_msg.motor_id] + state.motor_H_state[status_msg.body.motor_driver_height_speed_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; + static_cast(msg.data.status.rpm.low_byte) | + static_cast(msg.data.status.rpm.high_byte) << 8); + state.motor_H_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] + .motor_pose = static_cast(static_cast(msg.data.status.moter_pose.lowest)|static_cast(msg.data.status.moter_pose.sec_lowest + )<<8|static_cast(msg.data.status.moter_pose.sec_heighest)<<16|static_cast(msg.data.status.moter_pose.heighest)<<24); } break; } + case ScoutMotorDriverLowSpeedStatusMsg: { + // std::cout << "motor 1 driver feedback received" << std::endl; + const MotorDriverLowSpeedStatusMessage &msg =status_msg.body.motor_driver_low_speed_status_msg; + for (int i = 0; i < ScoutState::motor_num; ++i) { + state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + .driver_voltage = + (static_cast(msg.data.status.driver_voltage.low_byte) | + static_cast(msg.data.status.driver_voltage.high_byte) << 8) / + 10.0; + state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + .driver_temperature = static_cast( + static_cast(msg.data.status.driver_temperature.low_byte) | + static_cast(msg.data.status.driver_temperature.high_byte) << 8); + state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + .motor_temperature = msg.data.status.motor_temperature; + state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + .driver_state = msg.data.status.driver_state; + } + break; + } + case ScoutodometerMsg:{ + const OdomterMessage &msg=status_msg.body.odom_msg; + + + state.left_odomter=static_cast(static_cast(msg.data.status.left.lowest)|static_cast(msg.data.status.left.sec_lowest)<<8|(static_cast(msg.data.status.left.sec_heighest))<<16|static_cast(msg.data.status.left.heighest)<<24); + state.right_odomter=static_cast((static_cast(msg.data.status.right.lowest))|(static_cast(msg.data.status.right.sec_lowest)<<8)|(static_cast(msg.data.status.right.sec_heighest)<<16)|(static_cast(msg.data.status.right.heighest)<<24)); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.heighest); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_heighest); +// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_lowest); +// printf("%x\r\n",status_msg.body.odom_msg.data.status.left.lowest); + break; + } } } } // namespace westonrobot diff --git a/ugv_sdk/src/scout_can_parser.c b/ugv_sdk/src/scout_can_parser.c index 5b20e46..06bab5f 100644 --- a/ugv_sdk/src/scout_can_parser.c +++ b/ugv_sdk/src/scout_can_parser.c @@ -7,12 +7,14 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/scout/scout_can_parser.h" +#include "ugv_sdk/scout_v2/scout_can_parser.h" #include "string.h" static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame); +static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame); + bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg) { @@ -42,38 +44,6 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg) memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); break; } - case CAN_MSG_MOTOR1_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR2_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR3_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_MOTOR4_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; - memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } // in the current implementation, both MsgType and can_frame include 8 * uint8_t case CAN_MSG_MOTION_CONTROL_CMD_ID: { @@ -89,6 +59,75 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg) memcpy(msg->body.light_control_msg.data.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, 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: break; } @@ -122,25 +161,30 @@ void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame) memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc); break; } - case ScoutMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) - tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) - tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) - tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) - tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc); - break; - } +// case ScoutMotorDriverStatusMsg: +// { +// if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) +// tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; +// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) +// tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; +// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) +// tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; +// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) +// tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; +// tx_frame->can_dlc = 8; +// memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc); +// break; +// } case ScoutMotionControlMsg: { EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); break; } + case ScoutControlModeMsg: + { + EncodeScoutnControlModeMsgToCAN(&(msg->body.motion_control_msg), tx_frame); + break; + } case ScoutLightControlMsg: { EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); @@ -149,7 +193,7 @@ void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame) default: break; } - tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +// tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); } void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame) @@ -159,7 +203,13 @@ void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct ca memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); } - +void 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] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame) { tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID; @@ -175,4 +225,4 @@ uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; return checksum; -} \ No newline at end of file +} diff --git a/ugv_sdk/src/scout_uart_parser.c b/ugv_sdk/src/scout_uart_parser.c index 636ec1b..cb015b7 100644 --- a/ugv_sdk/src/scout_uart_parser.c +++ b/ugv_sdk/src/scout_uart_parser.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/scout/scout_uart_parser.h" +#include "ugv_sdk/scout_v2/scout_uart_parser.h" // #define USE_XOR_CHECKSUM @@ -96,13 +96,13 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len) 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[5] = msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte; + buf[6] = msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte; + buf[7] = msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte; + buf[8] = msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte; buf[9] = 0; buf[10] = 0; - buf[11] = msg->body.motion_status_msg.data.status.count; + buf[11] = 0; break; } case ScoutLightStatusMsg: @@ -124,9 +124,9 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len) 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; + buf[9] = msg->body.system_status_msg.data.status.fault_code; + buf[10] = 0; + buf[11] = 0; break; } case ScoutMotorDriverStatusMsg: @@ -190,16 +190,16 @@ void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, 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[5] = msg->data.cmd.linear_velocity.high_byte; + buf[6] = msg->data.cmd.linear_velocity.low_byte; + buf[7] = msg->data.cmd.angular_velocity.high_byte; + buf[8] = msg->data.cmd.angular_velocity.low_byte; buf[9] = 0x00; buf[10] = 0x00; // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); + buf[11] = 0x00; + buf[12] = 0x00; // length: SOF + Frame + Checksum *len = buf[2] + FRAME_SOF_LEN + 1; @@ -453,14 +453,14 @@ bool ConstructControlMessage(ScoutMessage *msg) 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.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motion_control_msg.data.cmd.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motion_control_msg.data.cmd.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motion_control_msg.data.cmd.angular_velocity.low_byte = 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; + msg->body.motion_control_msg.data.cmd.reserved2 = uart_parsing_data.payload_buffer[6]; + msg->body.motion_control_msg.data.cmd.reserved3 = uart_parsing_data.payload_buffer[7]; break; } case UART_FRAME_LIGHT_CONTROL_ID: @@ -473,7 +473,7 @@ bool ConstructControlMessage(ScoutMessage *msg) 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; + // msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum; break; } } @@ -494,23 +494,23 @@ bool ConstructStatusMessage(ScoutMessage *msg) 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; + msg->body.system_status_msg.data.status.fault_code = uart_parsing_data.payload_buffer[4]; + msg->body.system_status_msg.data.status.reserved0 = uart_parsing_data.payload_buffer[5]; + msg->body.system_status_msg.data.status.reserved1 = 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; + msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0]; + msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1]; + msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2]; + msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3]; + msg->body.motion_status_msg.data.cmd.reserved0 = 0x00; + msg->body.motion_status_msg.data.cmd.reserved1 = 0x00; + msg->body.motion_status_msg.data.cmd.reserved2 = 0x00; + msg->body.motion_status_msg.data.cmd.reserved3 = 0x00; break; } case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: @@ -579,7 +579,7 @@ bool ConstructStatusMessage(ScoutMessage *msg) 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; + msg->body.light_status_msg.data.status.reserved1 = 0x00; break; } } diff --git a/ugv_sdk/src/tracer_base.cpp b/ugv_sdk/src/tracer_base.cpp index 1dc22d6..092c548 100644 --- a/ugv_sdk/src/tracer_base.cpp +++ b/ugv_sdk/src/tracer_base.cpp @@ -1,4 +1,4 @@ -#include "tracer_base/tracer_base.hpp" +#include "ugv_sdk/tracer/tracer_base.hpp" #include #include @@ -10,117 +10,124 @@ #include #include -#include "stopwatch/stopwatch.h" +#include "stopwatch.hpp" namespace westonrobot { -TracerBase::~TracerBase() -{ - if (serial_connected_) - serial_if_->close(); - - if (cmd_thread_.joinable()) - cmd_thread_.join(); +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::Connect(std::string dev_name) -{ - // if (baud_rate == 0) { - ConfigureCANBus(dev_name); - // } else { - // ConfigureSerial(dev_name, baud_rate); - - // if (!serial_connected_) - // std::cerr << "ERROR: Failed to connect to serial port" << std::endl; - // } -} - -void TracerBase::Disconnect() -{ - if (serial_connected_) - { - if (serial_if_->is_open()) - serial_if_->close(); - } -} - -void TracerBase::ConfigureCANBus(const std::string &can_if_name) -{ - can_if_ = std::make_shared(can_if_name); - - can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1)); - - can_connected_ = true; -} - -void TracerBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate) -{ - serial_if_ = std::make_shared(uart_name, baud_rate); - serial_if_->open(); - - if (serial_if_->is_open()) - serial_connected_ = true; - - serial_if_->set_receive_callback(std::bind(&TracerBase::ParseUARTBuffer, this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); -} - -void TracerBase::StartCmdThread() -{ - current_motion_cmd_.linear_velocity = 0; - current_motion_cmd_.angular_velocity = 0; - current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT; - - cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_)); - cmd_thread_started_ = true; -} void TracerBase::SendMotionCmd(uint8_t count) { - // motion control message - TracerMessage m_msg; - m_msg.type = TracerMotionCmdMsg; - - if (can_connected_) - m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; - else if (serial_connected_) - m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; - - motion_cmd_mutex_.lock(); - m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast(current_motion_cmd_.fault_clear_flag); - m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity; - m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity; - 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.count = count; - - 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 - - if (can_connected_) + if(can_connected_) { - // send to can bus - can_frame m_frame; - EncodeTracerMsgToCAN(&m_msg, &m_frame); - can_if_->send_frame(m_frame); + // 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 - { - // TODO - // send to serial port - // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); - // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); + 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) { - TracerMessage l_msg; + 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(); @@ -130,8 +137,8 @@ void TracerBase::SendLightCmd(uint8_t count) 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; + //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; @@ -143,8 +150,8 @@ void TracerBase::SendLightCmd(uint8_t count) 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; + //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(); @@ -153,7 +160,7 @@ void TracerBase::SendLightCmd(uint8_t count) 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); + // 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_) @@ -162,7 +169,7 @@ void TracerBase::SendLightCmd(uint8_t count) can_frame l_frame; EncodeTracerMsgToCAN(&l_msg, &l_frame); - can_if_->send_frame(l_frame); + can_if_->SendFrame(l_frame); } // else // { @@ -179,28 +186,39 @@ void TracerBase::SendLightCmd(uint8_t count) // 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 << std::endl;*/ } -void TracerBase::ControlLoop(int32_t period_ms) +void TracerBase::SendControlCmd() { - StopWatch ctrl_sw; - uint8_t cmd_count = 0; - uint8_t light_cmd_count = 0; - while (true) - { - ctrl_sw.tic(); + TracerMessage c_msg; + c_msg.type = TracerModeControlMsg; - // motion control message - SendMotionCmd(cmd_count++); + 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_); + } - // check if there is request for light control - if (light_ctrl_requested_) - SendLightCmd(light_cmd_count++); - - ctrl_sw.sleep_until_ms(period_ms); - // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl; - } } TracerState TracerBase::GetTracerState() @@ -209,25 +227,54 @@ TracerState TracerBase::GetTracerState() return tracer_state_; } +UartTracerState TracerBase::GetUartTracerState() +{ + std::lock_guard guard(uart_tracer_state_mutex_); + return uart_tracer_state_; +} + 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 (!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; - 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_); - current_motion_cmd_.linear_velocity = static_cast(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0); - current_motion_cmd_.fault_clear_flag = fault_clr_flag; + 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:"<data[7] == CalcTracerCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc)) - { - std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl; - return; - } - // otherwise, update robot state with new frame TracerMessage status_msg; - DecodeTracerMsgFromCAN(rx_frame, &status_msg); + DecodeTracerMsgFromCAN(rx_frame, &status_msg);//assigned a value to status_msg from can include status_msg->type NewStatusMsgReceivedCallback(status_msg); } @@ -269,13 +310,12 @@ void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t byte // serial_parser_.PrintStatistics(); // serial_parser_.ParseBuffer(buf, bytes_received); - // TODO - // TracerMessage status_msg; - // for (int i = 0; i < bytes_received; ++i) - // { - // if (DecodeTracerMsgFromUART(buf[i], &status_msg)) - // NewStatusMsgReceivedCallback(status_msg); - // } + UartTracerMessage status_msg; + for (int i = 0; i < bytes_received; ++i) + { + if (DecodeTracerMsgFromUART(buf[i], &status_msg)) + UartNewStatusMsgReceivedCallback(status_msg); + } } void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg) @@ -285,6 +325,13 @@ void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg) 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) @@ -307,8 +354,8 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState 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.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: @@ -318,21 +365,101 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState 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 = (static_cast(msg.data.status.fault_code.low_byte) | static_cast(msg.data.status.fault_code.high_byte) << 8); + 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; - for (int i = 0; i < 4; ++i) + //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_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.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) +{ + switch (status_msg.type) { + case UartTracerMotionStatusMsg: { + // std::cout << "motion control feedback received" << std::endl; + const UartMotionStatusMessage &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 UartTracerLightStatusMsg: { + // 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) + 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 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) / + 10.0; + state.fault_code = + (static_cast(msg.data.status.fault_code.low_byte) | + static_cast(msg.data.status.fault_code.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) / + 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; + } + break; + } + default:break; + } +} } // namespace westonrobot diff --git a/ugv_sdk/src/tracer_can_parser.c b/ugv_sdk/src/tracer_can_parser.c index 1f9386f..89f7dfa 100644 --- a/ugv_sdk/src/tracer_can_parser.c +++ b/ugv_sdk/src/tracer_can_parser.c @@ -7,11 +7,12 @@ * Copyright (c) 2020 Ruixiang Du (rdu) */ -#include "tracer_protocol/tracer_can_parser.h" +#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) { @@ -36,14 +37,18 @@ bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg { 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 @@ -53,6 +58,12 @@ bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg 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; } @@ -94,10 +105,15 @@ void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame) 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); + //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) @@ -105,9 +121,15 @@ void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_f 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); + //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; @@ -115,4 +137,4 @@ uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; return checksum; -} \ No newline at end of file +} diff --git a/ugv_sdk/src/tracer_uart_parser.c b/ugv_sdk/src/tracer_uart_parser.c new file mode 100644 index 0000000..d497dc9 --- /dev/null +++ b/ugv_sdk/src/tracer_uart_parser.c @@ -0,0 +1,587 @@ +#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; +}