From d7ab21b8d68cc087c6c542fdb19257b2ec91a44a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 12 Mar 2025 18:17:21 +0100 Subject: [PATCH] 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 0000000000..eaff6b1add --- /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 e07888adb3..fdc23a889f 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 faaf26dace..597b4abf4e 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 fd72acce45..d541abb92a 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 @@ private: 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 @@ private: 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)