battery: publish measured values also when battery not connected

msg/battery_status.msg

@@ -1,4 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
+bool connected				# Whether or not a battery is connected, based on a voltage threshold
 float32 voltage_v			# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a			# Battery current in amperes, -1 if unknown
@@ -9,7 +10,6 @@ float32 remaining			# From 1 to 0, -1 if unknown
 float32 scale				# Power scaling factor, >= 1, or -1 if unknown
 float32 temperature			# temperature of the battery. NaN if unknown
 int32 cell_count			# Number of cells
-bool connected				# Whether or not a battery is connected, based on a voltage threshold
 
 uint8 BATTERY_SOURCE_POWER_MODULE = 0
 uint8 BATTERY_SOURCE_EXTERNAL = 1

src/lib/battery/battery.cpp

@@ -125,6 +125,11 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 
 	if (_voltage_filter_v.getState() > 2.1f) {
 		_battery_initialized = true;
+
+	} else {
+		connected = false;
+	}
+
 	_battery_status.voltage_v = voltage_v;
 	_battery_status.voltage_filtered_v = _voltage_filter_v.getState();
 	_battery_status.current_a = current_a;
@@ -140,7 +145,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 	_battery_status.source = source;
 	_battery_status.priority = priority;
 	_battery_status.capacity = _params.capacity > 0.f ? static_cast<uint16_t>(_params.capacity) : 0;
-		_battery_status.id = (uint8_t) _index;
+	_battery_status.id = static_cast<uint8_t>(_index);
 	_battery_status.warning = _warning;
 
 	static constexpr int32_t uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof(
@@ -153,18 +158,6 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 		_battery_status.voltage_cell_v[i] = _battery_status.voltage_filtered_v / max_cells;
 	}
 
-	} else {
-		_battery_status.current_a = -1.f;
-		_battery_status.remaining = 1.f;
-		_battery_status.scale = 1.f;
-		// TODO: check if it is sane to reset warning to NONE
-		_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
-		_battery_status.connected = false;
-		_battery_status.capacity = _params.capacity > 0.0f ? (uint16_t)_params.capacity : 0;
-		_battery_status.temperature = NAN;
-		_battery_status.id = (uint8_t) _index;
-	}
-
 	if (source == _params.source) {
 		_battery_status.timestamp = hrt_absolute_time();
 		_battery_status_pub.publish(_battery_status);

src/modules/control_allocator/ControlAllocator.hpp

@@ -146,7 +146,6 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};			/**< battery status subscription */
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};				/**< airspeed subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -565,12 +565,10 @@ void FixedwingAttitudeControl::Run()
 						if (_battery_status_sub.updated()) {
 							battery_status_s battery_status{};
 
-							if (_battery_status_sub.copy(&battery_status)) {
-								if (battery_status.scale > 0.0f) {
+							if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
 								_battery_scale = battery_status.scale;
 							}
 						}
-						}
 
 						_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
 					}

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -241,7 +241,7 @@ MulticopterRateControl::Run()
 				if (_battery_status_sub.updated()) {
 					battery_status_s battery_status;
 
-					if (_battery_status_sub.copy(&battery_status)) {
+					if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
 						_battery_status_scale = battery_status.scale;
 					}
 				}