From df0fe1ccc04b350e7ae8b0d9f70d98978aca4bc3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 12 Mar 2025 18:17:21 +0100 Subject: [PATCH 1/8] Add battery_info message with serial number compatible with UAVCAN, MAVLink and drivers I'm starting the separate battery info message because no char[32] should be published and logged at high rate and we need a separate battery info message for static information as discussed. --- msg/BatteryInfo.msg | 9 +++++ msg/CMakeLists.txt | 1 + src/modules/logger/logged_topics.cpp | 1 + src/modules/mavlink/streams/BATTERY_INFO.hpp | 39 ++++++++++++++------ 4 files changed, 38 insertions(+), 12 deletions(-) create mode 100644 msg/BatteryInfo.msg diff --git a/msg/BatteryInfo.msg b/msg/BatteryInfo.msg new file mode 100644 index 000000000000..eaff6b1add9a --- /dev/null +++ b/msg/BatteryInfo.msg @@ -0,0 +1,9 @@ +# Battery information +# +# Static or near-invariant battery information. +# Should be streamed at low rate. + +uint64 timestamp # [us] Time since system start + +uint8 id # Must match the id in the battery_status message for the same battery +char[32] serial_number # [@invalid 0 All bytes] Serial number of the battery pack in ASCII characters, 0 terminated diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e07888adb3a2..fdc23a889f11 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -47,6 +47,7 @@ set(msg_files Airspeed.msg AirspeedWind.msg AutotuneAttitudeControlStatus.msg + BatteryInfo.msg ButtonEvent.msg CameraCapture.msg CameraStatus.msg diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index faaf26dace46..597b4abf4e4e 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -51,6 +51,7 @@ void LoggedTopics::add_default_topics() add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); add_optional_topic("autotune_attitude_control_status", 100); + add_topic_multi("battery_info", 5000, 2); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); add_topic("cellular_status", 200); diff --git a/src/modules/mavlink/streams/BATTERY_INFO.hpp b/src/modules/mavlink/streams/BATTERY_INFO.hpp index fd72acce45ed..d541abb92a73 100644 --- a/src/modules/mavlink/streams/BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/BATTERY_INFO.hpp @@ -34,6 +34,7 @@ #ifndef BATTERY_INFO_HPP #define BATTERY_INFO_HPP +#include #include class MavlinkStreamBatteryInfo : public MavlinkStream @@ -57,21 +58,43 @@ class MavlinkStreamBatteryInfo : public MavlinkStream explicit MavlinkStreamBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _battery_info_subs{ORB_ID::battery_info}; + + uint8_t _serial_number_ids[battery_status_s::MAX_INSTANCES] {}; + char _serial_numbers[battery_status_s::MAX_INSTANCES][32] {}; ///< keep track of serial numbers until all static battery info is split out bool send() override { bool updated = false; + for (int i = 0; i < _battery_info_subs.size(); ++i) { + battery_info_s battery_info; + + if (_battery_info_subs[i].update(&battery_info)) { + _serial_number_ids[i] = battery_info.id; + memcpy(_serial_numbers[i], battery_info.serial_number, sizeof(_serial_numbers[0])); + } + } + for (auto &battery_sub : _battery_status_subs) { battery_status_s battery_status; if (battery_sub.update(&battery_status)) { - if (battery_status.serial_number == 0) { - // Required to emit - continue; + mavlink_battery_info_t msg{}; + bool battery_has_serial_number = false; + + // load serial number from battery_info message until all static information fields were moved + for (int i = 0; i < battery_status_s::MAX_INSTANCES; ++i) { + if ((_serial_number_ids[i] != 0) && (_serial_number_ids[i] == battery_status.id)) { + snprintf(msg.serial_number, sizeof(msg.serial_number), "%s", _serial_numbers[i]); + battery_has_serial_number = true; + } } - mavlink_battery_info_t msg{}; + if (!battery_has_serial_number) { + // Only publish BATTERY_INFO if the battery has a serial number + continue; + } msg.id = battery_status.id - 1; msg.design_capacity = (float)(battery_status.capacity * 1000); @@ -82,17 +105,9 @@ class MavlinkStreamBatteryInfo : public MavlinkStream uint16_t day = battery_status.manufacture_date % 32; uint16_t month = (battery_status.manufacture_date >> 5) % 16; uint16_t year = (80 + (battery_status.manufacture_date >> 9)); - uint16_t year2dig = year % 100; //Formatted as 'ddmmyyyy' (maxed 9 chars) snprintf(msg.manufacture_date, sizeof(msg.manufacture_date), "%d%d%d", day, month, year); - //Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars) - snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year2dig, - battery_status.serial_number); - - } else { - - snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number); } // Not supported by PX4 (not in battery_status uorb topic) From 33a6f016b324c5ab96ece73a5c087d87e5c0e052 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 May 2025 17:43:03 +0200 Subject: [PATCH 2/8] batt_smbus: switch to battery_info.serial_number --- src/drivers/batt_smbus/batt_smbus.cpp | 9 ++++++++- src/drivers/batt_smbus/batt_smbus.h | 3 ++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index da47a5090509..ffbdb7953a6f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,6 +77,7 @@ BATT_SMBUS::BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface) : BATT_SMBUS::~BATT_SMBUS() { + orb_unadvertise(_battery_info_topic); orb_unadvertise(_batt_topic); perf_free(_cycle); @@ -165,7 +166,6 @@ void BATT_SMBUS::RunImpl() if (ret == PX4_OK) { new_report.capacity = _batt_capacity; new_report.cycle_count = _cycle_count; - new_report.serial_number = _serial_number; new_report.max_cell_voltage_delta = _max_cell_voltage_delta; new_report.cell_count = _cell_count; new_report.state_of_health = _state_of_health; @@ -192,6 +192,13 @@ void BATT_SMBUS::RunImpl() int instance = 0; orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance); + battery_info_s battery_info{}; + battery_info.timestamp = new_report.timestamp; + battery_info.id = new_report.id; + snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number); + orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance); + + _last_report = new_report; } } diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index b2b1e0b702b9..a95d78871918 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -242,7 +243,7 @@ class BATT_SMBUS : public I2CSPIDriver /** @param _last_report Last published report, used for test(). */ battery_status_s _last_report{}; - /** @param _batt_topic uORB battery topic. */ + orb_advert_t _battery_info_topic{nullptr}; orb_advert_t _batt_topic{nullptr}; /** @param _cell_count Number of series cell. */ From b901b289778c9009fe6fb749b72a19c4a8673c38 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 May 2025 17:52:37 +0200 Subject: [PATCH 3/8] smbus_sbs: switch to battery_info.serial_number --- src/lib/drivers/smbus_sbs/SBS.hpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp index dc421b1ca7fe..4e112bd7e4d3 100644 --- a/src/lib/drivers/smbus_sbs/SBS.hpp +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -66,7 +67,7 @@ class SMBUS_SBS_BaseClass : public I2CSPIDriver friend SMBus; - int populate_smbus_data(battery_status_s &msg); + int populate_smbus_data(battery_status_s &msg, battery_info_s &battery_info); virtual void RunImpl(); // Can be overridden by derived implimentation @@ -136,7 +137,7 @@ class SMBUS_SBS_BaseClass : public I2CSPIDriver perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batmon_cycle")}; // TODO - /** @param _batt_topic uORB battery topic. */ + orb_advert_t _battery_info_topic{nullptr}; orb_advert_t _batt_topic{nullptr}; /** @param _cell_count Number of series cell (retrieved from cell_count PX4 params) */ @@ -173,8 +174,10 @@ SMBUS_SBS_BaseClass::SMBUS_SBS_BaseClass(const I2CSPIDriverConfig &config, SM I2CSPIDriver(config), _interface(interface) { + battery_info_s battery_info{}; battery_status_s new_report = {}; int SBS_instance_number = 0; + _battery_info_topic = orb_advertise_multi(ORB_ID(battery_info), &battery_info, &SBS_instance_number); _batt_topic = orb_advertise_multi(ORB_ID(battery_status), &new_report, &SBS_instance_number); _interface->init(); } @@ -251,7 +254,7 @@ int SMBUS_SBS_BaseClass::get_startup_info() } template -int SMBUS_SBS_BaseClass::populate_smbus_data(battery_status_s &data) +int SMBUS_SBS_BaseClass::populate_smbus_data(battery_status_s &data, battery_info_s &battery_info) { // Temporary variable for storing SMBUS reads. @@ -285,7 +288,7 @@ int SMBUS_SBS_BaseClass::populate_smbus_data(battery_status_s &data) // Read serial number. ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result); - data.serial_number = result; + snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, result); // Read battery temperature and covert to Celsius. ret |= _interface->read_word(BATT_SMBUS_TEMP, result); @@ -312,12 +315,17 @@ void SMBUS_SBS_BaseClass::RunImpl() new_report.connected = true; - int ret = populate_smbus_data(new_report); + battery_info_s battery_info{}; + battery_info.timestamp = now; + battery_info.id = new_report.id; + + int ret = populate_smbus_data(new_report, battery_info); new_report.cell_count = _cell_count; // Only publish if no errors. if (!ret) { orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); + orb_publish(ORB_ID(battery_info), _battery_info_topic, &battery_info); } } From a45c28671519cb986d5755cc3a8547807a87326e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 May 2025 18:10:28 +0200 Subject: [PATCH 4/8] batmon: switch to battery_info.serial_number --- src/drivers/smart_battery/batmon/batmon.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index ed424c1e18eb..a29c04aa28fe 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -191,7 +191,6 @@ void Batmon::RunImpl() if (ret == PX4_OK) { new_report.capacity = _batt_capacity; new_report.cycle_count = _cycle_count; - new_report.serial_number = _serial_number; new_report.max_cell_voltage_delta = _max_cell_voltage_delta; new_report.cell_count = _cell_count; new_report.state_of_health = _state_of_health; @@ -220,6 +219,12 @@ void Batmon::RunImpl() orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance); _last_report = new_report; + + battery_info_s battery_info{}; + battery_info.timestamp = new_report.timestamp; + battery_info.id = new_report.id; + snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number); + orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance); } } From 9652611c2e6682e2d3bee424ddf14087be6a7df5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 May 2025 18:05:57 +0200 Subject: [PATCH 5/8] uavcan battery: switch to battery_info.serial_number --- src/drivers/uavcan/sensors/battery.cpp | 14 ++++++++++++-- src/drivers/uavcan/sensors/battery.hpp | 6 ++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 0c8528670f75..0198749f73e3 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -125,7 +125,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructuregetBatteryStatus(); _battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius - _battery_status[instance].serial_number = msg.model_instance_id; _battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery publish(msg.getSrcNodeID().get(), &_battery_status[instance]); + + _battery_info[instance].timestamp = _battery_status[instance].timestamp; + _battery_info[instance].id = _battery_status[instance].id; + snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32, + msg.model_instance_id); + _battery_info_pub[instance].publish(_battery_info[instance]); } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index a27ad64505ab..cba57acfe799 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -38,6 +38,7 @@ #pragma once #include "sensor_bridge.hpp" +#include #include #include #include @@ -95,6 +96,11 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams float _discharged_mah_loop = 0.f; uint8_t _warning; hrt_abstime _last_timestamp; + + // Separate battery info publication because UavcanSensorBridgeBase only supports publishing one topic + uORB::PublicationMulti _battery_info_pub[battery_status_s::MAX_INSTANCES] {ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info)}; + + battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {}; battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {}; BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {}; From 8fc1c71b5855f475e526c6cb3155fd252bcfebd8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 3 Jun 2025 17:20:16 +0200 Subject: [PATCH 6/8] cyphal: switch to battery_info.serial_number --- .../cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp | 9 ++++++++- src/drivers/cyphal/Subscribers/udral/Battery.hpp | 9 ++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 0e15b8f91e8b..e0ea2d0aff22 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -94,7 +94,6 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE; bat_status.source = 1; // External bat_status.capacity = bat_info.full_charge_capacity_wh; - bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits bat_status.state_of_health = bat_info.state_of_health_pct; // External bat_status.id = bat_info.battery_id; @@ -118,9 +117,17 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber _battery_status_pub.publish(bat_status); print_message(ORB_ID(battery_status), bat_status); + + battery_info_s battery_info{}; + battery_info.timestamp = bat_status.timestamp; + battery_info.id = bat_status.id; + snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu32, + bat_info.model_instance_id); + _battery_info_pub.publish(battery_info); }; private: + uORB::PublicationMulti _battery_info_pub{ORB_ID(battery_info)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; }; diff --git a/src/drivers/cyphal/Subscribers/udral/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp index 0a8a6c798e1c..310bc9766325 100644 --- a/src/drivers/cyphal/Subscribers/udral/Battery.hpp +++ b/src/drivers/cyphal/Subscribers/udral/Battery.hpp @@ -41,6 +41,7 @@ #pragma once +#include #include #include @@ -120,6 +121,10 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber // TODO uORB publication rate limiting _battery_status_pub.publish(bat_status); + _battery_info.timestamp = bat_status.timestamp; + _battery_info.id = bat_status.id; + _battery_info_pub.publish(_battery_info); + } else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) { reg_udral_service_battery_Status_0_2 bat {}; size_t bat_size_in_bytes = receive.payload_size; @@ -163,7 +168,7 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh bat_status.cycle_count = parameters.cycle_count; - bat_status.serial_number = parameters.unique_id & 0xFFFF; + snprintf(_battery_info.serial_number, sizeof(_battery_info.serial_number), "%" PRIu64, parameters.unique_id); bat_status.state_of_health = parameters.state_of_health_pct; bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic bat_status.id = 0; //TODO instancing @@ -174,6 +179,7 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber private: float _nominal_voltage = NAN; + uORB::PublicationMulti _battery_info_pub{ORB_ID(battery_info)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; SubjectSubscription _status_sub; @@ -182,5 +188,6 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber const char *_status_name = "battery_status"; const char *_parameters_name = "battery_parameters"; + battery_info_s _battery_info{}; battery_status_s bat_status {0}; }; From 2ced7499aaee34f01fd891867de7b3e65d9d27b5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Jun 2025 18:47:17 +0200 Subject: [PATCH 7/8] msg translations: sort headers alphabetically --- msg/translation_node/translations/all_translations.h | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 22202033f17a..815c4657adbb 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -6,12 +6,8 @@ #include -//#include "example_translation_direct_v1.h" -//#include "example_translation_multi_v2.h" -//#include "example_translation_service_v1.h" - -#include "translation_vehicle_status_v1.h" #include "translation_airspeed_validated_v1.h" -#include "translation_vehicle_attitude_setpoint_v1.h" #include "translation_arming_check_reply_v1.h" #include "translation_event_v1.h" +#include "translation_vehicle_attitude_setpoint_v1.h" +#include "translation_vehicle_status_v1.h" From 046eb10ecfb659d564a83f195cff9d49a9c3c9b9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 May 2025 17:14:17 +0200 Subject: [PATCH 8/8] battery_status message: remove serial_number which is now in battery_info message --- msg/px4_msgs_old/msg/BatteryStatusV0.msg | 79 ++++++++++++ .../translations/all_translations.h | 1 + .../translation_battery_status_v1.h | 114 ++++++++++++++++++ msg/versioned/BatteryStatus.msg | 3 +- 4 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 msg/px4_msgs_old/msg/BatteryStatusV0.msg create mode 100644 msg/translation_node/translations/translation_battery_status_v1.h diff --git a/msg/px4_msgs_old/msg/BatteryStatusV0.msg b/msg/px4_msgs_old/msg/BatteryStatusV0.msg new file mode 100644 index 000000000000..f8f9e3f12dbe --- /dev/null +++ b/msg/px4_msgs_old/msg/BatteryStatusV0.msg @@ -0,0 +1,79 @@ +# Battery status +# +# Battery status information for up to 4 battery instances. +# These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. +# Battery instance information is also logged and streamed in MAVLink telemetry. + +uint32 MESSAGE_VERSION = 0 +uint8 MAX_INSTANCES = 4 + +uint64 timestamp # [us] Time since system start +bool connected # Whether or not a battery is connected. For power modules this is based on a voltage threshold. +float32 voltage_v # [V] [@invalid 0] Battery voltage +float32 current_a # [A] [@invalid -1] Battery current +float32 current_average_a # [A] [@invalid -1] Battery current average (for FW average in level flight) +float32 discharged_mah # [mAh] [@invalid -1] Discharged amount +float32 remaining # [@range 0,1] [@invalid -1] Remaining capacity +float32 scale # [@range 1,] [@invalid -1] Scaling factor to compensate for lower actuation power caused by voltage sag +float32 time_remaining_s # [s] [@invalid NaN] Predicted time remaining until battery is empty under previous averaged load +float32 temperature # [°C] [@invalid NaN] Temperature of the battery +uint8 cell_count # [@invalid 0] Number of cells + + +uint8 source # [@enum SOURCE] Battery source +uint8 SOURCE_POWER_MODULE = 0 # Power module +uint8 SOURCE_EXTERNAL = 1 # External +uint8 SOURCE_ESCS = 2 # ESCs + +uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 +uint16 capacity # [mAh] Capacity of the battery when fully charged +uint16 cycle_count # Number of discharge cycles the battery has experienced +uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge +uint16 serial_number # Serial number of the battery pack +uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512 +uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity +uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation +uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed +uint16 interface_error # Interface error counter + +float32[14] voltage_cell_v # [V] [@invalid 0] Battery individual cell voltages +float32 max_cell_voltage_delta # Max difference between individual cell voltages + +bool is_powering_off # Power off event imminent indication, false if unknown +bool is_required # Set if the battery is explicitly required before arming + +uint8 warning # [@enum WARNING STATE] Current battery warning +uint8 WARNING_NONE = 0 # No battery low voltage warning active +uint8 WARNING_LOW = 1 # Low voltage warning +uint8 WARNING_CRITICAL = 2 # Critical voltage, return / abort immediately +uint8 WARNING_EMERGENCY = 3 # Immediate landing required +uint8 WARNING_FAILED = 4 # Battery has failed completely +uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field +uint8 STATE_CHARGING = 7 # Battery is charging + +uint16 faults # [@enum FAULT] Smart battery supply status/fault flags (bitmask) for health indication +uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 FAULT_SPIKES = 1 # Voltage spikes +uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 FAULT_OVER_CURRENT = 3 # Over-current +uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage) +uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 FAULT_HARDWARE_FAILURE = 9 # Hardware problem +uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming +uint8 FAULT_COUNT = 11 # Counter. Keep this as last element + +float32 full_charge_capacity_wh # [Wh] Compensated battery capacity +float32 remaining_capacity_wh # [Wh] Compensated battery capacity remaining +uint16 over_discharge_count # Number of battery overdischarge +float32 nominal_voltage # [V] Nominal voltage of the battery pack + +float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate +float32 ocv_estimate # [V] Open circuit voltage estimate +float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate +float32 volt_based_soc_estimate # [@range 0, 1] Normalized volt based state of charge estimate +float32 voltage_prediction # [V] Predicted voltage +float32 prediction_error # [V] Prediction error +float32 estimation_covariance_norm # Norm of the covariance matrix diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 815c4657adbb..32828db00795 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -8,6 +8,7 @@ #include "translation_airspeed_validated_v1.h" #include "translation_arming_check_reply_v1.h" +#include "translation_battery_status_v1.h" #include "translation_event_v1.h" #include "translation_vehicle_attitude_setpoint_v1.h" #include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/translation_battery_status_v1.h b/msg/translation_node/translations/translation_battery_status_v1.h new file mode 100644 index 000000000000..5d09b3cbb195 --- /dev/null +++ b/msg/translation_node/translations/translation_battery_status_v1.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate BatteryStatus v0 <--> v1 +#include +#include + +class BatteryStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::BatteryStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::BatteryStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/battery_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.connected = msg_older.connected; + msg_newer.voltage_v = msg_older.voltage_v; + msg_newer.current_a = msg_older.current_a; + msg_newer.current_average_a = msg_older.current_average_a; + msg_newer.discharged_mah = msg_older.discharged_mah; + msg_newer.remaining = msg_older.remaining; + msg_newer.scale = msg_older.scale; + msg_newer.time_remaining_s = msg_older.time_remaining_s; + msg_newer.temperature = msg_older.temperature; + msg_newer.cell_count = msg_older.cell_count; + msg_newer.source = msg_older.source; + msg_newer.priority = msg_older.priority; + msg_newer.capacity = msg_older.capacity; + msg_newer.cycle_count = msg_older.cycle_count; + msg_newer.average_time_to_empty = msg_older.average_time_to_empty; + // The serial number moved to the battery_info message and is char[32] instead of uint16 + msg_newer.manufacture_date = msg_older.manufacture_date; + msg_newer.state_of_health = msg_older.state_of_health; + msg_newer.max_error = msg_older.max_error; + msg_newer.id = msg_older.id; + msg_newer.interface_error = msg_older.interface_error; + + for (int i = 0; i < 14; ++i) { + msg_newer.voltage_cell_v[i] = msg_older.voltage_cell_v[i]; + } + + msg_newer.max_cell_voltage_delta = msg_older.max_cell_voltage_delta; + msg_newer.is_powering_off = msg_older.is_powering_off; + msg_newer.is_required = msg_older.is_required; + msg_newer.warning = msg_older.warning; + msg_newer.faults = msg_older.faults; + msg_newer.full_charge_capacity_wh = msg_older.full_charge_capacity_wh; + msg_newer.remaining_capacity_wh = msg_older.remaining_capacity_wh; + msg_newer.over_discharge_count = msg_older.over_discharge_count; + msg_newer.nominal_voltage = msg_older.nominal_voltage; + msg_newer.internal_resistance_estimate = msg_older.internal_resistance_estimate; + msg_newer.ocv_estimate = msg_older.ocv_estimate; + msg_newer.ocv_estimate_filtered = msg_older.ocv_estimate_filtered; + msg_newer.volt_based_soc_estimate = msg_older.volt_based_soc_estimate; + msg_newer.voltage_prediction = msg_older.voltage_prediction; + msg_newer.prediction_error = msg_older.prediction_error; + msg_newer.estimation_covariance_norm = msg_older.estimation_covariance_norm; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.connected = msg_newer.connected; + msg_older.voltage_v = msg_newer.voltage_v; + msg_older.current_a = msg_newer.current_a; + msg_older.current_average_a = msg_newer.current_average_a; + msg_older.discharged_mah = msg_newer.discharged_mah; + msg_older.remaining = msg_newer.remaining; + msg_older.scale = msg_newer.scale; + msg_older.time_remaining_s = msg_newer.time_remaining_s; + msg_older.temperature = msg_newer.temperature; + msg_older.cell_count = msg_newer.cell_count; + msg_older.source = msg_newer.source; + msg_older.priority = msg_newer.priority; + msg_older.capacity = msg_newer.capacity; + msg_older.cycle_count = msg_newer.cycle_count; + msg_older.average_time_to_empty = msg_newer.average_time_to_empty; + msg_older.serial_number = 0; // The serial number moved to the battery_info message and is char[32] instead of uint16 + msg_older.manufacture_date = msg_newer.manufacture_date; + msg_older.state_of_health = msg_newer.state_of_health; + msg_older.max_error = msg_newer.max_error; + msg_older.id = msg_newer.id; + msg_older.interface_error = msg_newer.interface_error; + + for (int i = 0; i < 14; ++i) { + msg_older.voltage_cell_v[i] = msg_newer.voltage_cell_v[i]; + } + + msg_older.max_cell_voltage_delta = msg_newer.max_cell_voltage_delta; + msg_older.is_powering_off = msg_newer.is_powering_off; + msg_older.is_required = msg_newer.is_required; + msg_older.warning = msg_newer.warning; + msg_older.faults = msg_newer.faults; + msg_older.full_charge_capacity_wh = msg_newer.full_charge_capacity_wh; + msg_older.remaining_capacity_wh = msg_newer.remaining_capacity_wh; + msg_older.over_discharge_count = msg_newer.over_discharge_count; + msg_older.nominal_voltage = msg_newer.nominal_voltage; + msg_older.internal_resistance_estimate = msg_newer.internal_resistance_estimate; + msg_older.ocv_estimate = msg_newer.ocv_estimate; + msg_older.ocv_estimate_filtered = msg_newer.ocv_estimate_filtered; + msg_older.volt_based_soc_estimate = msg_newer.volt_based_soc_estimate; + msg_older.voltage_prediction = msg_newer.voltage_prediction; + msg_older.prediction_error = msg_newer.prediction_error; + msg_older.estimation_covariance_norm = msg_newer.estimation_covariance_norm; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(BatteryStatusV1Translation); diff --git a/msg/versioned/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg index f8f9e3f12dbe..39f75ed61189 100644 --- a/msg/versioned/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -4,7 +4,7 @@ # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. # Battery instance information is also logged and streamed in MAVLink telemetry. -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint8 MAX_INSTANCES = 4 uint64 timestamp # [us] Time since system start @@ -29,7 +29,6 @@ uint8 priority # Zero based priority is the connection on the Power Controller V uint16 capacity # [mAh] Capacity of the battery when fully charged uint16 cycle_count # Number of discharge cycles the battery has experienced uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge -uint16 serial_number # Serial number of the battery pack uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512 uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation