Re-add Battery instances for smart batteries

PX4 · Jan 31, 2025 · 3b83bb4 · 3b83bb4
1 parent bdbf717
commit 3b83bb4
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp
@@ -65,6 +65,7 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 	enum class BatteryDataType {
 		Raw, // data from BatteryInfo message only
 		RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
+		Filter, // filter data from BatteryInfo message with Battery library
 		RawAuxCBAT, // data combination from BatteryInfo, BatteryInfoAux, and CBAT messages
 	};
 
@@ -73,6 +74,7 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 	void cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
 	void sumDischarged(hrt_abstime timestamp, float current_a);
 	void determineWarning(float remaining);
+	void filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg, uint8_t instance);
 
 	typedef uavcan::MethodBinder < UavcanBatteryBridge *,
 		void (UavcanBatteryBridge::*)
@@ -104,8 +106,20 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 	battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
 	BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};
 
+	static constexpr int FILTER_DATA = 2;
+	static constexpr int BATTERY_INDEX_1 = 1;
+	static constexpr int BATTERY_INDEX_2 = 2;
+	static constexpr int BATTERY_INDEX_3 = 3;
 	static constexpr int RAW_DATA = 1;
 	static constexpr int RAW_AUX_DATA = 2;
 	static constexpr int RAW_AUX_CBAT_DATA = 3;
 	static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
+
+	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::BATTERY_SOURCE_EXTERNAL};
+	Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
+	Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
+
+	Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3};
 };