Re-add filter functionality

src/drivers/uavcan/sensors/battery.cpp

@@ -57,7 +57,10 @@ int UavcanBatteryBridge::init()
 
 	for (uint8_t instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
 
-		if (uavcan_sub_bat == RAW_DATA) {
+		if (uavcan_sub_bat == FILTER_DATA) {
+			_batt_update_mod[instance] = BatteryDataType::Filter;
+
+		} else if (uavcan_sub_bat == RAW_DATA) {
 			_batt_update_mod[instance] = BatteryDataType::Raw;
 
 		} else if (uavcan_sub_bat == RAW_AUX_DATA) {
@@ -107,6 +110,12 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 		return;
 	}
 
+	if (_batt_update_mod[instance] == BatteryDataType::Filter) {
+
+		filterData(msg, instance);
+		return;
+	}
+
 	_battery_status[instance].timestamp = hrt_absolute_time();
 	_battery_status[instance].voltage_v = msg.voltage;
 	_battery_status[instance].current_a = msg.current;
@@ -169,6 +178,12 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 		return;
 	}
 
+	if (_batt_update_mod[instance] == BatteryDataType::Filter) {
+		return;
+	}
+
+	_batt_update_mod[instance] = BatteryDataType::RawAux;
+
 	_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
 			_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
 			1000;
@@ -258,3 +273,21 @@ UavcanBatteryBridge::determineWarning(float remaining)
 		_warning = battery_status_s::BATTERY_WARNING_LOW;
 	}
 }
+
+void
+UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
+				uint8_t instance)
+{
+	_battery[instance]->setConnected(true);
+	_battery[instance]->updateVoltage(msg.voltage);
+	_battery[instance]->updateCurrent(msg.current);
+	_battery[instance]->updateBatteryStatus(hrt_absolute_time());
+
+	/* Override data that is expected to arrive from UAVCAN msg*/
+	_battery_status[instance] = _battery[instance]->getBatteryStatus();
+	_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]);
+}

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};
 };

src/drivers/uavcan/uavcan_params.c

@@ -300,8 +300,9 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
  * @max 2
  * @value 0 Disable
  * @value 1 Raw data
- * @value 2 Raw and auxiliary data
- * @value 3 Raw, auxiliary, and CBAT-specific data
+ * @value 2 Filter data
+ * @value 3 Raw and auxiliary data
+ * @value 4 Raw, auxiliary, and CBAT-specific data
  * @reboot_required true
  * @group UAVCAN
  */