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 ada44e18..61902c49 100644 --- 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 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 4e83793c..d3073512 100644 --- 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, 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; + 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::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 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