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]);
+}