From a08f32f601e4c5a305d103f662f9ac84d8a4dff3 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 6 Nov 2020 10:39:56 +0800 Subject: [PATCH] updated scout code --- ugv_sdk/CMakeLists.txt | 12 +- .../ugv_sdk/hunter/hunter_can_parser.h | 42 -- .../include/ugv_sdk/hunter/hunter_protocol.h | 298 --------- .../include/ugv_sdk/proto/agx_msg_parser.h | 6 +- .../include/ugv_sdk/proto/agx_protocol_v2.h | 74 ++- ugv_sdk/include/ugv_sdk/scout/scout_base.hpp | 54 +- .../include/ugv_sdk/scout/scout_can_parser.h | 42 -- .../include/ugv_sdk/scout/scout_protocol.h | 393 ----------- ugv_sdk/include/ugv_sdk/scout/scout_types.hpp | 69 +- .../include/ugv_sdk/scout/scout_uart_parser.h | 32 - .../include/ugv_sdk/tracer/tracer_base.hpp | 3 - ugv_sdk/src/hunter_can_parser.c | 169 ----- ugv_sdk/src/scout_base.cpp | 405 ++++------- ugv_sdk/src/scout_can_parser.c | 228 ------- ugv_sdk/src/scout_uart_parser.c | 629 ------------------ ugv_sdk/src/tracer_base.cpp | 2 - 16 files changed, 247 insertions(+), 2211 deletions(-) delete mode 100644 ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h delete mode 100644 ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h delete mode 100644 ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h delete mode 100644 ugv_sdk/include/ugv_sdk/scout/scout_protocol.h delete mode 100644 ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h delete mode 100644 ugv_sdk/src/hunter_can_parser.c delete mode 100644 ugv_sdk/src/scout_can_parser.c delete mode 100644 ugv_sdk/src/scout_uart_parser.c diff --git a/ugv_sdk/CMakeLists.txt b/ugv_sdk/CMakeLists.txt index 7b99562..e58d4c0 100755 --- a/ugv_sdk/CMakeLists.txt +++ b/ugv_sdk/CMakeLists.txt @@ -78,21 +78,15 @@ endif() # Build libraries find_package(Threads REQUIRED) -# import asio library -# add_subdirectory(asio) - add_library(${PROJECT_NAME} + # agx common src/mobile_base.cpp src/agx_msg_parser.c - # robots + # robot support + src/scout_base.cpp # src/hunter_base.cpp - # src/hunter_can_parser.c - # src/scout_base.cpp - # src/scout_can_parser.c - # src/scout_uart_parser.c src/tracer_base.cpp # src/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/hunter/hunter_can_parser.h b/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h deleted file mode 100644 index f887be3..0000000 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * hunter_can_parser.h - * - * Created on: Jan 02, 2020 12:36 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef HUNTER_CAN_PARSER_H -#define HUNTER_CAN_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -#include "ugv_sdk/hunter/hunter_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 DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg); -void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame); - -uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); - -#ifdef __cplusplus -} -#endif - -#endif /* HUNTER_CAN_PARSER_H */ diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h b/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h deleted file mode 100644 index d8b417c..0000000 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h +++ /dev/null @@ -1,298 +0,0 @@ -/* - * hunter_protocol.h - * - * Created on: Jan 02, 2020 12:06 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef HUNTER_PROTOCOL_H -#define HUNTER_PROTOCOL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define HUNTER_CMD_BUF_LEN 32 -#define HUNTER_STATUS_BUF_LEN 32 -#define HUNTER_FRAME_SIZE 13 - -#define HUNTER_MOTOR1_ID ((uint8_t)0x00) -#define HUNTER_MOTOR2_ID ((uint8_t)0x01) -#define HUNTER_MOTOR3_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_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 ------------------------*/ - -// 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) - -// System Configuration -#define STEERING_ZERO_CONFIG_FAIL ((uint8_t)0x00) -#define STEERING_ZERO_CONFIG_SUCCESS ((uint8_t)0xaa) - -// System Status Feedback -#define BASE_STATE_NORMAL ((uint8_t)0x00) -#define BASE_STATE_ESTOP ((uint8_t)0x01) -#define BASE_STATE_EXCEPTION ((uint8_t)0x02) - -#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100) -#define FAULT_FRONT_STEER_ENCODER_F ((uint16_t)0x0200) -#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x0400) -#define FAULT_HIGH_BYTE_RESERVED1 ((uint16_t)0x0800) -#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x1000) -#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x2000) -#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x4000) -#define FAULT_HIGH_BYTE_RESERVED5 ((uint16_t)0x8000) - -#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) -#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) -#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004) -#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008) -#define FAULT_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 Status Feedback -typedef struct { - union - { - struct - { - 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; - } status; - uint8_t raw[8]; - } data; -} MotionStatusMessage; - -// System Status Feedback -typedef struct { - union - { - struct - { - uint8_t base_state; - uint8_t control_mode; - struct - { - uint8_t high_byte; - uint8_t low_byte; - } battery_voltage; - struct - { - uint8_t high_byte; - uint8_t low_byte; - } fault_code; - uint8_t park_mode; - uint8_t count; - } status; - uint8_t raw[8]; - } data; -} SystemStatusMessage; - -// Motor Driver Height Speed Status Feedback -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 - { - uint8_t heighest; - uint8_t sec_heighest; - uint8_t sec_lowest; - uint8_t lowest; - } moter_pose; - } status; - uint8_t raw[8]; - } data; -} 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 -{ - HunterMsgNone = 0x00, - // status messages - HunterMotionStatusMsg = 0x01, - HunterSystemStatusMsg = 0x02, - HunterMotorDriverHeightSpeedStatusMsg = 0x03, - HunterMotorDriverLowSpeedStatusMsg = 0x04, - // control messages - HunterMotionControlMsg = 0x21, - HunterParkControlMsg = 0x22, - HunterControlModeMsg = 0x23 -} HunterMsgType; - -typedef struct -{ - HunterMsgType type; - union { - // status messages - MotionStatusMessage motion_status_msg; - SystemStatusMessage system_status_msg; - MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg; - MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg; - // control messages - MotionControlMessage motion_control_msg; - ParkControlMessage park_control_msg; - ModSelectMessage mode_cmd_msg; - } body; -} HunterMessage; - -#pragma pack(pop) - -#ifdef __cplusplus -} -#endif - -#endif /* HUNTER_PROTOCOL_H */ diff --git a/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h b/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h index d7b68f8..b654e4e 100644 --- a/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h +++ b/ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h @@ -45,7 +45,9 @@ typedef enum { AgxMsgRcState = 0x24, AgxMsgActuatorHSState = 0x25, AgxMsgActuatorLSState = 0x26, - AgxMsgOdometry = 0x27 + AgxMsgOdometry = 0x27, + AgxMsgVersionQuery = 0x28, + AgxMsgPlatformVersion = 0x29 } MsgType; typedef struct { @@ -64,6 +66,8 @@ typedef struct { ActuatorHSStateMessage actuator_hs_state_msg; ActuatorLSStateMessage actuator_ls_state_msg; OdometryMessage odometry_msg; + VersionQueryMessage version_query_msg; + PlatformVersionMessage platform_version_msg; } body; } AgxMessage; #pragma pack(pop) diff --git a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h index f8e64d9..9cc4399 100644 --- a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h +++ b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h @@ -45,6 +45,9 @@ extern "C" { #define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) +#define CAN_MSG_VERSION_QUERY_ID ((uint32_t)0x411) +#define CAN_MSG_PLATFORM_VERSION_ID ((uint32_t)0x41a) + #define CAN_MSG_CTRL_MODE_SELECT_ID ((uint32_t)0x421) #define CAN_MSG_STATE_RESET_ID ((uint32_t)0x441) @@ -55,11 +58,17 @@ extern "C" { #define VEHICLE_STATE_ESTOP ((uint8_t)0x01) #define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02) +#define FAULT_BATTERY_LOW_ERROR ((uint8_t)0x01) +#define FAULT_BATTERY_LOW_WARN ((uint8_t)0x02) +#define FAULT_RC_SIGNAL_LOSS ((uint8_t)0x04) + #define FAULT_CLR_ALL ((uint8_t)0x00) #define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x01) #define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x02) -#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x04) -#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x08) +#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x03) +#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x04) + +#define QUERY_PLATFORM_VERSION_REQUEST ((uint8_t)0x01) // Motion Control #define CTRL_MODE_RC ((uint8_t)0x00) @@ -69,20 +78,21 @@ extern "C" { // Light Control #define LIGHT_CTRL_DISABLE ((uint8_t)0x00) #define LIGHT_CTRL_ENABLE ((uint8_t)0x01) + #define LIGHT_MODE_CONST_OFF ((uint8_t)0x00) #define LIGHT_MODE_CONST_ON ((uint8_t)0x01) #define LIGHT_MODE_BREATH ((uint8_t)0x02) #define LIGHT_MODE_CUSTOM ((uint8_t)0x03) // Actuator State -#define BATTERY_VOLTAGE_LOW ((uint8_t)0x00) -#define MOTOR_OVERHEAT ((uint8_t)0x01) -#define MOTOR_DRIVER_OVERLOAD ((uint8_t)0x02) -#define MOTOR_DRIVER_OVERHEAT ((uint8_t)0x03) -#define MOTOR_SENSOR_FAULT ((uint8_t)0x04) -#define MOTOR_DRIVER_FAULT ((uint8_t)0x05) -#define MOTOR_DRIVER_ENABLED ((uint8_t)0x06) -#define MOTOR_DRIVER_RESERVED0 ((uint8_t)0x07) +#define BATTERY_VOLTAGE_LOW ((uint8_t)0x01) +#define MOTOR_OVERHEAT ((uint8_t)0x02) +#define MOTOR_DRIVER_OVERLOAD ((uint8_t)0x04) +#define MOTOR_DRIVER_OVERHEAT ((uint8_t)0x08) +#define MOTOR_SENSOR_FAULT ((uint8_t)0x10) +#define MOTOR_DRIVER_FAULT ((uint8_t)0x20) +#define MOTOR_DRIVER_ENABLED ((uint8_t)0x40) +#define MOTOR_DRIVER_RESERVED0 ((uint8_t)0x80) /*-------------------- Control/Feedback Messages -----------------------*/ @@ -182,11 +192,13 @@ typedef union { struct { uint8_t high_byte; uint8_t low_byte; - } angular_velocity; + } angular_velocity; // only valid for differential drivering uint8_t reserved0; uint8_t reserved1; - uint8_t reserved2; - uint8_t reserved3; + struct { + uint8_t high_byte; + uint8_t low_byte; + } steering_angle; // only valid for ackermann steering } state; uint8_t raw[8]; } MotionStateMessage; @@ -281,6 +293,42 @@ typedef union { uint8_t raw[8]; } OdometryMessage; +typedef union { + struct { + uint8_t request; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; + } state; + uint8_t raw[8]; +} VersionQueryMessage; + +typedef union { + struct { + struct { + uint8_t high_byte; + uint8_t low_byte; + } controller_hw_version; + struct { + uint8_t high_byte; + uint8_t low_byte; + } motor_driver_hw_version; + struct { + uint8_t high_byte; + uint8_t low_byte; + } controller_sw_version; + struct { + uint8_t high_byte; + uint8_t low_byte; + } motor_driver_sw_version; + } state; + uint8_t raw[8]; +} PlatformVersionMessage; + #pragma pack(pop) #ifdef __cplusplus diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp index ea82e43..0d2ac4c 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp @@ -17,10 +17,7 @@ #include #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/proto/agx_msg_parser.h" #include "ugv_sdk/scout/scout_types.hpp" namespace westonrobot { @@ -29,55 +26,34 @@ class ScoutBase : public MobileBase { ScoutBase() : MobileBase(){}; ~ScoutBase() = default; - public: - // motion control - void SetMotionCommand(double linear_vel, double angular_vel, - ScoutMotionCmd::FaultClearFlag fault_clr_flag = - ScoutMotionCmd::FaultClearFlag::NO_FAULT); - - // light control - void SetLightCommand(ScoutLightCmd cmd); - void DisableLightCmdControl(); - // get robot state ScoutState GetScoutState(); - private: - // serial port buffer - uint8_t tx_cmd_len_; - uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN]; + void EnableCommandedMode(); + // motion control + void SetMotionCommand(double linear_vel, double angular_vel); + + // light control + void SetLightCommand(const ScoutLightCmd &cmd); + + private: // 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; // internal functions - void SendRobotCmd() override; - void ParseCANFrame(can_frame *rx_frame); - void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received); - void SendMotionCmd(uint8_t count); - void SendLightCmd(uint8_t count); - void SendModeCtl(); - void NewStatusMsgReceivedCallback(const ScoutMessage &msg); + void SendLightCmd(const ScoutLightCmd &cmd, uint8_t count); - public: - static void UpdateScoutState(const ScoutMessage &status_msg, - ScoutState &state); + void SendRobotCmd() override; + void ParseCANFrame(can_frame *rx_frame) override; + + void NewStatusMsgReceivedCallback(const AgxMessage &msg); + static void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state); }; } // namespace westonrobot diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h b/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h deleted file mode 100644 index 0e44f25..0000000 --- a/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * scout_can_parser.h - * - * Created on: Aug 31, 2019 04:23 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_CAN_PARSER_H -#define SCOUT_CAN_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -#include "ugv_sdk/scout/scout_protocol.h" - -#ifdef __linux__ -#include -#else -struct can_frame -{ - uint32_t can_id; - uint8_t can_dlc; - uint8_t data[8]__attribute__((aligned(8))); -}; -#endif - -bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg); -void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame); - -uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); - -#ifdef __cplusplus -} -#endif - -#endif /* SCOUT_CAN_PARSER_H */ diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h b/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h deleted file mode 100644 index 7e37444..0000000 --- a/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h +++ /dev/null @@ -1,393 +0,0 @@ -/* - * scout_protocol.h - * - * Created on: Aug 07, 2019 21:49 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_PROTOCOL_H -#define SCOUT_PROTOCOL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define SCOUT_CMD_BUF_LEN 32 -#define SCOUT_STATUS_BUF_LEN 32 -#define SCOUT_FRAME_SIZE 13 - -#define SCOUT_MOTOR1_ID ((uint8_t)0x00) -#define SCOUT_MOTOR2_ID ((uint8_t)0x01) -#define SCOUT_MOTOR3_ID ((uint8_t)0x02) -#define SCOUT_MOTOR4_ID ((uint8_t)0x03) - -// UART Definitions -#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) -#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02) -#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03) -#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04) -#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05) -#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06) -#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07) - -#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) -#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) - -// CAN Definitions -#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)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/scout_types.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp index 04bc481..d9b6b29 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp @@ -22,28 +22,17 @@ struct ScoutState { 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 MotorHighSpeedState { - double current = 0; // in A - double rpm = 0; - double motor_pose = 0; - }; - struct MotorLowSpeedState { - double driver_voltage = 0; - double driver_temperature = 0; + struct ActuatorState { + double motor_current = 0; // in A + uint16_t motor_rpm = 0; + uint16_t motor_pulses = 0; double motor_temperature = 0; + + double driver_voltage = 0; + double driver_temperature = 0; uint8_t driver_state = 0; }; + struct LightState { uint8_t mode = 0; uint8_t custom_value = 0; @@ -57,10 +46,7 @@ struct ScoutState { // motor state static constexpr uint8_t motor_num = 4; - // MotorState motor_states[motor_num]; - // DriverState driver_states[motor_num]; - MotorHighSpeedState motor_hs_state[motor_num]; - MotorLowSpeedState motor_ls_state[motor_num]; + ActuatorState actuator_states[motor_num]; // light state bool light_control_enabled = false; @@ -72,37 +58,16 @@ struct ScoutState { double angular_velocity = 0; // odometer state - double left_odomter = 0; - double right_odomter = 0; + double left_odometry = 0; + double right_odometry = 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(double linear = 0.0, double angular = 0.0) + : linear_velocity(linear), angular_velocity(angular) {} - 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; + double linear_velocity; + double angular_velocity; static constexpr double max_linear_velocity = 1.5; // 1.5 m/s static constexpr double min_linear_velocity = -1.5; // -1.5 m/s @@ -121,11 +86,13 @@ struct ScoutLightCmd { ScoutLightCmd() = default; ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) - : front_mode(f_mode), + : enable_ctrl(true), + front_mode(f_mode), front_custom_value(f_value), rear_mode(r_mode), rear_custom_value(r_value) {} + bool enable_ctrl = false; LightMode front_mode; uint8_t front_custom_value; LightMode rear_mode; diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h b/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h deleted file mode 100644 index 26fb95b..0000000 --- a/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * scout_uart_parser.h - * - * Created on: Aug 14, 2019 12:01 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_UART_PARSER_H -#define SCOUT_UART_PARSER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -#include "ugv_sdk/scout/scout_protocol.h" - -bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg); -void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len); - -uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len); - -#ifdef __cplusplus -} -#endif - -#endif /* SCOUT_UART_PARSER_H */ diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp index 1bc9dd2..2f23db4 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp @@ -44,9 +44,6 @@ class TracerBase : public MobileBase { TracerState tracer_state_; TracerMotionCmd current_motion_cmd_; - bool light_ctrl_enabled_ = false; - bool light_ctrl_requested_ = false; - void SendMotionCmd(uint8_t count); void SendLightCmd(const TracerLightCmd &cmd, uint8_t count); diff --git a/ugv_sdk/src/hunter_can_parser.c b/ugv_sdk/src/hunter_can_parser.c deleted file mode 100644 index d7ffb55..0000000 --- a/ugv_sdk/src/hunter_can_parser.c +++ /dev/null @@ -1,169 +0,0 @@ -/* - * hunter_can_parser.c - * - * Created on: Jan 02, 2020 12:40 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/hunter/hunter_can_parser.h" -#include "string.h" - -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) -{ - msg->type = HunterMsgNone; - - 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 = HunterMotionStatusMsg; - memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_SYSTEM_STATUS_STATUS_ID: - { - msg->type = HunterSystemStatusMsg; - memcpy(msg->body.system_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 = 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_MOTOR2_HEIGHT_DRIVER_STATUS_ID: - { - 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_MOTOR3_HEIGHT_DRIVER_STATUS_ID: - { - 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_MOTOR1_LOW_DRIVER_STATUS_ID: - { - 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; - } - case CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID: - { - 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_MOTOR3_LOW_DRIVER_STATUS_ID: - { - 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; - } - - return true; -} - -void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame) -{ - switch (msg->type) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case HunterMotionStatusMsg: - { - 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 HunterSystemStatusMsg: - { - 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 HunterMotionControlMsg: - { - EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); - break; - } - case HunterControlModeMsg: - { - EncodeHunterControlModeMsgToCAN(&(msg->body.mode_cmd_msg), tx_frame); - break; - } - case HunterParkControlMsg: - { - EncodeHunterControlParkMsgToCAN(&(msg->body.park_control_msg), tx_frame); - - break; - } - default: - break; - } -} - -void EncodeHunterMotionControlMsgToCAN(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); -} -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); -} - -uint8_t CalcHunterCANChecksum(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/scout_base.cpp b/ugv_sdk/src/scout_base.cpp index 77ba039..c823c15 100644 --- a/ugv_sdk/src/scout_base.cpp +++ b/ugv_sdk/src/scout_base.cpp @@ -1,4 +1,5 @@ #include "ugv_sdk/scout/scout_base.hpp" + #include #include #include @@ -15,143 +16,78 @@ 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++); + // EnableCommandedMode(); + if (can_connected_) { + SendMotionCmd(cmd_count++); + } +} + +void ScoutBase::EnableCommandedMode() { + AgxMessage c_msg; + c_msg.type = AgxMsgCtrlModeSelect; + memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8); + c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN; + + // send to can bus + can_frame c_frame; + EncodeCanFrame(&c_msg, &c_frame); + can_if_->SendFrame(c_frame); } void ScoutBase::SendMotionCmd(uint8_t count) { // motion control message - 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; + AgxMessage m_msg; + m_msg.type = AgxMsgMotionCommand; + memset(m_msg.body.motion_command_msg.raw, 0, 8); motion_cmd_mutex_.lock(); - 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; + int16_t linear_cmd = + static_cast(current_motion_cmd_.linear_velocity * 1000); + int16_t angular_cmd = + static_cast(current_motion_cmd_.angular_velocity * 1000); motion_cmd_mutex_.unlock(); - 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; + // SendControlCmd(); + m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte = + (static_cast(linear_cmd) >> 8) & 0x00ff; + m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte = + (static_cast(linear_cmd) >> 0) & 0x00ff; + m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte = + (static_cast(angular_cmd) >> 8) & 0x00ff; + m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte = + (static_cast(angular_cmd) >> 0) & 0x00ff; - 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_); - } + // send to can bus + can_frame m_frame; + EncodeCanFrame(&m_msg, &m_frame); + can_if_->SendFrame(m_frame); } -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); +void ScoutBase::SendLightCmd(const ScoutLightCmd &lcmd, uint8_t count) { + AgxMessage l_msg; + l_msg.type = AgxMsgLightCommand; + memset(l_msg.body.light_command_msg.raw, 0, 8); + + if (lcmd.enable_ctrl) { + l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE; + + l_msg.body.light_command_msg.cmd.front_light_mode = + static_cast(lcmd.front_mode); + l_msg.body.light_command_msg.cmd.front_light_custom = + lcmd.front_custom_value; + l_msg.body.light_command_msg.cmd.rear_light_mode = + static_cast(lcmd.rear_mode); + l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value; } 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; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - static_cast(current_light_cmd_.front_mode); - l_msg.body.light_control_msg.data.cmd.front_light_custom = - current_light_cmd_.front_custom_value; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - static_cast(current_light_cmd_.rear_mode); - l_msg.body.light_control_msg.data.cmd.rear_light_custom = - current_light_cmd_.rear_custom_value; - - // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << - // l_msg.data.cmd.front_light_custom << " , " - // << l_msg.data.cmd.rear_light_mode << " , " << - // l_msg.data.cmd.rear_light_custom << std::endl; - // std::cout << "light cmd generated" << std::endl; - } else { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = - LIGHT_DISABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; - } - light_ctrl_requested_ = false; - light_cmd_mutex_.unlock(); - - l_msg.body.light_control_msg.data.cmd.reserved0 = 0; - l_msg.body.light_control_msg.data.cmd.count = count; - -// if (can_connected_) -// l_msg.body.light_control_msg.data.cmd.checksum = 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 - - if (can_connected_) { - // send to can bus - can_frame l_frame; - EncodeScoutMsgToCAN(&l_msg, &l_frame); - - can_if_->SendFrame(l_frame); - } else { - // send to serial port - EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); - serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); + l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE; } - // std::cout << "cmd: " << static_cast(l_msg.data.cmd.front_light_mode) - // << " , " << static_cast(l_msg.data.cmd.front_light_custom) << " , " - // << static_cast(l_msg.data.cmd.rear_light_mode) << " , " << - // static_cast(l_msg.data.cmd.rear_light_custom) << std::endl; - // std::cout << "can: "; - // for (int i = 0; i < 8; ++i) - // std::cout << static_cast(l_frame.data[i]) << " "; - // std::cout << "uart: "; - // for (int i = 0; i < tx_cmd_len_; ++i) - // std::cout << static_cast(tx_buffer_[i]) << " "; - // std::cout << std::endl; + l_msg.body.light_command_msg.cmd.count = count; + + // send to can bus + can_frame l_frame; + EncodeCanFrame(&l_msg, &l_frame); + can_if_->SendFrame(l_frame); } ScoutState ScoutBase::GetScoutState() { @@ -159,9 +95,7 @@ ScoutState ScoutBase::GetScoutState() { return scout_state_; } -void ScoutBase::SetMotionCommand( - double linear_vel, double angular_vel, - ScoutMotionCmd::FaultClearFlag fault_clr_flag) { +void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) { // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); @@ -175,171 +109,122 @@ void ScoutBase::SetMotionCommand( angular_vel = ScoutMotionCmd::max_angular_velocity; std::lock_guard guard(motion_cmd_mutex_); - 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; + current_motion_cmd_.linear_velocity = linear_vel; + current_motion_cmd_.angular_velocity = angular_vel; + + FeedCmdTimeoutWatchdog(); } -void ScoutBase::SetLightCommand(ScoutLightCmd cmd) { - if (!cmd_thread_started_) StartCmdThread(); - - std::lock_guard guard(light_cmd_mutex_); - current_light_cmd_ = cmd; - light_ctrl_enabled_ = true; - light_ctrl_requested_ = true; -} - -void ScoutBase::DisableLightCmdControl() { - std::lock_guard guard(light_cmd_mutex_); - light_ctrl_enabled_ = false; - light_ctrl_requested_ = true; +void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) { + static uint8_t light_cmd_count = 0; + SendLightCmd(cmd, light_cmd_count++); } 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; -// } - - // 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); + AgxMessage status_msg; + DecodeCanFrame(rx_frame, &status_msg); NewStatusMsgReceivedCallback(status_msg); } -void ScoutBase::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); - ScoutMessage status_msg; - for (int i = 0; i < bytes_received; ++i) { - if (DecodeScoutMsgFromUART(buf[i], &status_msg)) - NewStatusMsgReceivedCallback(status_msg); - } -} - -void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) { +void ScoutBase::NewStatusMsgReceivedCallback(const AgxMessage &msg) { // std::cout << "new status msg received" << std::endl; std::lock_guard guard(scout_state_mutex_); UpdateScoutState(msg, scout_state_); } -void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, +void ScoutBase::UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) { switch (status_msg.type) { - 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.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; + case AgxMsgSystemState: { + // std::cout << "system status feedback received" << std::endl; + const SystemStateMessage &msg = status_msg.body.system_state_msg; + state.control_mode = msg.state.control_mode; + state.base_state = msg.state.vehicle_state; + state.battery_voltage = + (static_cast(msg.state.battery_voltage.low_byte) | + static_cast(msg.state.battery_voltage.high_byte) << 8) / + 10.0; + state.fault_code = msg.state.fault_code; break; } - case ScoutLightStatusMsg: { + case AgxMsgMotionState: { + // std::cout << "motion control feedback received" << std::endl; + const MotionStateMessage &msg = status_msg.body.motion_state_msg; + state.linear_velocity = + static_cast( + static_cast(msg.state.linear_velocity.low_byte) | + static_cast(msg.state.linear_velocity.high_byte) << 8) / + 1000.0; + state.angular_velocity = + static_cast( + static_cast(msg.state.angular_velocity.low_byte) | + static_cast(msg.state.angular_velocity.high_byte) + << 8) / + 1000.0; + break; + } + case AgxMsgLightState: { // 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) + const LightStateMessage &msg = status_msg.body.light_state_msg; + if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE) state.light_control_enabled = false; else state.light_control_enabled = true; - state.front_light_state.mode = msg.data.status.front_light_mode; - state.front_light_state.custom_value = msg.data.status.front_light_custom; - state.rear_light_state.mode = msg.data.status.rear_light_mode; - state.rear_light_state.custom_value = msg.data.status.rear_light_custom; + state.front_light_state.mode = msg.state.front_light_mode; + state.front_light_state.custom_value = msg.state.front_light_custom; + state.rear_light_state.mode = msg.state.rear_light_mode; + state.rear_light_state.custom_value = msg.state.rear_light_custom; break; } - case ScoutSystemStatusMsg: { - // 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) / + case AgxMsgActuatorHSState: { + // std::cout << "actuator hs feedback received" << std::endl; + const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg; + state.actuator_states[msg.motor_id].motor_current = + (static_cast(msg.data.state.current.low_byte) | + static_cast(msg.data.state.current.high_byte) << 8) / 10.0; - state.fault_code =msg.data.status.fault_code; + state.actuator_states[msg.motor_id].motor_rpm = static_cast( + static_cast(msg.data.state.rpm.low_byte) | + static_cast(msg.data.state.rpm.high_byte) << 8); + state.actuator_states[msg.motor_id].motor_pulses = static_cast( + static_cast(msg.data.state.pulse_count.low_byte) | + static_cast(msg.data.state.pulse_count.high_byte) << 8); break; } -// case 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 MotorDriverHeightSpeedStatusMessage &msg =status_msg.body.motor_driver_height_speed_status_msg; - for (int i = 0; i < ScoutState::motor_num; ++i) { - state.motor_hs_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) / + case AgxMsgActuatorLSState: { + // std::cout << "actuator ls feedback received" << std::endl; + const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg; + for (int i = 0; i < 2; ++i) { + state.actuator_states[msg.motor_id].driver_voltage = + (static_cast(msg.data.state.driver_voltage.low_byte) | + static_cast(msg.data.state.driver_voltage.high_byte) + << 8) / 10.0; - state.motor_hs_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_hs_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_ls_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_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + state.actuator_states[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_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] - .motor_temperature = msg.data.status.motor_temperature; - state.motor_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] - .driver_state = msg.data.status.driver_state; + static_cast(msg.data.state.driver_temperature.low_byte) | + static_cast(msg.data.state.driver_temperature.high_byte) + << 8); + state.actuator_states[msg.motor_id].motor_temperature = + msg.data.state.motor_temperature; + state.actuator_states[msg.motor_id].driver_state = + msg.data.state.driver_state; } break; } - 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; + case AgxMsgOdometry: { + // std::cout << "Odometer msg feedback received" << std::endl; + const OdometryMessage &msg = status_msg.body.odometry_msg; + state.right_odometry = static_cast( + (static_cast(msg.state.right_wheel.lsb)) | + (static_cast(msg.state.right_wheel.low_byte) << 8) | + (static_cast(msg.state.right_wheel.high_byte) << 16) | + (static_cast(msg.state.right_wheel.msb) << 24)); + state.left_odometry = static_cast( + (static_cast(msg.state.left_wheel.lsb)) | + (static_cast(msg.state.left_wheel.low_byte) << 8) | + (static_cast(msg.state.left_wheel.high_byte) << 16) | + (static_cast(msg.state.left_wheel.msb) << 24)); } } } diff --git a/ugv_sdk/src/scout_can_parser.c b/ugv_sdk/src/scout_can_parser.c deleted file mode 100644 index bb4a5a3..0000000 --- a/ugv_sdk/src/scout_can_parser.c +++ /dev/null @@ -1,228 +0,0 @@ -/* - * scout_can_parser.c - * - * Created on: Aug 31, 2019 04:25 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/scout/scout_can_parser.h" - -#include "string.h" - -static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); -static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame); -static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame); - - -bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg) -{ - msg->type = ScoutMsgNone; - - switch (rx_frame->can_id) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_CONTROL_STATUS_ID: - { - msg->type = ScoutMotionStatusMsg; - // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; - memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_LIGHT_CONTROL_STATUS_ID: - { - msg->type = ScoutLightStatusMsg; - // msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_SYSTEM_STATUS_STATUS_ID: - { - msg->type = ScoutSystemStatusMsg; - // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; - memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case CAN_MSG_MOTION_CONTROL_CMD_ID: - { - msg->type = ScoutMotionControlMsg; - // msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID; - memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - case CAN_MSG_LIGHT_CONTROL_CMD_ID: - { - msg->type = ScoutLightControlMsg; - // msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); - break; - } - 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; - } - - return true; -} - -void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame) -{ - switch (msg->type) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case ScoutMotionStatusMsg: - { - tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutLightStatusMsg: - { - tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc); - break; - } - case ScoutSystemStatusMsg: - { - tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc); - break; - } -// case ScoutMotorDriverStatusMsg: -// { -// if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) -// tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; -// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) -// tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; -// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) -// tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; -// else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) -// tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; -// tx_frame->can_dlc = 8; -// memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc); -// break; -// } - case ScoutMotionControlMsg: - { - EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); - break; - } - case ScoutControlModeMsg: - { - EncodeScoutnControlModeMsgToCAN(&(msg->body.motion_control_msg), tx_frame); - break; - } - case ScoutLightControlMsg: - { - EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); - break; - } - default: - break; - } -// tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} - -void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame) -{ - tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} -void 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; - tx_frame->can_dlc = 8; - memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); - tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); -} - -uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) -{ - uint8_t checksum = 0x00; - checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; - for (int i = 0; i < (dlc - 1); ++i) - checksum += data[i]; - return checksum; -} diff --git a/ugv_sdk/src/scout_uart_parser.c b/ugv_sdk/src/scout_uart_parser.c deleted file mode 100644 index b8184e5..0000000 --- a/ugv_sdk/src/scout_uart_parser.c +++ /dev/null @@ -1,629 +0,0 @@ -/* - * scout_uart_parser.c - * - * Created on: Aug 14, 2019 12:02 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/scout/scout_uart_parser.h" - -// #define USE_XOR_CHECKSUM - -// #define PRINT_CPP_DEBUG_INFO -// #define PRINT_JLINK_DEBUG_INFO - -#ifdef PRINT_CPP_DEBUG_INFO -#undef PRINT_JLINK_DEBUG_INFO -#endif - -#ifdef PRINT_CPP_DEBUG_INFO -#define -#elif (defined(PRINT_JLINK_DEBUG_INFO)) -#include "segger/jlink_rtt.h" -#endif - -typedef enum -{ - WAIT_FOR_SOF1 = 0, - WAIT_FOR_SOF2, - WAIT_FOR_FRAME_LEN, - WAIT_FOR_FRAME_TYPE, - WAIT_FOR_FRAME_ID, - WAIT_FOR_PAYLOAD, - WAIT_FOR_FRAME_COUNT, - WAIT_FOR_CHECKSUM -} ScoutSerialDecodeState; - -#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2) - -#define FRAME_SOF_LEN ((uint8_t)2) -#define FRAME_FIXED_FIELD_LEN ((uint8_t)4) - -#define FRAME_SOF1 ((uint8_t)0x5a) -#define FRAME_SOF2 ((uint8_t)0xa5) - -#define FRAME_TYPE_CONTROL ((uint8_t)0x55) -#define FRAME_TYPE_STATUS ((uint8_t)0xaa) - -#define FRAME_NONE_ID ((uint8_t)0x00) - -// frame buffer -static struct -{ - uint8_t frame_id; - uint8_t frame_type; - uint8_t frame_len; - uint8_t frame_cnt; - uint8_t frame_checksum; - uint8_t internal_checksum; - uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE]; - size_t payload_data_pos; -} uart_parsing_data; - -// statisctics -typedef struct -{ - uint32_t frame_parsed; - uint32_t frame_with_wrong_checksum; -} UARTParsingStats; - -static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123}; - -// internal functions -static bool ParseChar(uint8_t c, ScoutMessage *msg); -static uint8_t CalcBufferedFrameChecksum(); -static bool ConstructStatusMessage(ScoutMessage *msg); -static bool ConstructControlMessage(ScoutMessage *msg); - -static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len); -static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len); - -void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_TYPE_STATUS; - - switch (msg->type) - { - // in the current implementation, both MsgType and can_frame include 8 * uint8_t - case ScoutMotionStatusMsg: - { - buf[4] = UART_FRAME_MOTION_STATUS_ID; - buf[5] = msg->body.motion_status_msg.data.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] = 0; - break; - } - case ScoutLightStatusMsg: - { - buf[4] = UART_FRAME_LIGHT_STATUS_ID; - buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable; - buf[6] = msg->body.light_status_msg.data.status.front_light_mode; - buf[7] = msg->body.light_status_msg.data.status.front_light_custom; - buf[8] = msg->body.light_status_msg.data.status.rear_light_mode; - buf[9] = msg->body.light_status_msg.data.status.rear_light_custom; - buf[10] = 0; - buf[11] = msg->body.light_status_msg.data.status.count; - break; - } - case ScoutSystemStatusMsg: - { - buf[4] = UART_FRAME_SYSTEM_STATUS_ID; - buf[5] = msg->body.system_status_msg.data.status.base_state; - buf[6] = msg->body.system_status_msg.data.status.control_mode; - buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte; - buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte; - buf[9] = msg->body.system_status_msg.data.status.fault_code; - buf[10] = 0; - buf[11] = 0; - break; - } - case ScoutMotorDriverStatusMsg: - { - if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) - buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) - buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) - buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) - buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID; - buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte; - buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte; - buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte; - buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte; - buf[9] = msg->body.motor_driver_status_msg.data.status.temperature; - buf[10] = 0; - buf[11] = msg->body.motor_driver_status_msg.data.status.count; - break; - } - case ScoutMotionControlMsg: - { - EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len); - break; - } - case ScoutLightControlMsg: - { - EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len); - break; - } - default: - break; - } - - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg) -{ - static ScoutMessage decoded_msg; - - bool result = ParseChar(c, &decoded_msg); - if (result) - *msg = decoded_msg; - return result; -} - -void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_TYPE_CONTROL; - buf[4] = UART_FRAME_MOTION_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.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] = 0x00; - buf[12] = 0x00; - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len) -{ - // SOF - buf[0] = FRAME_SOF1; - buf[1] = FRAME_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = FRAME_TYPE_CONTROL; - buf[4] = UART_FRAME_LIGHT_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.light_ctrl_enable; - buf[6] = msg->data.cmd.front_light_mode; - buf[7] = msg->data.cmd.front_light_custom; - buf[8] = msg->data.cmd.rear_light_mode; - buf[9] = msg->data.cmd.rear_light_custom; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + FRAME_SOF_LEN + 1; -} - -bool ParseChar(uint8_t c, ScoutMessage *msg) -{ - static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1; - - bool new_frame_parsed = false; - switch (decode_state) - { - case WAIT_FOR_SOF1: - { - if (c == FRAME_SOF1) - { - uart_parsing_data.frame_id = FRAME_NONE_ID; - uart_parsing_data.frame_type = 0; - uart_parsing_data.frame_len = 0; - uart_parsing_data.frame_cnt = 0; - uart_parsing_data.frame_checksum = 0; - uart_parsing_data.internal_checksum = 0; - uart_parsing_data.payload_data_pos = 0; - memset(uart_parsing_data.payload_buffer, 0, PAYLOAD_BUFFER_SIZE); - - decode_state = WAIT_FOR_SOF2; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof1" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof1\n"); -#endif - } - break; - } - case WAIT_FOR_SOF2: - { - if (c == FRAME_SOF2) - { - decode_state = WAIT_FOR_FRAME_LEN; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof2\n"); -#endif - } - else - { - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "failed to find sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "failed to find sof2\n"); -#endif - } - break; - } - case WAIT_FOR_FRAME_LEN: - { - uart_parsing_data.frame_len = c; - decode_state = WAIT_FOR_FRAME_TYPE; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame len: " << std::hex << static_cast(frame_len) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame len: %d\n", frame_len); -#endif - break; - } - case WAIT_FOR_FRAME_TYPE: - { - switch (c) - { - case FRAME_TYPE_CONTROL: - { - uart_parsing_data.frame_type = FRAME_TYPE_CONTROL; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "control type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "control type frame received\n"); -#endif - break; - } - case FRAME_TYPE_STATUS: - { - uart_parsing_data.frame_type = FRAME_TYPE_STATUS; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "status type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "status type frame received\n"); -#endif - break; - } - default: - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - case WAIT_FOR_FRAME_ID: - { - switch (c) - { - case UART_FRAME_SYSTEM_STATUS_ID: - case UART_FRAME_MOTION_STATUS_ID: - case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: - case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: - case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: - case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: - case UART_FRAME_LIGHT_STATUS_ID: - { - uart_parsing_data.frame_id = c; - decode_state = WAIT_FOR_PAYLOAD; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame id: " << std::hex << static_cast(frame_id) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame id: %d\n", frame_id); -#endif - break; - } - default: - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Unknown frame id" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "ERROR: Unknown frame id\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - case WAIT_FOR_PAYLOAD: - { - uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "1 byte added: " << std::hex << static_cast(c) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "1 byte added: %d\n", c); -#endif - if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN)) - decode_state = WAIT_FOR_FRAME_COUNT; - break; - } - case WAIT_FOR_FRAME_COUNT: - { - uart_parsing_data.frame_cnt = c; - decode_state = WAIT_FOR_CHECKSUM; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame count: " << std::hex << static_cast(frame_cnt) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt); -#endif - break; - } - case WAIT_FOR_CHECKSUM: - { - uart_parsing_data.frame_checksum = c; - uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum(); - new_frame_parsed = true; - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum); - JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum); -#endif - break; - } - default: - break; - } - - if (new_frame_parsed) - { - if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum) - { -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum correct" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum correct\n"); -#endif - if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS) - ConstructStatusMessage(msg); - else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL) - ConstructControlMessage(msg); - ++uart_parsing_stats.frame_parsed; - } - else - { - ++uart_parsing_stats.frame_with_wrong_checksum; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum is NOT correct" << std::endl; - std::cout << std::hex << static_cast(frame_id) << " , " << static_cast(frame_len) << " , " << static_cast(frame_cnt) << " , " << static_cast(frame_checksum) << " : " << std::dec << std::endl; - std::cout << "payload: "; - for (int i = 0; i < payload_data_pos; ++i) - std::cout << std::hex << static_cast(payload_buffer[i]) << std::dec << " "; - std::cout << std::endl; - std::cout << "--- frame checksum: " << std::hex << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum is NOT correct\n"); -#endif - } - } - - return new_frame_parsed; -} - -bool ConstructControlMessage(ScoutMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_MOTION_CONTROL_ID: - { - msg->type = ScoutMotionControlMsg; - msg->body.motion_control_msg.data.cmd.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.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: - { - msg->type = ScoutLightControlMsg; - msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_control_msg.data.cmd.rear_light_custom = uart_parsing_data.payload_buffer[4]; - msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5]; - msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt; - // msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum; - break; - } - } - return true; -} - -bool ConstructStatusMessage(ScoutMessage *msg) -{ - if (msg == NULL) - return false; - - switch (uart_parsing_data.frame_id) - { - case UART_FRAME_SYSTEM_STATUS_ID: - { - msg->type = ScoutSystemStatusMsg; - msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0]; - msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1]; - msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.system_status_msg.data.status.fault_code = 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.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: - { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: - { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum; - break; - } - case UART_FRAME_LIGHT_STATUS_ID: - { - msg->type = ScoutLightStatusMsg; - msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0]; - msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1]; - msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2]; - msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3]; - msg->body.light_status_msg.data.status.rear_light_custom = uart_parsing_data.payload_buffer[4]; - msg->body.light_status_msg.data.status.reserved0 = 0x00; - msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt; - msg->body.light_status_msg.data.status.reserved1 = 0x00; - break; - } - } - return true; -} - -uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len) -{ - uint8_t checksum = 0; - -#ifdef USE_XOR_CHECKSUM - for (int i = 0; i < len; ++i) - checksum ^= buf[i]; -#else - for (int i = 0; i < len; ++i) - checksum += buf[i]; -#endif - - return checksum; -} - -uint8_t CalcBufferedFrameChecksum() -{ - uint8_t checksum = 0x00; - -#ifdef USE_XOR_CHECKSUM - checksum ^= FRAME_SOF1; - checksum ^= FRAME_SOF2; - checksum ^= uart_parsing_data.frame_len; - checksum ^= uart_parsing_data.frame_type; - checksum ^= uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum ^= uart_parsing_data.payload_buffer[i]; - checksum ^= uart_parsing_data.frame_cnt; -#else - checksum += FRAME_SOF1; - checksum += FRAME_SOF2; - checksum += uart_parsing_data.frame_len; - checksum += uart_parsing_data.frame_type; - checksum += uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum += uart_parsing_data.payload_buffer[i]; - checksum += uart_parsing_data.frame_cnt; -#endif - - return checksum; -} diff --git a/ugv_sdk/src/tracer_base.cpp b/ugv_sdk/src/tracer_base.cpp index af0c226..f22ed15 100644 --- a/ugv_sdk/src/tracer_base.cpp +++ b/ugv_sdk/src/tracer_base.cpp @@ -87,8 +87,6 @@ void TracerBase::SendLightCmd(const TracerLightCmd &lcmd, uint8_t count) { can_frame l_frame; EncodeCanFrame(&l_msg, &l_frame); can_if_->SendFrame(l_frame); - - std::cout << "light cmd sent <---------------------" << std::endl; } TracerState TracerBase::GetTracerState() {