Multi-EKF support (ekf2)

- ekf2 can now run in multi-instance mode (currently up to 9 instances)
    - in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
 - new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
 - sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
 - existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
 - ekf2 single instance mode is still fully supported and the default

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -38,6 +38,7 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 
@@ -66,7 +67,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	bool gps_present = true;
 
 	// Get estimator status data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
+	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
 	status_sub.update();
 	const estimator_status_s &status = status_sub.get();
 
@@ -238,7 +240,9 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r
 	param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);
 
 	// Get estimator states data if available and exit with a fail recorded if not
-	uORB::Subscription states_sub{ORB_ID(estimator_states)};
+	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription states_sub{ORB_ID(estimator_states), estimator_selector_status_sub.get().primary_instance};
+
 	estimator_states_s states;
 
 	if (states_sub.copy(&states)) {

src/modules/commander/Commander.cpp

@@ -4055,6 +4055,17 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
 
 	const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
 
+	// use primary estimator_status
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
+				_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
+			}
+		}
+	}
+
 	if (_estimator_status_sub.update()) {
 		const estimator_status_s &estimator_status = _estimator_status_sub.get();
 

src/modules/commander/Commander.hpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
@@ -384,6 +385,7 @@ private:
 	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                                      _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};