Removed all swapping of uORB instances

src/lib/battery/battery.cpp

@@ -165,14 +165,6 @@ Battery::publish()
 	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
 }
 
-void
-Battery::swapUorbAdvert(Battery &other)
-{
-	orb_advert_t tmp = _orb_advert;
-	_orb_advert = other._orb_advert;
-	other._orb_advert = tmp;
-}
-
 void
 Battery::filterVoltage(float voltage_v)
 {

src/lib/battery/battery.h

@@ -108,15 +108,6 @@ public:
 	 */
 	void publish();
 
-	/**
-	 * Some old functionality expects the primary battery to be published on instance 0. To maintain backwards
-	 * compatibility, this function allows the advertisements (and therefore instances) of 2 batteries to be swapped.
-	 * However, this should not be relied upon anywhere, and should be considered for all intents deprecated.
-	 *
-	 * The proper way to uniquely identify batteries is by the `id` field in the `battery_status` message.
-	 */
-	void swapUorbAdvert(Battery &other);
-
 protected:
 	struct {
 		param_t v_empty;

src/modules/battery_status/battery_status.cpp

@@ -117,10 +117,6 @@ private:
 #endif
 	}; // End _analogBatteries
 
-#if BOARD_NUMBER_BRICKS > 1
-	int 			_battery_pub_intance0ndx {0}; /**< track the index of instance 0 */
-#endif /* BOARD_NUMBER_BRICKS > 1 */
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	/**
@@ -210,17 +206,7 @@ BatteryStatus::adc_poll()
 						 * VDD_5V_IN
 						 */
 						selected_source = b;
-#  if BOARD_NUMBER_BRICKS > 1
-
-						/* Move the selected_source to instance 0 */
-						if (_battery_pub_intance0ndx != selected_source) {
-							_analogBatteries[_battery_pub_intance0ndx]->swapUorbAdvert(
-								*_analogBatteries[selected_source]
-							);
-							_battery_pub_intance0ndx = selected_source;
-						}
 
-#  endif /* BOARD_NUMBER_BRICKS > 1 */
 					}
 
 					/* look for specific channels and process the raw voltage to measurement data */

src/modules/commander/Commander.cpp

@@ -3717,7 +3717,7 @@ void Commander::battery_status_check()
 	size_t num_connected_batteries = 0;
 
 
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (size_t i = 0; i < sizeof(_battery_subs) / sizeof(_battery_subs[0]); i++) {
 		if (_battery_subs[i].updated() && _battery_subs[i].copy(&batteries[num_connected_batteries])) {
 			// We need to update the status flag if ANY battery is updated, because the system source might have
 			// changed, or might be nothing (if there is no battery connected)
@@ -3729,84 +3729,83 @@ void Commander::battery_status_check()
 		}
 	}
 
-	/* update battery status */
-	if (battery_sub_updated) {
-
-		// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
-		// option is to check if ANY of them have a warning, and specifically find which one has the most
-		// urgent warning.
-		uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
-		// Sum the total current of all connected batteries. This is used to detect engine failure.
-		float total_current = 0;
-		// To make sure that all connected batteries are being regularly reported, we check which one has the
-		// oldest timestamp.
-		hrt_abstime oldest_update = hrt_absolute_time();
+	if (!battery_sub_updated) {
+		// Nothing has changed since the last time this function was called, so nothing needs to be done now.
+		return;
+	}
 
-		// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
-		for (size_t i = 0; i < num_connected_batteries; i++) {
-			if (batteries[i].warning > worst_warning) {
-				worst_warning = batteries[i].warning;
-			}
+	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
+	// option is to check if ANY of them have a warning, and specifically find which one has the most
+	// urgent warning.
+	uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
+	// To make sure that all connected batteries are being regularly reported, we check which one has the
+	// oldest timestamp.
+	hrt_abstime oldest_update = hrt_absolute_time();
 
-			if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
-				oldest_update = batteries[i].timestamp;
-			}
+	// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
+	for (size_t i = 0; i < num_connected_batteries; i++) {
+		if (batteries[i].warning > worst_warning) {
+			worst_warning = batteries[i].warning;
+		}
 
-			total_current += batteries[i].current_filtered_a;
+		if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
+			oldest_update = batteries[i].timestamp;
 		}
 
-		bool battery_warning_level_increased_while_armed = false;
-		bool update_internal_battery_state = false;
+		if (batteries[i].system_source) {
+			_battery_current = batteries[i].current_filtered_a;
+		}
+	}
 
-		if (armed.armed) {
-			if (worst_warning > _battery_warning) {
-				battery_warning_level_increased_while_armed = true;
-				update_internal_battery_state = true;
-			}
+	bool battery_warning_level_increased_while_armed = false;
+	bool update_internal_battery_state = false;
 
-		} else {
-			if (_battery_warning != worst_warning) {
-				update_internal_battery_state = true;
-			}
+	if (armed.armed) {
+		if (worst_warning > _battery_warning) {
+			battery_warning_level_increased_while_armed = true;
+			update_internal_battery_state = true;
 		}
 
-		if (update_internal_battery_state) {
-			_battery_warning = worst_warning;
+	} else {
+		if (_battery_warning != worst_warning) {
+			update_internal_battery_state = true;
 		}
+	}
 
-		status_flags.condition_battery_healthy =
-			// All connected batteries are regularly being published
-			(hrt_elapsed_time(&oldest_update) < 5_s)
-			// There is at least one connected battery (in any slot)
-			&& num_connected_batteries > 0
-			// No currently-connected batteries have any warning
-			&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
+	if (update_internal_battery_state) {
+		_battery_warning = worst_warning;
+	}
 
-		// execute battery failsafe if the state has gotten worse while we are armed
-		if (battery_warning_level_increased_while_armed) {
-			battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
-					 (low_battery_action_t)_param_com_low_bat_act.get());
-		}
+	status_flags.condition_battery_healthy =
+		// All connected batteries are regularly being published
+		(hrt_elapsed_time(&oldest_update) < 5_s)
+		// There is at least one connected battery (in any slot)
+		&& num_connected_batteries > 0
+		// No currently-connected batteries have any warning
+		&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
 
-		// Handle shutdown request from emergency battery action
-		if (update_internal_battery_state) {
+	// execute battery failsafe if the state has gotten worse while we are armed
+	if (battery_warning_level_increased_while_armed) {
+		battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
+				 (low_battery_action_t)_param_com_low_bat_act.get());
+	}
 
-			if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
-				mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
-				px4_usleep(200000);
+	// Handle shutdown request from emergency battery action
+	if (update_internal_battery_state) {
 
-				int ret_val = px4_shutdown_request(false, false);
+		if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
+			mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
+			px4_usleep(200000);
 
-				if (ret_val) {
-					mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
+			int ret_val = px4_shutdown_request(false, false);
 
-				} else {
-					while (1) { px4_usleep(1); }
-				}
+			if (ret_val) {
+				mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
+
+			} else {
+				while (1) { px4_usleep(1); }
 			}
 		}
-
-		_battery_current = total_current;
 	}
 }
 

src/modules/commander/Commander.hpp

@@ -43,7 +43,6 @@
 #include <lib/mathlib/mathlib.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <containers/Array.hpp>
 
 // publications
 #include <uORB/Publication.hpp>
@@ -375,13 +374,19 @@ private:
 
 	// Subscriptions
 	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
+#if BOARD_NUMBER_BRICKS > 1
 	uORB::Subscription					_battery_subs[ORB_MULTI_MAX_INSTANCES] {
 		uORB::Subscription(ORB_ID(battery_status), 0),
 		uORB::Subscription(ORB_ID(battery_status), 1),
 		uORB::Subscription(ORB_ID(battery_status), 2),
 		uORB::Subscription(ORB_ID(battery_status), 3),
 	};
-	uORB::Subscription					_cmd_sub{ORB_ID(vehicle_command)};
+#else
+	uORB::Subscription					_battery_subs[1] {
+		uORB::Subscription(ORB_ID(battery_status), 0)
+	};
+#endif
+	uORB::Subscription					_cmd_sub {ORB_ID(vehicle_command)};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};