Merge pull request #5 from agilexrobotics/feat-bms

Feat bms
agilexrobotics · Dec 20, 2023 · 357f85d · 357f85d
2 parents 20d85ec + 1e3ad30
commit 357f85d
Show file tree

Hide file tree

Showing 9 changed files with 91 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
@@ -34,6 +34,12 @@ struct HunterActuatorState {
   ActuatorStateMessageV1 actuator_state[3];
 };
 
+struct HunterCommonSensorState {
+  AgxMsgTimeStamp time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+  BmsExtendedMessage bms_extend_state;
+};
 struct HunterInterface {
   virtual ~HunterInterface() = default;
 
@@ -45,6 +51,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/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
@@ -61,6 +61,37 @@ class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
     return hunter_actuator;
   }
 
+  HunterCommonSensorState GetCommonSensorState() override {
+    auto common_sensor =
+        AgilexBase<ParserType>::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;
+    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;
+  }
   void ActivateBrake() override {
     AgilexBase<ParserType>::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
@@ -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
@@ -63,4 +63,8 @@ HunterActuatorState HunterRobot::GetActuatorState() {
   auto hunter = dynamic_cast<HunterInterface*>(robot_);
   return hunter->GetActuatorState();
 }
+HunterCommonSensorState HunterRobot::GetCommonSensorState() {
+  auto hunter = dynamic_cast<HunterInterface*>(robot_);
+  return hunter->GetCommonSensorState();
+}
 }  // namespace westonrobot
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