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] 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