Merge pull request westonrobot#45 from westonrobot/feature-bms_extend

Feature bms extend

include/ugv_sdk/details/interface/agilex_message.h

@@ -230,12 +230,10 @@ typedef struct {
 #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;
+  uint8_t alarm_status_1;
+  uint8_t alarm_status_2;
+  uint8_t warn_status_1;
+  uint8_t warn_status_2;
 } BmsExtendedMessage;
 
 /************  Query/config messages ****************/

include/ugv_sdk/details/interface/hunter_interface.hpp

@@ -34,6 +34,13 @@ struct HunterActuatorState {
   ActuatorStateMessageV1 actuator_state[3];
 };
 
+struct HunterCommonSensorState {
+  SdkTimePoint time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+  BmsExtendedMessage bms_extend_state;
+};
+
 struct HunterInterface {
   virtual ~HunterInterface() = default;
 
@@ -45,6 +52,7 @@ struct HunterInterface {
   // get robot state
   virtual HunterCoreState GetRobotState() = 0;
   virtual HunterActuatorState GetActuatorState() = 0;
+  virtual HunterCommonSensorState GetCommonSensorState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -27,7 +27,6 @@ struct RangerCoreState {
 
   RcStateMessage rc_state;
   OdometryMessage odometry;
-  BmsBasicMessage bms_basic_state;
 };
 
 struct RangerActuatorState {
@@ -40,6 +39,12 @@ struct RangerActuatorState {
   ActuatorLSStateMessage actuator_ls_state[8];
 };
 
+struct RangerCommonSensorState {
+  SdkTimePoint time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+};
+
 /////////////////////////////////////////////////////////////////////////
 
 struct RangerInterface {
@@ -64,6 +69,7 @@ struct RangerInterface {
   // get robot state
   virtual RangerCoreState GetRobotState() = 0;
   virtual RangerActuatorState GetActuatorState() = 0;
+  virtual RangerCommonSensorState GetCommonSensorState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -23,7 +23,6 @@ struct ScoutCoreState {
   MotionStateMessage motion_state;
   LightStateMessage light_state;
   RcStateMessage rc_state;
-  BmsBasicMessage bms_basic_state;
 };
 
 struct ScoutActuatorState {
@@ -37,6 +36,12 @@ struct ScoutActuatorState {
   ActuatorStateMessageV1 actuator_state[4];
 };
 
+struct ScoutCommonSensorState {
+  SdkTimePoint time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+};
+
 struct ScoutInterface {
   virtual ~ScoutInterface() = default;
 

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -31,8 +31,6 @@ struct CoreStateMsgGroup {
   LightStateMessage light_state;
   MotionModeStateMessage motion_mode_state;
   RcStateMessage rc_state;
-
-  BmsBasicMessage bms_basic_state;
 };
 
 struct ActuatorStateMsgGroup {
@@ -46,8 +44,10 @@ struct ActuatorStateMsgGroup {
   MotorSpeedMessage motor_speeds;  // ranger only
 };
 
-struct SensorStateMsgGroup {
+struct CommonSensorStateMsgGroup {
   SdkTimePoint time_stamp;
+  BmsBasicMessage bms_basic_state;
+  BmsExtendedMessage bms_extend_state;
 };
 
 template <typename ParserType>
@@ -183,7 +183,7 @@ class AgilexBase : public RobotCommonInterface {
     return actuator_state_msgs_;
   }
 
-  SensorStateMsgGroup GetSensorStateMsgGroup() {
+  CommonSensorStateMsgGroup GetCommonSensorStateMsgGroup() {
     std::lock_guard<std::mutex> guard(common_sensor_state_mtx_);
     return common_sensor_state_msgs_;
   }
@@ -205,7 +205,7 @@ class AgilexBase : public RobotCommonInterface {
 
   /* feedback group 3: common sensor */
   std::mutex common_sensor_state_mtx_;
-  SensorStateMsgGroup common_sensor_state_msgs_;
+  CommonSensorStateMsgGroup common_sensor_state_msgs_;
 
   std::mutex version_str_buf_mtx_;
   std::string version_string_buffer_;
@@ -279,7 +279,7 @@ class AgilexBase : public RobotCommonInterface {
     if (parser_.DecodeMessage(rx_frame, &status_msg)) {
       UpdateRobotCoreState(status_msg);
       UpdateActuatorState(status_msg);
-      UpdateSensorState(status_msg);
+      UpdateCommonSensorState(status_msg);
       UpdateResponseVersion(status_msg);
     }
   }
@@ -318,12 +318,6 @@ class AgilexBase : public RobotCommonInterface {
         core_state_msgs_.rc_state = status_msg.body.rc_state_msg;
         break;
       }
-      case AgxMsgBmsBasic: {
-        //      std::cout << "system status feedback received" << std::endl;
-        core_state_msgs_.time_stamp = SdkClock::now();
-        core_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg;
-        break;
-      }
       default:
         break;
     }
@@ -381,11 +375,22 @@ class AgilexBase : public RobotCommonInterface {
     }
   }
 
-  void UpdateSensorState(const AgxMessage &status_msg) {
+  void UpdateCommonSensorState(const AgxMessage &status_msg) {
     std::lock_guard<std::mutex> guard(common_sensor_state_mtx_);
     //    std::cout << common_sensor_state_msgs_.bms_basic_state.battery_soc<<
     //    std::endl;
     switch (status_msg.type) {
+      case AgxMsgBmsBasic: {
+        //      std::cout << "system status feedback received" << std::endl;
+        common_sensor_state_msgs_.time_stamp = SdkClock::now();
+        common_sensor_state_msgs_.bms_basic_state =
+            status_msg.body.bms_basic_msg;
+        break;
+      }
+      case AgxMsgBmsExtended: {
+        common_sensor_state_msgs_.bms_extend_state = 
+          status_msg.body.bms_extended_msg;
+      }
       default:
         break;
     }

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);
   }

include/ugv_sdk/details/robot_base/ranger_base.hpp

@@ -63,18 +63,6 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     ranger_state.rc_state = state.rc_state;
     ranger_state.motion_mode_state = state.motion_mode_state;
 
-    ranger_state.bms_basic_state.current = state.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.
-    ranger_state.bms_basic_state.voltage = state.bms_basic_state.voltage * 0.1f;
-    ranger_state.bms_basic_state.battery_soc =
-        state.bms_basic_state.battery_soc;
-    ranger_state.bms_basic_state.battery_soh =
-        state.bms_basic_state.battery_soh;
-    ranger_state.bms_basic_state.temperature =
-        state.bms_basic_state.temperature;
-
     return ranger_state;
   }
 
@@ -99,6 +87,30 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     }
     return ranger_actuator;
   }
+
+  RangerCommonSensorState GetCommonSensorState() override {
+    auto common_sensor =
+        AgilexBase<ProtocolV2Parser>::GetCommonSensorStateMsgGroup();
+
+    RangerCommonSensorState ranger_bms;
+
+    ranger_bms.time_stamp = common_sensor.time_stamp;
+
+    ranger_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.
+    ranger_bms.bms_basic_state.voltage =
+        common_sensor.bms_basic_state.voltage * 0.1f;
+    ranger_bms.bms_basic_state.battery_soc =
+        common_sensor.bms_basic_state.battery_soc;
+    ranger_bms.bms_basic_state.battery_soh =
+        common_sensor.bms_basic_state.battery_soh;
+    ranger_bms.bms_basic_state.temperature =
+        common_sensor.bms_basic_state.temperature;
+
+    return ranger_bms;
+  }
 };
 
 // Note: Ranger Mini V1 uses a modified AgileX V2 protocol

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -56,7 +56,6 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     scout_state.motion_state = state.motion_state;
     scout_state.light_state = state.light_state;
     scout_state.rc_state = state.rc_state;
-    scout_state.bms_basic_state = state.bms_basic_state;
     return scout_state;
   }
 

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_;

include/ugv_sdk/mobile_robot/ranger_robot.hpp

@@ -44,6 +44,7 @@ class RangerRobot : public RobotCommonInterface, public RangerInterface {
   // get robot state
   RangerCoreState GetRobotState() override;
   RangerActuatorState GetActuatorState() override;
+  RangerCommonSensorState GetCommonSensorState() override;
 
  private:
   RobotCommonInterface* robot_;

sample/ranger_demo/ranger_robot_demo.cpp

@@ -49,6 +49,7 @@ int main(int argc, char *argv[]) {
 
     auto state = ranger->GetRobotState();
     auto motion = ranger->GetActuatorState();
+    auto sensor = ranger->GetCommonSensorState();
 
     std::cout << "-------------------------------" << std::endl;
     std::cout << "count: " << count << std::endl;
@@ -58,9 +59,10 @@ int main(int argc, char *argv[]) {
               << static_cast<int>(state.system_state.vehicle_state)
               << std::endl;
     std::cout << "battery voltage: " << state.system_state.battery_voltage
-              << ", battery current: " << state.bms_basic_state.current
+              << "battery voltage (BMS): " << sensor.bms_basic_state.voltage
+              << ", battery current: " << sensor.bms_basic_state.current
               << ", SOC: "
-              << static_cast<int>(state.bms_basic_state.battery_soc)
+              << static_cast<int>(sensor.bms_basic_state.battery_soc)
               << std::endl;
     std::cout << "velocity (linear, angular, lateral, steering): "
               << std::setw(6) << state.motion_state.linear_velocity << ", "

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

src/mobile_robot/ranger_robot.cpp

@@ -67,4 +67,8 @@ RangerActuatorState RangerRobot::GetActuatorState() {
   auto ranger = dynamic_cast<RangerInterface*>(robot_);
   return ranger->GetActuatorState();
 }
+RangerCommonSensorState RangerRobot::GetCommonSensorState() {
+  auto ranger = dynamic_cast<RangerInterface*>(robot_);
+  return ranger->GetCommonSensorState();
+}
 }  // namespace westonrobot

src/protocol_v2/agilex_msg_parser_v2.c

@@ -277,14 +277,10 @@ 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.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 **************/

src/protocol_v2/agilex_protocol_v2.h

@@ -316,13 +316,14 @@ typedef struct {
 } 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;
+  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