UAVCAN:BAT: improve remaining time calculation (#25500)

* UAVCAN:BAT: improve remaining time calculation

* UAVCAN:BAT: fix time_remaining calculation, bugfixes, improved filter convergence time

* UAVCAN:BAT: remove BatteryInfo Publishing if no valid info

* UAVCAN + Battery library: suggestions while reviewing

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>

src/drivers/uavcan/sensors/battery.cpp

@@ -112,28 +112,22 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	}
 
 	_battery_status[instance].timestamp = hrt_absolute_time();
+	_battery[instance]->updateDt(_battery_status[instance].timestamp);
 	_battery_status[instance].voltage_v = msg.voltage;
 	_battery_status[instance].current_a = msg.current;
-	_battery_status[instance].current_average_a = msg.current;
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
-		sumDischarged(_battery_status[instance].timestamp, _battery_status[instance].current_a);
-		_battery_status[instance].discharged_mah = _discharged_mah;
+		_battery_status[instance].discharged_mah = _battery[instance]->sumDischarged(fabsf(msg.current));
 		_battery_status[instance].time_remaining_s = NAN;
 	}
 
 	_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
 	_battery_status[instance].scale = -1.f;
 	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
-	// _battery_status[instance].cell_count = msg.;
 	_battery_status[instance].connected = true;
 	_battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
-	// _battery_status[instance].priority = msg.;
-	_battery_status[instance].capacity = msg.full_charge_capacity_wh;
 	_battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
 	_battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
-	// _battery_status[instance].cycle_count = msg.;
-	// _battery_status[instance].average_time_to_empty = msg.;
 	_battery_status[instance].id = msg.getSrcNodeID().get();
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -144,21 +138,18 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 		_battery_status[instance].cell_count = 1;
 	}
 
-	// _battery_status[instance].max_cell_voltage_delta = msg.;
-
-	// _battery_status[instance].is_powering_off = msg.;
-
-	determineWarning(_battery_status[instance].remaining);
-	_battery_status[instance].warning = _warning;
-
-	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
-	_battery_info[instance].id = _battery_status[instance].id;
-	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
-		 msg.model_instance_id);
+	_battery_status[instance].warning = _battery[instance]->determineWarning(_battery_status[instance].remaining);
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
-		_battery_info_pub[instance].publish(_battery_info[instance]);
+
+		if (msg.model_instance_id > 0) {
+			_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+			_battery_info[instance].id = _battery_status[instance].id;
+			snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number),
+				 "%" PRIu32, msg.model_instance_id);
+			_battery_info_pub[instance].publish(_battery_info[instance]);
+		}
 	}
 }
 
@@ -182,18 +173,24 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 
 	_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;
 	_battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14);
 	_battery_status[instance].cycle_count = msg.cycle_count;
 	_battery_status[instance].over_discharge_count = msg.over_discharge_count;
 	_battery_status[instance].nominal_voltage = msg.nominal_voltage;
-	_battery_status[instance].time_remaining_s = math::isZero(_battery_status[instance].current_a) ? NAN :
-			(_battery_status[instance].remaining_capacity_wh /
-			 _battery_status[instance].nominal_voltage / _battery_status[instance].current_a * 3600);
 	_battery_status[instance].is_powering_off = msg.is_powering_off;
 
+	if (msg.nominal_voltage > FLT_EPSILON) {
+		_battery_status[instance].capacity =
+			_battery_status[instance].full_charge_capacity_wh * 1000.f / msg.nominal_voltage;
+	}
+
+	_battery[instance]->setCapacityMah(_battery_status[instance].capacity);
+	_battery[instance]->setStateOfCharge(_battery_status[instance].remaining);
+	// Absolute value of current as sign not clearly defined and vendors are inconsistent
+	_battery_status[instance].time_remaining_s =
+		_battery[instance]->computeRemainingTime(fabsf(_battery_status[instance].current_a));
+	_battery_status[instance].current_average_a = _battery[instance]->getCurrentAverage();
+
 	for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) {
 		_battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
 	}
@@ -234,7 +231,7 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
 		msg.full_charge_capacity * msg.nominal_voltage / 1000.f; // mAh -> Wh
 	_battery_status[instance].remaining_capacity_wh = msg.remaining_capacity * msg.nominal_voltage / 1000.f; // mAh -> Wh
 	_battery_status[instance].nominal_voltage = msg.nominal_voltage;
-	_battery_status[instance].capacity = msg.design_capacity; // mAh
+	_battery_status[instance].capacity = msg.full_charge_capacity; // mAh
 	_battery_status[instance].cycle_count = msg.cycle_count;
 	_battery_status[instance].average_time_to_empty = msg.average_time_to_empty;
 	_battery_status[instance].manufacture_date = msg.manufacture_date;
@@ -247,19 +244,18 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
 	_battery_status[instance].id = msg.getSrcNodeID().get();
 	_battery_status[instance].is_powering_off = msg.is_powering_off;
 
-	// For time remaining calculation use the average current if supplied
-	const float remaining_ah = msg.remaining_capacity / 1000.f; // mAh -> Ah
-	const float current_a = math::isZero(_battery_status[instance].current_average_a) ?
-				_battery_status[instance].current_a : _battery_status[instance].current_average_a;
+	// use Battery class for time_remaining calculation
+	_battery[instance]->updateDt(_battery_status[instance].timestamp);
+	_battery[instance]->setStateOfCharge(_battery_status[instance].remaining);
+	_battery[instance]->setCapacityMah(_battery_status[instance].capacity);
 	_battery_status[instance].time_remaining_s =
-		math::isZero(current_a) ? NAN : (remaining_ah / current_a * 3600.f); // Ah / A = h * 3600 = s
+		_battery[instance]->computeRemainingTime(_battery_status[instance].current_a);
 
 	for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) {
 		_battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
 	}
 
-	determineWarning(_battery_status[instance].remaining);
-	_battery_status[instance].warning = _warning;
+	_battery_status[instance].warning = _battery[instance]->determineWarning(_battery_status[instance].remaining);
 
 	uint16_t faults = 0;
 
@@ -290,43 +286,6 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
 	_battery_info_pub[instance].publish(_battery_info[instance]);
 }
 
-void
-UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
-{
-	// Not a valid measurement
-	if (current_a < 0.f) {
-		// Because the measurement was invalid we need to stop integration
-		// and re-initialize with the next valid measurement
-		_last_timestamp = 0;
-		return;
-	}
-
-	// Ignore first update because we don't know dt.
-	if (_last_timestamp != 0) {
-		const float dt = (timestamp - _last_timestamp) / 1e6;
-		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
-		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
-		_discharged_mah += _discharged_mah_loop;
-	}
-
-	_last_timestamp = timestamp;
-}
-
-void
-UavcanBatteryBridge::determineWarning(float remaining)
-{
-	// propagate warning state only if the state is higher, otherwise remain in current warning state
-	if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) {
-		_warning = battery_status_s::WARNING_EMERGENCY;
-
-	} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::WARNING_CRITICAL)) {
-		_warning = battery_status_s::WARNING_CRITICAL;
-
-	} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::WARNING_LOW)) {
-		_warning = battery_status_s::WARNING_LOW;
-	}
-}
-
 void
 UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
 				uint8_t instance)
@@ -343,9 +302,11 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
 
 	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
 
-	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
-	_battery_info[instance].id = _battery_status[instance].id;
-	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
-		 msg.model_instance_id);
-	_battery_info_pub[instance].publish(_battery_info[instance]);
+	if (msg.model_instance_id > 0) {
+		_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+		_battery_info[instance].id = _battery_status[instance].id;
+		snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number),
+			 "%" PRIu32, msg.model_instance_id);
+		_battery_info_pub[instance].publish(_battery_info[instance]);
+	}
 }

src/drivers/uavcan/sensors/battery.hpp

@@ -73,8 +73,6 @@ private:
 	void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
 	void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
 	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 *,

src/lib/battery/battery.cpp

@@ -113,6 +113,8 @@ void Battery::updateTemperature(const float temperature_c)
 
 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
+	updateDt(timestamp);
+
 	// Require minimum voltage otherwise override connected status
 	if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
 		_connected = false;
@@ -129,7 +131,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 		resetInternalResistanceEstimation(_voltage_v, _current_a);
 	}
 
-	sumDischarged(timestamp, _current_a);
+	sumDischarged(_current_a);
 	_state_of_charge_volt_based =
 		calculateStateOfChargeVoltageBased(_voltage_v, _current_a);
 
@@ -159,7 +161,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.connected = _connected;
 	battery_status.source = _source;
 	battery_status.priority = _priority;
-	battery_status.capacity = _params.capacity > 0.f ? static_cast<uint16_t>(_params.capacity) : 0;
+	battery_status.capacity = static_cast<uint16_t>(_capacity_mah);
 	battery_status.id = static_cast<uint8_t>(_index);
 	battery_status.warning = _warning;
 	battery_status.timestamp = hrt_absolute_time();
@@ -188,28 +190,26 @@ void Battery::updateAndPublishBatteryStatus(const hrt_abstime &timestamp)
 	updateBatteryStatus(timestamp);
 	publishBatteryStatus(getBatteryStatus());
 }
-
-void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
+void Battery::updateDt(const hrt_abstime &timestamp)
 {
-	// Not a valid measurement
-	if (current_a < 0.f) {
-		// Because the measurement was invalid we need to stop integration
-		// and re-initialize with the next valid measurement
-		_last_timestamp = 0;
-		return;
-	}
-
-	// Ignore first update because we don't know dt.
 	if (_last_timestamp != 0) {
-		const float dt = (timestamp - _last_timestamp) / 1e6;
-		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
-		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
-		_discharged_mah += _discharged_mah_loop;
+		_dt = math::min((timestamp - _last_timestamp) / 1e6f, 2.f); // guard to a maximum 2 seconds dt
 	}
 
 	_last_timestamp = timestamp;
 }
 
+float Battery::sumDischarged(float current_a)
+{
+	if (_dt > FLT_EPSILON) {
+		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
+		_discharged_mah_loop = (current_a * 1e3f) * (_dt / 3600.f);
+		_discharged_mah += _discharged_mah_loop;
+	}
+
+	return _discharged_mah;
+}
+
 float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a)
 {
 	if (_params.n_cells == 0) {
@@ -287,16 +287,16 @@ void Battery::resetInternalResistanceEstimation(const float voltage_v, const flo
 void Battery::estimateStateOfCharge()
 {
 	// choose which quantity we're using for final reporting
-	if ((_params.capacity > 0.f) && _battery_initialized) {
+	if ((_capacity_mah > 0.f) && _battery_initialized) {
 		// if battery capacity is known, fuse voltage measurement with used capacity
 		// The lower the voltage the more adjust the estimate with it to avoid deep discharge
 		const float weight_v = 3e-2f * (1 - _state_of_charge_volt_based);
 		_state_of_charge = (1 - weight_v) * _state_of_charge + weight_v * _state_of_charge_volt_based;
 		// directly apply current capacity slope calculated using current
-		_state_of_charge -= _discharged_mah_loop / _params.capacity;
+		_state_of_charge -= _discharged_mah_loop / _capacity_mah;
 		_state_of_charge = math::max(_state_of_charge, 0.f);
 
-		const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _params.capacity, 0.f);
+		const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _capacity_mah, 0.f);
 		_state_of_charge = math::min(state_of_charge_current_based, _state_of_charge);
 
 	} else {
@@ -376,14 +376,18 @@ float Battery::computeRemainingTime(float current_a)
 		// For FW only update when we are in level flight
 		if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s
 					       && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) {
-			// only update with positive numbers
-			_current_average_filter_a.update(fmaxf(current_a, 0.f));
+			if (_dt > FLT_EPSILON) {
+				_current_average_filter_a.update(fmaxf(current_a, 0.f), _dt);
+
+			} else {
+				_current_average_filter_a.update(fmaxf(current_a, 0.f));
+			}
 		}
 	}
 
 	// Remaining time estimation only possible with capacity
-	if (_params.capacity > 0.f) {
-		const float remaining_capacity_mah = _state_of_charge * _params.capacity;
+	if (_capacity_mah > 0.f) {
+		const float remaining_capacity_mah = _state_of_charge * _capacity_mah;
 		const float current_ma = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON);
 		time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
 	}
@@ -397,7 +401,6 @@ void Battery::updateParams()
 	param_get(_param_handles.v_empty, &_params.v_empty);
 	param_get(_param_handles.v_charged, &_params.v_charged);
 	param_get(_param_handles.n_cells, &_params.n_cells);
-	param_get(_param_handles.capacity, &_params.capacity);
 	param_get(_param_handles.r_internal, &_params.r_internal);
 	param_get(_param_handles.source, &_params.source);
 	param_get(_param_handles.low_thr, &_params.low_thr);
@@ -405,6 +408,10 @@ void Battery::updateParams()
 	param_get(_param_handles.emergen_thr, &_params.emergen_thr);
 	param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current);
 
+	float capacity{0.f};
+	param_get(_param_handles.capacity, &capacity);
+	setCapacityMah(capacity);
+
 	if (n_cells != _params.n_cells) {
 		_internal_resistance_initialized = false;
 	}

src/lib/battery/battery.h

@@ -88,7 +88,8 @@ public:
 
 	void setPriority(const uint8_t priority) { _priority = priority; }
 	void setConnected(const bool connected) { _connected = connected; }
-	void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
+	void setStateOfCharge(const float soc) { _state_of_charge = math::constrain(soc, 0.f, 1.f); _external_state_of_charge = true; }
+	void setCapacityMah(const float capacity) { _capacity_mah = math::max(capacity, 0.f); }
 	void updateVoltage(const float voltage_v);
 	void updateCurrent(const float current_a);
 	void updateTemperature(const float temperature_c);
@@ -101,6 +102,7 @@ public:
 	void updateBatteryStatus(const hrt_abstime &timestamp);
 
 	battery_status_s getBatteryStatus();
+	float getCurrentAverage() const { return PX4_ISFINITE(_current_average_filter_a.getState()) ? _current_average_filter_a.getState() : -1.f; }
 	void publishBatteryStatus(const battery_status_s &battery_status);
 
 	/**
@@ -110,6 +112,27 @@ public:
 	 */
 	void updateAndPublishBatteryStatus(const hrt_abstime &timestamp);
 
+	/**
+	 * Calculates how much time is left before the battery is depleted,
+	 * given the heavily low-pass filtered current consumption.
+	 * Requires the capacity and state of charge e.g. externally set through setCapacity() and setStateOfCharge().
+	 *
+	 * @param current_a The current draw from the battery in amperes.
+	 * @return Estimated remaining time in seconds.
+	 */
+	float computeRemainingTime(float current_a);
+
+	/**
+	 * Updates coulomb counting
+	 * Requires a dt, seeupdateDt()
+	 *
+	 * @param current_a Positive current draw in A
+	 * @return Accumulated used capacity in mAh
+	 */
+	float sumDischarged(float current_a);
+	uint8_t determineWarning(float state_of_charge);
+	void updateDt(const hrt_abstime &timestamp);
+
 protected:
 	static constexpr float LITHIUM_BATTERY_RECOGNITION_VOLTAGE = 2.1f;
 
@@ -130,7 +153,6 @@ protected:
 		float v_empty;
 		float v_charged;
 		int32_t  n_cells;
-		float capacity;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -145,13 +167,10 @@ protected:
 	void updateParams() override;
 
 private:
-	void sumDischarged(const hrt_abstime &timestamp, float current_a);
 	float calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a);
 	void estimateStateOfCharge();
-	uint8_t determineWarning(float state_of_charge);
 	uint16_t determineFaults();
 	void computeScale();
-	float computeRemainingTime(float current_a);
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
@@ -176,6 +195,8 @@ private:
 	float _state_of_charge{-1.f}; // [0,1]
 	float _scale{1.f};
 	uint8_t _warning{battery_status_s::WARNING_NONE};
+	float _dt{0.f};
+	float _capacity_mah{0.f};
 	hrt_abstime _last_timestamp{0};
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};