From 4ebef2e906d7af33784f2beba4b0428e26c104d1 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Sat, 10 Jul 2021 21:50:27 +0800 Subject: [PATCH] saved work --- .../details/interface/agilex_message.h | 43 +++++-- .../details/interface/scout_interface.hpp | 10 +- .../ugv_sdk/details/robot_base/scout_base.hpp | 25 ++-- src/protocol_v1/agilex_message_v1.h | 111 ------------------ src/protocol_v1/agilex_msg_parser_v1.c | 4 +- src/protocol_v1/agilex_msg_parser_v1.h | 6 +- 6 files changed, 55 insertions(+), 144 deletions(-) delete mode 100644 src/protocol_v1/agilex_message_v1.h diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index 1b510d0..7b7a16f 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -44,6 +44,18 @@ typedef struct { uint8_t motion_mode; } MotionModeMessage; +// V1-only messages +typedef struct { + ControlMode control_mode; + bool clear_all_error; + float linear; + float angular; +} MotionCommandMessageV1; + +typedef struct { + bool set_neutral; +} ValueSetCommandMessageV1; + /**************** Feedback messages *****************/ #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) @@ -64,7 +76,6 @@ typedef struct { uint16_t error_code; } SystemStateMessage; -// 0x221 typedef struct { float linear_velocity; float angular_velocity; // only valid for differential drivering @@ -72,7 +83,6 @@ typedef struct { float steering_angle; // only valid for ackermann steering } MotionStateMessage; -// 0x231 typedef LightCommandMessage LightStateMessage; typedef struct { @@ -94,12 +104,6 @@ typedef struct { int32_t pulse_count; } ActuatorHSStateMessage; -typedef struct { - uint8_t motion_mode; - uint8_t mode_changing; -} MotionModeFeedbackMessage; - -// 0x261 - 0x264 #define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) #define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) #define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04) @@ -117,6 +121,18 @@ typedef struct { uint8_t driver_state; } ActuatorLSStateMessage; +typedef struct { + uint8_t motion_mode; + uint8_t mode_changing; +} MotionModeFeedbackMessage; + +// V1-only messages +typedef struct { + float current; + int16_t rpm; + float temperature; +} ActuatorStateMessageV1; + /***************** Sensor messages ******************/ typedef struct { @@ -167,7 +183,6 @@ typedef struct { float temperature; } BmsBasicMessage; -// 0x362 #define BMS_PROT1_CHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x01) #define BMS_PROT1_CHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x02) #define BMS_PROT1_DISCHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x10) @@ -273,7 +288,11 @@ typedef enum { AgxMsgControlModeConfig, AgxMsgSteerNeutralRequest, AgxMsgSteerNeutralResponse, - AgxMsgStateResetConfig + AgxMsgStateResetConfig, + // V1-only messages + AgxMsgMotionCommandV1, + AgxMsgValueSetCommandV1, + AgxMsgActuatorStateV1 } MsgType; typedef struct { @@ -309,6 +328,10 @@ typedef struct { SteerNeutralRequestMessage steer_neutral_request_msg; SteerNeutralResponseMessage steer_neutral_response_msg; StateResetConfigMessage state_reset_config_msg; + // V1-only messages + MotionCommandMessageV1 v1_motion_command_msg; + ValueSetCommandMessageV1 v1_value_set_command_msg; + ActuatorStateMessageV1 v1_actuator_stage_msg; } body; } AgxMessage; diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index 46d2014..61c491c 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -16,18 +16,16 @@ namespace westonrobot { struct ScoutState { - // system state SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; - RcStateMessage rc_state; - + // actuator state + // - for v2 robots only ActuatorHSStateMessage actuator_hs_state[4]; ActuatorLSStateMessage actuator_ls_state[4]; - - // sensor data - OdometryMessage odometry; + // - for v1 robots only + ActuatorStateMessageV1 actuator_state[4]; }; struct ScoutInterface { diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index 06a25ff..7ad7150 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -61,12 +61,13 @@ class ScoutBase : public AgilexBase, public ScoutInterface { void ParseCANFrame(can_frame *rx_frame) override { AgxMessage status_msg; - AgilexBase::parser_.DecodeMessage(rx_frame, &status_msg); - std::lock_guard guard(AgilexBase::state_mutex_); - UpdateScoutState(status_msg, scout_state_); + if (AgilexBase::parser_.DecodeMessage(rx_frame, &status_msg)) { + UpdateScoutState(status_msg, scout_state_); + } } void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) { + std::lock_guard guard(AgilexBase::state_mutex_); switch (status_msg.type) { case AgxMsgSystemState: { // std::cout << "system status feedback received" << std::endl; @@ -83,10 +84,10 @@ class ScoutBase : public AgilexBase, public ScoutInterface { state.light_state = status_msg.body.light_state_msg; break; } - case AgxMsgRcState: { - state.rc_state = status_msg.body.rc_state_msg; - break; - } + // case AgxMsgRcState: { + // state.rc_state = status_msg.body.rc_state_msg; + // break; + // } case AgxMsgActuatorHSState: { // std::cout << "actuator hs feedback received" << std::endl; state @@ -101,11 +102,11 @@ class ScoutBase : public AgilexBase, public ScoutInterface { status_msg.body.actuator_ls_state_msg; break; } - /* sensor feedback */ - case AgxMsgOdometry: { - // std::cout << "Odometer msg feedback received" << std::endl; - state.odometry = status_msg.body.odometry_msg; - } + /* sensor feedback */ + // case AgxMsgOdometry: { + // // std::cout << "Odometer msg feedback received" << std::endl; + // state.odometry = status_msg.body.odometry_msg; + // } default: break; } diff --git a/src/protocol_v1/agilex_message_v1.h b/src/protocol_v1/agilex_message_v1.h deleted file mode 100644 index a3971c4..0000000 --- a/src/protocol_v1/agilex_message_v1.h +++ /dev/null @@ -1,111 +0,0 @@ -/* - * agilex_message_v1.h - * - * Created on: Jul 09, 2021 20:55 - * Description: - * all values are using SI units (e.g. meter/second/radian) - * - * Re-implemented as a subset of AgxMessage defined in protocol v2 - * - * Copyright (c) 2021 Ruixiang Du (rdu) - */ - -#ifndef AGILEX_MESSAGE_V1_H -#define AGILEX_MESSAGE_V1_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -#include "ugv_sdk/details/interface/agilex_types.h" - -/***************** Control messages *****************/ - -typedef struct { - ControlMode control_mode; - bool clear_all_error; - float linear; - float angular; -} MotionCommandMessage; - -typedef struct { - bool enable_cmd_ctrl; - LightOperation front_light; - LightOperation rear_light; -} LightCommandMessage; - -typedef struct { - bool set_neutral; -} ValueSetCommandMessage; - -/**************** Feedback messages *****************/ - -#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) -#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) -#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) -#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002) -#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004) -#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008) -#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010) -#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020) -#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040) -#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080) - -typedef struct { - VehicleState vehicle_state; - ControlMode control_mode; - float battery_voltage; - uint16_t error_code; -} SystemStateMessage; - -typedef struct { - float linear_velocity; - float angular_velocity; -} MotionStateMessage; - -typedef LightCommandMessage LightStateMessage; - -typedef struct { - float current; - int16_t rpm; - float temperature; -} ActuatorStateMessage; - -////////////////////////////////////////////////////// - -typedef enum { - AgxMsgUnkonwn = 0x00, - // command - AgxMsgMotionCommand, - AgxMsgLightCommand, - AgxMsgValueSetCommand, - // state feedback - AgxMsgSystemState, - AgxMsgMotionState, - AgxMsgLightState, - AgxMsgActuatorState, -} MsgType; - -typedef struct { - MsgType type; - union { - // command - MotionCommandMessage motion_command_msg; - LightCommandMessage light_command_msg; - // state feedback - SystemStateMessage system_state_msg; - MotionStateMessage motion_state_msg; - LightStateMessage light_state_msg; - ActuatorStateMessage actuator_state_msg; - } body; -} AgxMessageV1; - -#ifdef __cplusplus -} -#endif - -#endif /* AGILEX_MESSAGE_V1_H */ diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c index 6218e68..257cadd 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.c +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -13,6 +13,6 @@ #include "stdio.h" #include "string.h" -bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg) {} -void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame) {} +bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {} +void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {} uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {} diff --git a/src/protocol_v1/agilex_msg_parser_v1.h b/src/protocol_v1/agilex_msg_parser_v1.h index 5140a75..5f560e5 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.h +++ b/src/protocol_v1/agilex_msg_parser_v1.h @@ -28,10 +28,10 @@ struct can_frame { }; #endif -#include "protocol_v1/agilex_message_v1.h" +#include "ugv_sdk/details/interface/agilex_message.h" -bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg); -void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame); +bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg); +void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame); uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc); #ifdef __cplusplus