From 380fdfe898294efd4ac8ba258fc1b46b19015897 Mon Sep 17 00:00:00 2001 From: lagrangeluo <654186749@qq.com> Date: Mon, 4 Dec 2023 11:07:45 +0800 Subject: [PATCH 1/2] add bms --- .../details/interface/hunter_interface.hpp | 6 +++++ .../details/robot_base/hunter_base.hpp | 23 +++++++++++++++++++ include/ugv_sdk/mobile_robot/hunter_robot.hpp | 1 + src/mobile_robot/hunter_robot.cpp | 4 ++++ 4 files changed, 34 insertions(+) diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp index ada44e18..1527eb4f 100644 --- a/include/ugv_sdk/details/interface/hunter_interface.hpp +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -34,6 +34,11 @@ struct HunterActuatorState { ActuatorStateMessageV1 actuator_state[3]; }; +struct HunterCommonSensorState { + AgxMsgTimeStamp time_stamp; + + BmsBasicMessage bms_basic_state; +}; struct HunterInterface { virtual ~HunterInterface() = default; @@ -45,6 +50,7 @@ struct HunterInterface { // get robot state virtual HunterCoreState GetRobotState() = 0; virtual HunterActuatorState GetActuatorState() = 0; + virtual HunterCommonSensorState GetCommonSensorState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp index 4e83793c..309f314b 100644 --- a/include/ugv_sdk/details/robot_base/hunter_base.hpp +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -61,6 +61,29 @@ class HunterBase : public AgilexBase, public HunterInterface { return hunter_actuator; } + HunterCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + HunterCommonSensorState hunter_bms; + + hunter_bms.time_stamp = common_sensor.time_stamp; + + hunter_bms.bms_basic_state.current = common_sensor.bms_basic_state.current; + // Note: BMS CAN message definition is not consistent across AgileX robots. + // Robots with steering mechanism should additionally divide the voltage by + // 10. + hunter_bms.bms_basic_state.voltage = + common_sensor.bms_basic_state.voltage * 0.1f; + hunter_bms.bms_basic_state.battery_soc = + common_sensor.bms_basic_state.battery_soc; + hunter_bms.bms_basic_state.battery_soh = + common_sensor.bms_basic_state.battery_soh; + hunter_bms.bms_basic_state.temperature = + common_sensor.bms_basic_state.temperature; + + return hunter_bms; + } void ActivateBrake() override { AgilexBase::SetBrakeMode(AgxBrakeMode::BRAKE_MODE_LOCK); } diff --git a/include/ugv_sdk/mobile_robot/hunter_robot.hpp b/include/ugv_sdk/mobile_robot/hunter_robot.hpp index 064524f4..49915efa 100644 --- a/include/ugv_sdk/mobile_robot/hunter_robot.hpp +++ b/include/ugv_sdk/mobile_robot/hunter_robot.hpp @@ -39,6 +39,7 @@ class HunterRobot : public RobotCommonInterface, public HunterInterface { // get robot state HunterCoreState GetRobotState() override; HunterActuatorState GetActuatorState() override; + HunterCommonSensorState GetCommonSensorState() override; private: RobotCommonInterface* robot_; diff --git a/src/mobile_robot/hunter_robot.cpp b/src/mobile_robot/hunter_robot.cpp index 1ca2ff63..4bff2fe3 100644 --- a/src/mobile_robot/hunter_robot.cpp +++ b/src/mobile_robot/hunter_robot.cpp @@ -63,4 +63,8 @@ HunterActuatorState HunterRobot::GetActuatorState() { auto hunter = dynamic_cast(robot_); return hunter->GetActuatorState(); } +HunterCommonSensorState HunterRobot::GetCommonSensorState() { + auto hunter = dynamic_cast(robot_); + return hunter->GetCommonSensorState(); +} } // namespace westonrobot \ No newline at end of file From 1e3ad3073a97484e7d5b9bfc22d3581bd69e91c5 Mon Sep 17 00:00:00 2001 From: lagrangeluo <654186749@qq.com> Date: Mon, 4 Dec 2023 14:24:16 +0800 Subject: [PATCH 2/2] support bms extend struct --- .../details/interface/agilex_message.h | 20 +++++++++----- .../details/interface/hunter_interface.hpp | 1 + .../interface/robot_common_interface.hpp | 1 + .../details/robot_base/agilex_base.hpp | 4 +++ .../details/robot_base/hunter_base.hpp | 8 ++++++ src/protocol_v2/agilex_msg_parser_v2.c | 20 ++++++++------ src/protocol_v2/agilex_protocol_v2.h | 26 +++++++++++++------ 7 files changed, 57 insertions(+), 23 deletions(-) diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index b0d605bc..350720e6 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -229,13 +229,19 @@ typedef struct { #define BMS_PROT4_DISCHARGING_MOS_FAILURE_SET_MASK ((uint8_t)0x08) #define BMS_PROT4_WEAK_SIGNAL_SWITCH_OPEN_SET_MASK ((uint8_t)0x10) -typedef struct { - uint8_t protection_code1; - uint8_t protection_code2; - uint8_t protection_code3; - uint8_t protection_code4; - uint8_t battery_max_teperature; - uint8_t battery_min_teperature; +// typedef struct { +// uint8_t protection_code1; +// uint8_t protection_code2; +// uint8_t protection_code3; +// uint8_t protection_code4; +// uint8_t battery_max_teperature; +// uint8_t battery_min_teperature; +// } BmsExtendedMessage; +typedef struct { + uint8_t alarm_status_1; + uint8_t alarm_status_2; + uint8_t warn_status_1; + uint8_t warn_status_2; } BmsExtendedMessage; /************ Query/config messages ****************/ diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp index 1527eb4f..61902c49 100644 --- a/include/ugv_sdk/details/interface/hunter_interface.hpp +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -38,6 +38,7 @@ struct HunterCommonSensorState { AgxMsgTimeStamp time_stamp; BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extend_state; }; struct HunterInterface { virtual ~HunterInterface() = default; diff --git a/include/ugv_sdk/details/interface/robot_common_interface.hpp b/include/ugv_sdk/details/interface/robot_common_interface.hpp index c6df3784..ad9f7545 100644 --- a/include/ugv_sdk/details/interface/robot_common_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_common_interface.hpp @@ -48,6 +48,7 @@ struct CommonSensorStateMsgGroup { AgxMsgTimeStamp time_stamp; BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extend_state; }; class RobotCommonInterface { diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index afa0d9a1..f37f453a 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -360,6 +360,10 @@ class AgilexBase : public RobotCommonInterface { status_msg.body.bms_basic_msg; break; } + case AgxMsgBmsExtended: { + common_sensor_state_msgs_.bms_extend_state = + status_msg.body.bms_extended_msg; + } default: break; } diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp index 309f314b..d3073512 100644 --- a/include/ugv_sdk/details/robot_base/hunter_base.hpp +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -81,6 +81,14 @@ class HunterBase : public AgilexBase, public HunterInterface { common_sensor.bms_basic_state.battery_soh; hunter_bms.bms_basic_state.temperature = common_sensor.bms_basic_state.temperature; + hunter_bms.bms_extend_state.alarm_status_1 = + common_sensor.bms_extend_state.alarm_status_1; + hunter_bms.bms_extend_state.alarm_status_2 = + common_sensor.bms_extend_state.alarm_status_2; + hunter_bms.bms_extend_state.warn_status_1 = + common_sensor.bms_extend_state.warn_status_1; + hunter_bms.bms_extend_state.warn_status_2 = + common_sensor.bms_extend_state.warn_status_2; return hunter_bms; } diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index 5b561138..d3ce8c3a 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -277,14 +277,18 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { case CAN_MSG_BMS_EXTENDED_ID: { msg->type = AgxMsgBmsExtended; BmsExtendedFrame *frame = (BmsExtendedFrame *)(rx_frame->data); - msg->body.bms_extended_msg.protection_code1 = frame->protection_code1; - msg->body.bms_extended_msg.protection_code2 = frame->protection_code2; - msg->body.bms_extended_msg.protection_code3 = frame->protection_code3; - msg->body.bms_extended_msg.protection_code4 = frame->protection_code4; - msg->body.bms_extended_msg.battery_max_teperature = - frame->battery_max_teperature; - msg->body.bms_extended_msg.battery_min_teperature = - frame->battery_min_teperature; + // msg->body.bms_extended_msg.protection_code1 = frame->protection_code1; + // msg->body.bms_extended_msg.protection_code2 = frame->protection_code2; + // msg->body.bms_extended_msg.protection_code3 = frame->protection_code3; + // msg->body.bms_extended_msg.protection_code4 = frame->protection_code4; + // msg->body.bms_extended_msg.battery_max_teperature = + // frame->battery_max_teperature; + // msg->body.bms_extended_msg.battery_min_teperature = + // frame->battery_min_teperature; + msg->body.bms_extended_msg.alarm_status_1 = frame->alarm_status_1; + msg->body.bms_extended_msg.alarm_status_2 = frame->alarm_status_2; + msg->body.bms_extended_msg.warn_status_1 = frame->warn_status_1; + msg->body.bms_extended_msg.warn_status_2 = frame->warn_status_2; break; } /*************** query/config frame **************/ diff --git a/src/protocol_v2/agilex_protocol_v2.h b/src/protocol_v2/agilex_protocol_v2.h index 81c27ac3..d0c97a99 100644 --- a/src/protocol_v2/agilex_protocol_v2.h +++ b/src/protocol_v2/agilex_protocol_v2.h @@ -315,14 +315,24 @@ typedef struct { struct16_t temperature; } BmsBasicFrame; -typedef struct { - uint8_t protection_code1; - uint8_t protection_code2; - uint8_t protection_code3; - uint8_t protection_code4; - uint8_t battery_max_teperature; - uint8_t battery_min_teperature; - struct16_t count; +// typedef struct { +// uint8_t protection_code1; +// uint8_t protection_code2; +// uint8_t protection_code3; +// uint8_t protection_code4; +// uint8_t battery_max_teperature; +// uint8_t battery_min_teperature; +// struct16_t count; +// } BmsExtendedFrame; +typedef struct { + uint8_t alarm_status_1; + uint8_t alarm_status_2; + uint8_t warn_status_1; + uint8_t warn_status_2; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; } BmsExtendedFrame; // query/config