From d3acee315a9ed36de70dd06e84e4fc5a6a198740 Mon Sep 17 00:00:00 2001 From: Claudio Chies Date: Wed, 17 Sep 2025 08:48:36 +0200 Subject: [PATCH] BAT: Consolidate the highest feasible number of batteries into just 3 --- .../nuttx-config/scripts/itcm_functions_includes.ld | 2 +- .../nuttx-config/scripts/itcm_functions_includes.ld | 2 +- msg/versioned/BatteryStatus.msg | 2 +- .../common/include/px4_platform_common/board_common.h | 4 ---- src/drivers/uavcan/sensors/battery.hpp | 8 +++----- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 2 +- 6 files changed, 7 insertions(+), 13 deletions(-) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld index 0e6701aae2..7102c196ae 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -394,7 +394,7 @@ *(.text._ZN4EKF215PublishBaroBiasERKy) *(.text._ZN4EKF221UpdateGyroCalibrationERKy) *(.text._ZN6matrix9constrainIfLj3ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) -*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh4EE16advertised_countEv) +*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh3EE16advertised_countEv) *(.text._ZN23MavlinkStreamScaledIMU34sendEv) *(.text.__aeabi_ldivmod) *(.text._ZN15PositionControl16setInputSetpointERK21trajectory_setpoint_s) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index a52a3b46b0..e6f5d02d04 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -402,7 +402,7 @@ *(.text._ZN4EKF215PublishBaroBiasERKy) *(.text._ZN4EKF221UpdateGyroCalibrationERKy) *(.text._ZN6matrix9constrainIfLj3ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) -*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh4EE16advertised_countEv) +*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh3EE16advertised_countEv) *(.text._ZN23MavlinkStreamScaledIMU34sendEv) *(.text.__aeabi_ldivmod) *(.text._ZN15PositionControl16setInputSetpointERK21trajectory_setpoint_s) diff --git a/msg/versioned/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg index 94926ac3a2..7c390c4576 100644 --- a/msg/versioned/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -5,7 +5,7 @@ # Battery instance information is also logged and streamed in MAVLink telemetry. uint32 MESSAGE_VERSION = 1 -uint8 MAX_INSTANCES = 4 +uint8 MAX_INSTANCES = 3 uint64 timestamp # [us] Time since system start diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 4b9678afd6..8905507cd0 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -173,10 +173,6 @@ # define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL, ADC_BATTERY3_VOLTAGE_CHANNEL} # define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL, ADC_BATTERY3_CURRENT_CHANNEL} # define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID, BOARD_ADC_BRICK3_VALID} -#elif BOARD_NUMBER_BRICKS == 4 -# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL, ADC_BATTERY3_VOLTAGE_CHANNEL, ADC_BATTERY4_VOLTAGE_CHANNEL} -# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL, ADC_BATTERY3_CURRENT_CHANNEL, ADC_BATTERY4_CURRENT_CHANNEL} -# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID, BOARD_ADC_BRICK3_VALID, BOARD_ADC_BRICK4_VALID} #else # error Unsuported BOARD_NUMBER_BRICKS number. #endif diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index 0c0ab3c949..654486d681 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -105,7 +105,7 @@ private: 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)}; + uORB::PublicationMulti _battery_info_pub[battery_status_s::MAX_INSTANCES] {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] {}; @@ -115,15 +115,13 @@ private: static constexpr int BATTERY_INDEX_1 = 1; static constexpr int BATTERY_INDEX_2 = 2; static constexpr int BATTERY_INDEX_3 = 3; - static constexpr int BATTERY_INDEX_4 = 4; static constexpr int SAMPLE_INTERVAL_US = 500_ms; // Typical message rate for a CAN battery monitor should be 2-5Hz. - static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big"); + static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_3, "Battery array too big"); Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; - Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; - Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 }; + Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3}; }; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 8723f14131..74ca48d554 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -685,7 +685,7 @@ private: hrt_abstime _last_update_time{0}; float _update_rate_filtered{0}; - PerBatteryData _batteries[battery_status_s::MAX_INSTANCES] {0, 1, 2, 3}; + PerBatteryData _batteries[battery_status_s::MAX_INSTANCES] {0, 1, 2}; }; #endif // HIGH_LATENCY2_HPP