battery: fix duplicate uORB publish and minor cleanup

- run battery_status module on adc_report publications rather than a fixed schedule

src/lib/battery/battery.cpp

@@ -49,9 +49,7 @@ using namespace time_literals;
 
 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
 	ModuleParams(parent),
-	_index(index < 1 || index > 9 ? 1 : index),
-	_warning(battery_status_s::BATTERY_WARNING_NONE),
-	_last_timestamp(0)
+	_index(index < 1 || index > 9 ? 1 : index)
 {
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
 	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
@@ -105,8 +103,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
 	updateParams();
 }
 
-void
-Battery::reset()
+void Battery::reset()
 {
 	memset(&_battery_status, 0, sizeof(_battery_status));
 	_battery_status.current_a = -1.f;
@@ -121,13 +118,10 @@ Battery::reset()
 	_battery_status.id = (uint8_t) _index;
 }
 
-void
-Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
-			     bool connected, int source, int priority,
-			     float throttle_normalized)
+void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
+				  int source, int priority, float throttle_normalized)
 {
 	reset();
-	_battery_status.timestamp = timestamp;
 
 	if (!_battery_initialized) {
 		_voltage_filter_v.reset(voltage_v);
@@ -169,24 +163,18 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		}
 	}
 
-	_battery_status.timestamp = timestamp;
-
-	const bool should_publish = (source == _params.source);
-
-	if (should_publish) {
-		_battery_status_pub.publish(_battery_status);
+	if (source == _params.source) {
 		publish();
 	}
 }
 
-void
-Battery::publish()
+void Battery::publish()
 {
+	_battery_status.timestamp = hrt_absolute_time();
 	_battery_status_pub.publish(_battery_status);
 }
 
-void
-Battery::sumDischarged(hrt_abstime timestamp, float current_a)
+void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
 {
 	// Not a valid measurement
 	if (current_a < 0.f) {
@@ -207,8 +195,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 	_last_timestamp = timestamp;
 }
 
-void
-Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
+void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
 {
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;
@@ -246,8 +233,7 @@ Battery::estimateRemaining(const float voltage_v, const float current_a, const f
 	}
 }
 
-void
-Battery::determineWarning(bool connected)
+void Battery::determineWarning(bool connected)
 {
 	if (connected) {
 		// propagate warning state only if the state is higher, otherwise remain in current warning state
@@ -266,8 +252,7 @@ Battery::determineWarning(bool connected)
 	}
 }
 
-void
-Battery::computeScale()
+void Battery::computeScale()
 {
 	const float voltage_range = (_params.v_charged - _params.v_empty);
 

src/lib/battery/battery.h

@@ -42,17 +42,18 @@
 
 #pragma once
 
-#include <uORB/uORB.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/battery_status.h>
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/module_params.h>
-#include <parameters/param.h>
-#include <board_config.h>
-#include <px4_platform_common/board_common.h>
 #include <math.h>
 #include <float.h>
+
+#include <board_config.h>
+#include <px4_platform_common/board_common.h>
+#include <px4_platform_common/module_params.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/parameters/param.h>
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/battery_status.h>
 
 /**
  * BatteryBase is a base class for any type of battery.
@@ -96,7 +97,7 @@ public:
 	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
 	 * @param throttle_normalized: Throttle of the vehicle, between 0 and 1
 	 */
-	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
+	void updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
 				 int source, int priority, float throttle_normalized);
 
 	/**
@@ -126,7 +127,7 @@ protected:
 		param_t v_load_drop_old;
 		param_t r_internal_old;
 		param_t source_old;
-	} _param_handles;
+	} _param_handles{};
 
 	struct {
 		float v_empty;
@@ -149,9 +150,9 @@ protected:
 		float v_load_drop_old;
 		float r_internal_old;
 		int source_old;
-	} _params;
+	} _params{};
 
-	battery_status_s _battery_status;
+	battery_status_s _battery_status{};
 
 	const int _index;
 
@@ -192,25 +193,25 @@ protected:
 		}
 	}
 
-	bool isFloatEqual(float a, float b) { return fabsf(a - b) > FLT_EPSILON; }
+	bool isFloatEqual(float a, float b) const { return fabsf(a - b) > FLT_EPSILON; }
 
 private:
-	void sumDischarged(hrt_abstime timestamp, float current_a);
+	void sumDischarged(const hrt_abstime &timestamp, float current_a);
 	void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
 	void determineWarning(bool connected);
 	void computeScale();
 
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
-	bool _battery_initialized = false;
+	bool _battery_initialized{false};
 	AlphaFilter<float> _voltage_filter_v;
 	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float> _throttle_filter;
-	float _discharged_mah = 0.f;
-	float _discharged_mah_loop = 0.f;
-	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
-	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
-	float _scale = 1.f;
-	uint8_t _warning;
-	hrt_abstime _last_timestamp;
+	float _discharged_mah{0.f};
+	float _discharged_mah_loop{0.f};
+	float _remaining_voltage{-1.f};		///< normalized battery charge level remaining based on voltage
+	float _remaining{-1.f};			///< normalized battery charge level, selected based on config param
+	float _scale{1.f};
+	uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
+	hrt_abstime _last_timestamp{0};
 };

src/modules/battery_status/battery_status.cpp

@@ -57,6 +57,7 @@
 #include <lib/battery/battery.h>
 #include <lib/conversion/rotation.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -101,7 +102,7 @@ private:
 
 	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+	uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
 
 	static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
 	static constexpr uint32_t SAMPLE_INTERVAL_US  = 1_s / SAMPLE_FREQUENCY_HZ;
@@ -280,9 +281,7 @@ BatteryStatus::task_spawn(int argc, char *argv[])
 bool
 BatteryStatus::init()
 {
-	ScheduleOnInterval(SAMPLE_INTERVAL_US);
-
-	return true;
+	return _adc_report_sub.registerCallback();
 }
 
 int BatteryStatus::custom_command(int argc, char *argv[])