support bms extend struct

agilexrobotics · Dec 4, 2023 · 1e3ad30 · 1e3ad30
1 parent 380fdfe
commit 1e3ad30
Show file tree

Hide file tree

Showing 7 changed files with 57 additions and 23 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -81,6 +81,14 @@ class HunterBase : public AgilexBase<ParserType>, 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
@@ -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
@@ -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