mavlink use all ORB battery_status instances instead of BOARD_NUMBER_BRICKS

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -101,7 +101,7 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
 	_windspeed(SimpleAnalyzer::AVERAGE)
 {
 
-	for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		_batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
 	}
 
@@ -119,7 +119,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 		bool updated = _airspeed.valid();
 		updated |= _airspeed_sp.valid();
 
-		for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 			updated |= _batteries[i].analyzer.valid();
 		}
 
@@ -159,7 +159,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 
 			int lowest = 0;
 
-			for (int i = 1; i < BOARD_NUMBER_BRICKS; i++) {
+			for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
 				const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid();
 				const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled(
 								       100.0f);
@@ -230,7 +230,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t)
 	_airspeed.reset();
 	_airspeed_sp.reset();
 
-	for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		_batteries[i].analyzer.reset();
 	}
 
@@ -278,7 +278,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
 	struct battery_status_s battery;
 	bool updated = false;
 
-	for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) {
 			updated = true;
 			_batteries[i].connected = battery.connected;
@@ -535,7 +535,7 @@ void MavlinkStreamHighLatency2::update_battery_status()
 {
 	battery_status_s battery;
 
-	for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		if (_batteries[i].subscription->update(&battery)) {
 			_batteries[i].connected = battery.connected;
 			_batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered);

src/modules/mavlink/mavlink_high_latency2.h

@@ -148,7 +148,7 @@ private:
 	hrt_abstime _last_update_time = 0;
 	float _update_rate_filtered = 0.0f;
 
-	PerBatteryData _batteries[BOARD_NUMBER_BRICKS];
+	PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES];
 
 	/* do not allow top copying this class */
 	MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);

src/modules/mavlink/mavlink_messages.cpp

@@ -543,11 +543,11 @@ public:
 private:
 	MavlinkOrbSubscription *_status_sub;
 	MavlinkOrbSubscription *_cpuload_sub;
-	MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS];
+	MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES];
 
 	uint64_t _status_timestamp{0};
 	uint64_t _cpuload_timestamp{0};
-	uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS];
+	uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {};
 
 	/* do not allow top copying this class */
 	MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete;
@@ -558,7 +558,7 @@ protected:
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
 		_cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload)))
 	{
-		for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 			_battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
 			_battery_status_timestamp[i] = 0;
 		}
@@ -566,63 +566,55 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		vehicle_status_s status = {};
-		cpuload_s cpuload = {};
-		battery_status_s battery_status[BOARD_NUMBER_BRICKS] = {};
+		vehicle_status_s status{};
+		cpuload_s cpuload{};
+		battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {};
 
 		const bool updated_status = _status_sub->update(&_status_timestamp, &status);
 		const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload);
-		bool updated_any = updated_status || updated_cpuload;
-		bool updated_battery[BOARD_NUMBER_BRICKS];
 
-		for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
-			updated_battery[i] = _battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i]);
-			updated_any = updated_any || updated_battery[i];
-		}
-
-		if (updated_any) {
+		bool updated_battery = false;
 
-			if (!updated_status) {
-				_status_sub->update(&status);
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i])) {
+				updated_battery = true;
 			}
+		}
 
-			battery_status_s *lowest_battery = &battery_status[0];
-
-			for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
-				if (!updated_battery[i]) {
-					_battery_status_sub[i]->update(&battery_status[i]);
-				}
+		if (updated_status || updated_cpuload || updated_battery) {
+			int lowest_battery_index = 0;
 
-				if (battery_status[i].connected && battery_status[i].remaining < lowest_battery->remaining) {
-					lowest_battery = &battery_status[i];
+			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+				if (battery_status[i].connected && (battery_status[i].remaining < battery_status[lowest_battery_index].remaining)) {
+					lowest_battery_index = i;
 				}
 			}
 
-			if (!updated_cpuload) {
-				_cpuload_sub->update(&cpuload);
-			}
-
-			mavlink_sys_status_t msg = {};
+			mavlink_sys_status_t msg{};
 
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
+
 			msg.load = cpuload.load * 1000.0f;
+
 			// TODO: Determine what data should be put here when there are multiple batteries.
 			//  Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking
 			//  one battery using this message, it should be the lowest.
 			//  In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS
 			//  to determine which battery is more important at a given time.
-			msg.voltage_battery = (lowest_battery->connected) ? lowest_battery->voltage_filtered_v * 1000.0f : UINT16_MAX;
-			msg.current_battery = (lowest_battery->connected) ? lowest_battery->current_filtered_a * 100.0f : -1;
-			msg.battery_remaining = (lowest_battery->connected) ? ceilf(lowest_battery->remaining * 100.0f) : -1;
-			// TODO: fill in something useful in the fields below
-			msg.drop_rate_comm = 0;
-			msg.errors_comm = 0;
-			msg.errors_count1 = 0;
-			msg.errors_count2 = 0;
-			msg.errors_count3 = 0;
-			msg.errors_count4 = 0;
+			const battery_status_s &lowest_battery = battery_status[lowest_battery_index];
+
+			if (lowest_battery.connected) {
+				msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f;
+				msg.current_battery = lowest_battery.current_filtered_a * 100.0f;
+				msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f);
+
+			} else {
+				msg.voltage_battery = UINT16_MAX;
+				msg.current_battery = -1;
+				msg.battery_remaining = -1;
+			}
 
 			mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);
 
@@ -667,9 +659,9 @@ public:
 	}
 
 private:
-	MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS] {};
+	MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES] {};
 
-	uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS] {};
+	uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {};
 
 	/* do not allow top copying this class */
 	MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete;
@@ -678,7 +670,7 @@ private:
 protected:
 	explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{
-		for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 			_battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
 		}
 	}
@@ -687,13 +679,13 @@ protected:
 	{
 		bool updated = false;
 
-		for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 
 			if (!_battery_status_sub[i]) {
 				continue;
 			}
 
-			battery_status_s battery_status;
+			battery_status_s battery_status{};
 
 			if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) {
 				/* battery status message with higher resolution */
@@ -706,18 +698,18 @@ protected:
 				bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
 				bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1;
 
+				// check if temperature valid
+				if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) {
+					bat_msg.temperature = battery_status.temperature * 100.0f;
+
+				} else {
+					bat_msg.temperature = INT16_MAX;
+				}
 
-				bool temperature_valid = battery_status.connected && PX4_ISFINITE(battery_status.temperature);
-				bat_msg.temperature = temperature_valid ? (int16_t)(battery_status.temperature * 100.0f) : INT16_MAX;
-				//bat_msg.average_current_battery = (battery_status.connected) ? battery_status.average_current_a * 100.0f : -1;
-				//bat_msg.serial_number = (battery_status.connected) ? battery_status.serial_number : 0;
-				//bat_msg.capacity = (battery_status.connected) ? battery_status.capacity : 0;
-				//bat_msg.cycle_count = (battery_status.connected) ? battery_status.cycle_count : UINT16_MAX;
-				//bat_msg.run_time_to_empty = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
-				//bat_msg.average_time_to_empty = (battery_status.connected) ? battery_status.average_time_to_empty * 60 : 0;
+				static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
 
-				for (unsigned cell = 0; cell < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); cell++) {
-					if ((int32_t)cell < battery_status.cell_count && battery_status.connected) {
+				for (int cell = 0; cell < mavlink_cells_max; cell++) {
+					if ((battery_status.cell_count > 0) && (cell < battery_status.cell_count) && battery_status.connected) {
 						bat_msg.voltages[cell] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
 
 					} else {