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>

.ci/Jenkinsfile-hardware

@@ -836,6 +836,8 @@ void statusFTDI() {
   // run logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"'
   // status commands
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "board_adc test"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"'
@@ -846,14 +848,15 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "gps status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener cpuload"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_attitude"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_local_position"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_selector_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor_bias"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"'
@@ -901,6 +904,8 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
   // stop logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger off"'
 }
@@ -909,6 +914,8 @@ void statusSEGGER() {
   // run logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger on"'
   // status commands
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "board_adc test"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"'
@@ -919,12 +926,13 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "gps status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener cpuload"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_attitude"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_local_position"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_selector_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_sensor_bias"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"'
@@ -974,6 +982,8 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
   // stop logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger off"'
 }

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -141,6 +141,12 @@ then
 	# Speedup SITL startup
 	param set EKF2_REQ_GPS_H 0.5
 
+	# Multi-EKF
+	#param set EKF2_MULTI_IMU 3
+	#param set SENS_IMU_MODE 0
+	#param set EKF2_MULTI_MAG 2
+	#param set SENS_MAG_MODE 0
+
 	# By default log from boot until first disarm.
 	param set SDLOG_MODE 1
 	# enable default, estimator replay and vision/avoidance logging profiles

ROMFS/px4fmu_common/init.d/rc.airship_apps

@@ -40,7 +40,7 @@ else
 		# EKF2
 		#
 		param set SYS_MC_EST_GROUP 2
-		ekf2 start
+		ekf2 start &
 	fi
 fi
 

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -8,7 +8,7 @@
 #
 # Start the attitude and position estimator.
 #
-ekf2 start
+ekf2 start &
 
 #
 # Start attitude controller.

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -40,7 +40,7 @@ else
 		# EKF2
 		#
 		param set SYS_MC_EST_GROUP 2
-		ekf2 start
+		ekf2 start &
 	fi
 fi
 

ROMFS/px4fmu_common/init.d/rc.rover_apps

@@ -8,7 +8,7 @@
 #
 # Start the attitude and position estimator.
 #
-ekf2 start
+ekf2 start &
 #attitude_estimator_q start
 #local_position_estimator start
 

ROMFS/px4fmu_common/init.d/rc.uuv_apps

@@ -9,7 +9,7 @@
 #                       Begin Estimator Group Selection                       #
 ###############################################################################
 
-ekf2 start
+ekf2 start &
 
 ###############################################################################
 #                        End Estimator Group Selection                        #

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -199,5 +199,5 @@ fi
 if [ $VEHICLE_TYPE = none ]
 then
 	echo "No autostart ID found"
-	ekf2 start
+	ekf2 start &
 fi

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -9,7 +9,7 @@
 #                       Begin Estimator group selection                       #
 ###############################################################################
 
-ekf2 start
+ekf2 start &
 
 ###############################################################################
 #                        End Estimator group selection                        #

Tools/ecl_ekf/analyse_logdata_ekf.py

@@ -14,13 +14,14 @@ from analysis.checks import perform_ecl_ekf_checks
 from analysis.post_processing import get_estimator_check_flags
 
 def analyse_ekf(
-        ulog: ULog, check_levels: Dict[str, float], red_thresh: float = 1.0,
-        amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0,
+        ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
+        red_thresh: float = 1.0, amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0,
         in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \
         Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]:
     """
     :param ulog:
     :param check_levels:
+    :param multi_instance:
     :param red_thresh:
     :param amb_thresh:
     :param min_flight_duration_seconds:
@@ -30,22 +31,19 @@ def analyse_ekf(
     """
 
     try:
-        estimator_states = ulog.get_dataset('estimator_states').data
-        print('found estimator_states data')
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_states data')
+        raise PreconditionError('could not find estimator_states instance', multi_instance)
 
     try:
-        estimator_status = ulog.get_dataset('estimator_status').data
-        print('found estimator_status data')
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_status data')
+        raise PreconditionError('could not find estimator_status instance', multi_instance)
 
     try:
-        _ = ulog.get_dataset('estimator_innovations').data
-        print('found estimator_innovations data')
+        _ = ulog.get_dataset('estimator_innovations', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_innovations data')
+        raise PreconditionError('could not find estimator_innovations instance', multi_instance)
 
     try:
         in_air = InAirDetector(
@@ -71,7 +69,7 @@ def analyse_ekf(
 
     metrics = calculate_ecl_ekf_metrics(
         ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
-        red_thresh=red_thresh, amb_thresh=amb_thresh)
+        multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh)
 
     check_status, master_status = perform_ecl_ekf_checks(
         metrics, sensor_checks, innov_fail_checks, check_levels)

Tools/ecl_ekf/analysis/metrics.py

@@ -13,7 +13,7 @@ from analysis.detectors import InAirDetector
 def calculate_ecl_ekf_metrics(
         ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
         sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
-        red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
+        multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
 
     sensor_metrics = calculate_sensor_metrics(
         ulog, sensor_checks, in_air, in_air_no_ground_effects,
@@ -22,9 +22,9 @@ def calculate_ecl_ekf_metrics(
     innov_fail_metrics = calculate_innov_fail_metrics(
         innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
 
-    imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects)
+    imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     # Check for internal filter nummerical faults
     ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])}
@@ -44,10 +44,10 @@ def calculate_ecl_ekf_metrics(
 
 def calculate_sensor_metrics(
         ulog: ULog, sensor_checks: List[str], in_air: InAirDetector,
-        in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0,
-        amb_thresh: float = 0.5) -> Dict[str, float]:
+        in_air_no_ground_effects: InAirDetector, multi_instance: int = 0,
+        red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]:
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     sensor_metrics = dict()
 
@@ -131,10 +131,9 @@ def calculate_innov_fail_metrics(
     return innov_fail_metrics
 
 
-def calculate_imu_metrics(
-        ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
+def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict:
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     imu_metrics = dict()
 
@@ -158,7 +157,7 @@ def calculate_imu_metrics(
                 in_air_no_ground_effects, np.mean)
 
     # IMU bias checks
-    estimator_states_data = ulog.get_dataset('estimator_states').data
+    estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data
 
     imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
         estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median))

Tools/ecl_ekf/plotting/pdf_report.py

@@ -17,7 +17,7 @@ from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSumma
 from analysis.detectors import PreconditionError
 import analysis.data_version_handler as dvh
 
-def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
+def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str) -> None:
     """
     creates a pdf report of the ekf analysis.
     :param ulog:
@@ -29,20 +29,18 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     # save the plots to PDF
 
     try:
-        estimator_status = ulog.get_dataset('estimator_status').data
-        print('found estimator_status data')
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_status data')
+        raise PreconditionError('could not find estimator_status instance', multi_instance)
 
     try:
-        estimator_states = ulog.get_dataset('estimator_states').data
-        print('found estimator_states data')
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_states data')
+        raise PreconditionError('could not find estimator_states instance', multi_instance)
 
     try:
-        estimator_innovations = ulog.get_dataset('estimator_innovations').data
-        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
+        estimator_innovations = ulog.get_dataset('estimator_innovations', multi_instance).data
+        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances', multi_instance).data
         innovation_data = estimator_innovations
         for key in estimator_innovation_variances:
             # append 'var' to the field name such that we can distingush between innov and innov_var
@@ -65,7 +63,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
             for key in innovation_data:
                 innovation_data[key] = innovation_data[key][0:innovation_data_min_length]
 
-        print('found innovation data (merged estimator_innovations + estimator_innovation_variances)')
+        print('found innovation data (merged estimator_innovations + estimator_innovation_variances) instance', multi_instance)
 
     except:
         raise PreconditionError('could not find innovation data')

Tools/ecl_ekf/process_logdata_ekf.py

@@ -90,6 +90,21 @@ def process_logdata_ekf(
     except:
         raise PreconditionError('could not open {:s}'.format(filename))
 
+    ekf_instances = 1
+
+    try:
+        estimator_selector_status = ulog.get_dataset('estimator_selector_status',).data
+        print('found estimator_selector_status (multi-ekf) data')
+
+        for instances_available in estimator_selector_status['instances_available']:
+            if instances_available > ekf_instances:
+                ekf_instances = instances_available
+
+        print(ekf_instances, 'ekf instances')
+
+    except:
+        print('could not find estimator_selector_status data')
+
     try:
         # get the dictionary of fail and warning test thresholds from a csv file
         with open(check_level_dict_filename, 'r') as file:
@@ -100,30 +115,35 @@ def process_logdata_ekf(
         raise PreconditionError('could not find {:s}'.format(check_level_dict_filename))
 
     in_air_margin = 5.0 if sensor_safety_margins else 0.0
-    # perform the ekf analysis
-    master_status, check_status, metrics, airtime_info = analyse_ekf(
-        ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0,
-        in_air_margin_seconds=in_air_margin)
 
-    test_results = create_results_table(
-        check_table_filename, master_status, check_status, metrics, airtime_info)
+    for multi_instance in range(ekf_instances):
 
-    # write metadata to a .csv file
-    with open('{:s}.mdat.csv'.format(filename), "w") as file:
+        print('\nestimator instance:', multi_instance)
 
-        file.write("name,value,description\n")
+        # perform the ekf analysis
+        master_status, check_status, metrics, airtime_info = analyse_ekf(
+            ulog, check_levels, multi_instance, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0,
+            in_air_margin_seconds=in_air_margin)
 
-        # loop through the test results dictionary and write each entry on a separate row, with data comma separated
-        # save data in alphabetical order
-        key_list = list(test_results.keys())
-        key_list.sort()
-        for key in key_list:
-            file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n")
-    print('Test results written to {:s}.mdat.csv'.format(filename))
+        test_results = create_results_table(
+            check_table_filename, master_status, check_status, metrics, airtime_info)
 
-    if plot:
-        create_pdf_report(ulog, '{:s}.pdf'.format(filename))
-        print('Plots saved to {:s}.pdf'.format(filename))
+        # write metadata to a .csv file
+        with open('{:s}-{:d}.mdat.csv'.format(filename, multi_instance), "w") as file:
+
+            file.write("name,value,description\n")
+
+            # loop through the test results dictionary and write each entry on a separate row, with data comma separated
+            # save data in alphabetical order
+            key_list = list(test_results.keys())
+            key_list.sort()
+            for key in key_list:
+                file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n")
+        print('Test results written to {:s}-{:d}.mdat.csv'.format(filename, multi_instance))
+
+        if plot:
+            create_pdf_report(ulog, multi_instance, '{:s}-{:d}.pdf'.format(filename, multi_instance))
+            print('Plots saved to {:s}-{:d}.pdf'.format(filename, multi_instance))
 
     return test_results
 

msg/CMakeLists.txt

@@ -57,6 +57,7 @@ set(msg_files
 	esc_status.msg
 	estimator_innovations.msg
 	estimator_optical_flow_vel.msg
+	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg

msg/estimator_selector_status.msg

@@ -0,0 +1,22 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 primary_instance
+
+uint8 instances_available
+
+uint32 instance_changed_count
+uint64 last_instance_change
+
+uint32 accel_device_id
+uint32 baro_device_id
+uint32 gyro_device_id
+uint32 mag_device_id
+
+float32[9] combined_test_ratio
+float32[9] relative_test_ratio
+bool[9] healthy
+
+float32[4] accumulated_gyro_error
+float32[4] accumulated_accel_error
+bool gyro_fault_detected
+bool accel_fault_detected

msg/sensors_status_imu.msg

@@ -5,13 +5,13 @@ uint64 timestamp # time since system start (microseconds)
 
 uint32 accel_device_id_primary       # current primary accel device id for reference
 
-uint32[3] accel_device_ids
-float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s).
-bool[3] accel_healthy
+uint32[4] accel_device_ids
+float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in (m/s/s).
+bool[4] accel_healthy
 
 
 uint32 gyro_device_id_primary       # current primary gyro device id for reference
 
-uint32[3] gyro_device_ids
-float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s).
-bool[3] gyro_healthy
+uint32[4] gyro_device_ids
+float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s).
+bool[4] gyro_healthy

msg/tools/uorb_rtps_message_ids.yaml

@@ -287,6 +287,8 @@ rtps:
     id: 137
   - msg: estimator_optical_flow_vel
     id: 138
+  - msg: estimator_selector_status
+    id: 139
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_attitude.msg

@@ -9,3 +9,4 @@ float32[4] delta_q_reset 	# Amount by which quaternion has changed during last r
 uint8 quat_reset_counter	# Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
+# TOPICS estimator_attitude

msg/vehicle_global_position.msg

@@ -26,3 +26,4 @@ bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth
+# TOPICS estimator_global_position

msg/vehicle_local_position.msg

@@ -65,3 +65,4 @@ float32 hagl_min			# minimum height above ground level - set to 0 when limiting
 float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth
+# TOPICS estimator_local_position

msg/vehicle_odometry.msg

@@ -63,3 +63,4 @@ float32 yawspeed		# Angular velocity about Z body axis
 float32[21] velocity_covariance
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned
+# TOPICS estimator_odometry estimator_visual_odometry_aligned

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -65,23 +65,27 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11};
 static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12};
 
 // PX4 att/pos controllers, highest priority after sensors.
-static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1672, -13};
-static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 7200, -14};
+static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1728, -13};
 
-static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -15};
+static constexpr wq_config_t INS0{"wq:INS0", 7200, -14};
+static constexpr wq_config_t INS1{"wq:INS1", 7200, -15};
+static constexpr wq_config_t INS2{"wq:INS2", 7200, -16};
+static constexpr wq_config_t INS3{"wq:INS3", 7200, -17};
 
-static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -16};
+static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
 
-static constexpr wq_config_t UART0{"wq:UART0", 1400, -17};
-static constexpr wq_config_t UART1{"wq:UART1", 1400, -18};
-static constexpr wq_config_t UART2{"wq:UART2", 1400, -19};
-static constexpr wq_config_t UART3{"wq:UART3", 1400, -20};
-static constexpr wq_config_t UART4{"wq:UART4", 1400, -21};
-static constexpr wq_config_t UART5{"wq:UART5", 1400, -22};
-static constexpr wq_config_t UART6{"wq:UART6", 1400, -23};
-static constexpr wq_config_t UART7{"wq:UART7", 1400, -24};
-static constexpr wq_config_t UART8{"wq:UART8", 1400, -25};
-static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26};
+static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -19};
+
+static constexpr wq_config_t UART0{"wq:UART0", 1400, -21};
+static constexpr wq_config_t UART1{"wq:UART1", 1400, -22};
+static constexpr wq_config_t UART2{"wq:UART2", 1400, -23};
+static constexpr wq_config_t UART3{"wq:UART3", 1400, -24};
+static constexpr wq_config_t UART4{"wq:UART4", 1400, -25};
+static constexpr wq_config_t UART5{"wq:UART5", 1400, -26};
+static constexpr wq_config_t UART6{"wq:UART6", 1400, -27};
+static constexpr wq_config_t UART7{"wq:UART7", 1400, -28};
+static constexpr wq_config_t UART8{"wq:UART8", 1400, -29};
+static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -30};
 
 static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
 
@@ -129,5 +133,7 @@ const wq_config_t &device_bus_to_wq(uint32_t device_id);
  */
 const wq_config_t &serial_port_to_wq(const char *serial);
 
+const wq_config_t &ins_instance_to_wq(uint8_t instance);
+
 
 } // namespace px4

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -201,6 +201,23 @@ serial_port_to_wq(const char *serial)
 	return wq_configurations::UART_UNKNOWN;
 }
 
+const wq_config_t &ins_instance_to_wq(uint8_t instance)
+{
+	switch (instance) {
+	case 0: return wq_configurations::INS0;
+
+	case 1: return wq_configurations::INS1;
+
+	case 2: return wq_configurations::INS2;
+
+	case 3: return wq_configurations::INS3;
+	}
+
+	PX4_WARN("no INS%d wq configuration, using INS0", instance);
+
+	return wq_configurations::INS0;
+}
+
 static void *
 WorkQueueRunner(void *context)
 {

src/lib/cdev/posix/cdev_platform.cpp

@@ -62,9 +62,9 @@ private:
 	px4_dev_t() = default;
 };
 
-static px4_dev_t *devmap[128] {};
+static px4_dev_t *devmap[256] {};
 
-#define PX4_MAX_FD 128
+#define PX4_MAX_FD 256
 static cdev::file_t filemap[PX4_MAX_FD] {};
 
 class VFile : public cdev::CDev

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -47,6 +47,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/parameter_update.h>
@@ -105,6 +106,7 @@ private:
 	uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
 	orb_advert_t 	_mavlink_log_pub {nullptr}; 						/**< mavlink log topic*/
 
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription _param_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -430,6 +432,17 @@ void AirspeedModule::update_params()
 
 void AirspeedModule::poll_topics()
 {
+	// 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);
+			}
+		}
+	}
+
 	_estimator_status_sub.update(&_estimator_status);
 	_vehicle_acceleration_sub.update(&_accel);
 	_vehicle_air_data_sub.update(&_vehicle_air_data);

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)};

src/modules/ekf2/CMakeLists.txt

@@ -41,10 +41,13 @@ px4_add_module(
 	SRCS
 		EKF2.cpp
 		EKF2.hpp
+		EKF2Selector.cpp
+		EKF2Selector.hpp
 	DEPENDS
 		git_ecl
 		ecl_EKF
 		ecl_geo
 		perf
 		EKF2Utility
+		px4_work_queue
 	)

src/modules/ekf2/EKF2.hpp

@@ -39,8 +39,12 @@
  */
 
 #pragma once
+
+#include "EKF2Selector.hpp"
+
 #include <float.h>
 
+#include <containers/LockGuard.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/EKF/ekf.h>
 #include <lib/mathlib/mathlib.h>
@@ -61,6 +65,7 @@
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_drift.h>
 #include <uORB/topics/estimator_innovations.h>
+#include <uORB/topics/estimator_optical_flow_vel.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
@@ -81,14 +86,16 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/yaw_estimator_status.h>
-#include <uORB/topics/estimator_optical_flow_vel.h>
 
 #include "Utility/PreFlightChecker.hpp"
 
-class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
+extern pthread_mutex_t ekf2_module_mutex;
+
+class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	explicit EKF2(bool replay_mode = false);
+	EKF2() = delete;
+	EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool replay_mode);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -100,9 +107,15 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	bool init();
+	int print_status();
 
-	int print_status() override;
+	bool should_exit() const { return _task_should_exit.load(); }
+
+	void request_stop() { _task_should_exit.store(true); }
+
+	static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); }
+	static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); }
+	static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); }
 
 private:
 	void Run() override;
@@ -123,19 +136,23 @@ private:
 	bool update_mag_decl(Param &mag_decl_param);
 
 	void publish_attitude(const hrt_abstime &timestamp);
+	void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);
 	void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
 	void publish_wind_estimate(const hrt_abstime &timestamp);
 	void publish_yaw_estimator_status(const hrt_abstime &timestamp);
-	void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);
 
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
 	float filter_altitude_ellipsoid(float amsl_hgt);
 
-	inline float sq(float x) { return x * x; };
+	static constexpr float sq(float x) { return x * x; };
 
-	const bool 	_replay_mode;			///< true when we use replay data from a log
+	const bool _replay_mode{false};			///< true when we use replay data from a log
+	const bool _multi_mode;
+	const int _instance;
+
+	px4::atomic_bool _task_should_exit{false};
 
 	// time slip monitoring
 	uint64_t _integrated_time_us = 0;	///< integral of gyro delta time from start (uSec)
@@ -186,9 +203,8 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
-	static constexpr int MAX_SENSOR_COUNT = 3;
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
-	int _imu_sub_index{-1};
+
 	bool _callback_registered{false};
 	int _lockstep_component{-1};
 
@@ -200,23 +216,25 @@ private:
 	vehicle_land_detected_s		_vehicle_land_detected{};
 	vehicle_status_s		_vehicle_status{};
 
-	uORB::Publication<ekf2_timestamps_s>			_ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
-	uORB::Publication<ekf_gps_drift_s>			_ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
-	uORB::Publication<estimator_innovations_s>		_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
-	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
-	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
-	uORB::Publication<estimator_optical_flow_vel_s>		_estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
-	uORB::Publication<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
-	uORB::Publication<estimator_states_s>			_estimator_states_pub{ORB_ID(estimator_states)};
-	uORB::PublicationData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
-	uORB::Publication<vehicle_attitude_s>			_att_pub{ORB_ID(vehicle_attitude)};
-	uORB::Publication<vehicle_odometry_s>			_vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
-	uORB::Publication<yaw_estimator_status_s>		_yaw_est_pub{ORB_ID(yaw_estimator_status)};
-	uORB::PublicationData<vehicle_global_position_s>	_vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
-	uORB::PublicationData<vehicle_local_position_s>		_vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
-	uORB::PublicationData<vehicle_odometry_s>		_vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
+	uORB::PublicationMulti<ekf2_timestamps_s>		_ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
+	uORB::PublicationMulti<ekf_gps_drift_s>			_ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
+	uORB::PublicationMulti<estimator_innovations_s>		_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
+	uORB::PublicationMulti<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
+	uORB::PublicationMulti<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
+	uORB::PublicationMulti<estimator_optical_flow_vel_s>	_estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
+	uORB::PublicationMulti<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
+	uORB::PublicationMulti<estimator_states_s>		_estimator_states_pub{ORB_ID(estimator_states)};
+	uORB::PublicationMultiData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
+	uORB::PublicationMulti<yaw_estimator_status_s>		_yaw_est_pub{ORB_ID(yaw_estimator_status)};
 	uORB::PublicationMulti<wind_estimate_s>			_wind_pub{ORB_ID(wind_estimate)};
 
+	// publications with topic dependent on multi-mode
+	uORB::PublicationMulti<vehicle_attitude_s>            _attitude_pub;
+	uORB::PublicationMultiData<vehicle_local_position_s>  _local_position_pub;
+	uORB::PublicationMultiData<vehicle_global_position_s> _global_position_pub;
+	uORB::PublicationMulti<vehicle_odometry_s>            _odometry_pub;
+	uORB::PublicationMultiData<vehicle_odometry_s>        _visual_odometry_aligned_pub;
+
 	Ekf _ekf;
 
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)
@@ -368,8 +386,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_OF_GATE>)
 		_param_ekf2_of_gate,	///< optical flow fusion innovation consistency gate size (STD)
 
-		(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
-
 		// sensor positions in body frame
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x,		///< X position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y,		///< Y position of IMU in body frame (m)

src/modules/ekf2/EKF2Selector.hpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/time.h>
+#include <lib/mathlib/mathlib.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/sensors_status_imu.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_odometry.h>
+
+static constexpr uint8_t EKF2_MAX_INSTANCES{9};
+static_assert(EKF2_MAX_INSTANCES <= ORB_MULTI_MAX_INSTANCES, "EKF2_MAX_INSTANCES must be <= ORB_MULTI_MAX_INSTANCES");
+
+class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	EKF2Selector();
+	~EKF2Selector() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	static constexpr uint8_t INVALID_INSTANCE = UINT8_MAX;
+	void Run() override;
+	void PublishVehicleAttitude(bool reset = false);
+	void PublishVehicleLocalPosition(bool reset = false);
+	void PublishVehicleGlobalPosition(bool reset = false);
+	void SelectInstance(uint8_t instance);
+
+	// Update the error scores for all available instances
+	bool UpdateErrorScores();
+
+	// Subscriptions (per estimator instance)
+	struct EstimatorInstance {
+		EstimatorInstance(EKF2Selector *selector, uint8_t i) :
+			estimator_attitude_sub{selector, ORB_ID(estimator_attitude), i},
+			estimator_status_sub{selector, ORB_ID(estimator_status), i},
+			estimator_local_position_sub{ORB_ID(estimator_local_position), i},
+			estimator_global_position_sub{ORB_ID(estimator_global_position), i},
+			estimator_odometry_sub{ORB_ID(estimator_odometry), i},
+			estimator_visual_odometry_aligned_sub{ORB_ID(estimator_visual_odometry_aligned), i},
+			instance(i)
+		{}
+
+		uORB::SubscriptionCallbackWorkItem estimator_attitude_sub;
+		uORB::SubscriptionCallbackWorkItem estimator_status_sub;
+
+		uORB::Subscription estimator_local_position_sub;
+		uORB::Subscription estimator_global_position_sub;
+		uORB::Subscription estimator_odometry_sub;
+		uORB::Subscription estimator_visual_odometry_aligned_sub;
+
+		estimator_status_s estimator_status{};
+
+		hrt_abstime time_last_selected{0};
+
+		float combined_test_ratio{0.f};
+		float relative_test_ratio{0.f};
+
+		bool healthy{false};
+
+		const uint8_t instance;
+	};
+
+	static constexpr float _rel_err_score_lim{1.0f}; // +- limit applied to the relative error score
+	static constexpr float _rel_err_thresh{0.5f};    // the relative score difference needs to be greater than this to switch from an otherwise healthy instance
+
+	EstimatorInstance _instance[EKF2_MAX_INSTANCES] {
+		{this, 0},
+		{this, 1},
+		{this, 2},
+		{this, 3},
+		{this, 4},
+		{this, 5},
+		{this, 6},
+		{this, 7},
+		{this, 8},
+	};
+
+	static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(
+				sensors_status_imu_s::gyro_inconsistency_rad_s[0]));
+	static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof(
+			      estimator_selector_status_s::accumulated_gyro_error[0]),
+		      "increase estimator_selector_status_s::accumulated_gyro_error size");
+	static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof(
+			      estimator_selector_status_s::accumulated_accel_error[0]),
+		      "increase estimator_selector_status_s::accumulated_accel_error size");
+	static_assert(EKF2_MAX_INSTANCES == sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof(
+			      estimator_selector_status_s::combined_test_ratio[0]),
+		      "increase estimator_selector_status_s::combined_test_ratio size");
+
+	float _accumulated_gyro_error[IMU_STATUS_SIZE] {};
+	float _accumulated_accel_error[IMU_STATUS_SIZE] {};
+	hrt_abstime _last_update_us{0};
+	bool _gyro_fault_detected{false};
+	bool _accel_fault_detected{false};
+
+	uint8_t _available_instances{0};
+	uint8_t _selected_instance{INVALID_INSTANCE};
+
+	uint32_t _instance_changed_count{0};
+	hrt_abstime _last_instance_change{0};
+
+	// vehicle_attitude: reset counters
+	vehicle_attitude_s _attitude_last{};
+	matrix::Quatf _delta_q_reset{};
+	uint8_t _quat_reset_counter{0};
+
+	// vehicle_local_position: reset counters
+	vehicle_local_position_s _local_position_last{};
+	matrix::Vector2f _delta_xy_reset{};
+	float _delta_z_reset{0.f};
+	matrix::Vector2f _delta_vxy_reset{};
+	float _delta_vz_reset{0.f};
+	float _delta_heading_reset{0};
+	uint8_t _xy_reset_counter{0};
+	uint8_t _z_reset_counter{0};
+	uint8_t _vxy_reset_counter{0};
+	uint8_t _vz_reset_counter{0};
+	uint8_t _heading_reset_counter{0};
+
+	// vehicle_global_position: reset counters
+	vehicle_global_position_s _global_position_last{};
+	double _delta_lat_reset{0};
+	double _delta_lon_reset{0};
+	float _delta_alt_reset{0.f};
+	uint8_t _lat_lon_reset_counter{0};
+	uint8_t _alt_reset_counter{0};
+
+	int _lockstep_component{-1};
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
+
+	// Publications
+	uORB::Publication<estimator_selector_status_s> _estimator_selector_status_pub{ORB_ID(estimator_selector_status)};
+	uORB::Publication<sensor_selection_s>          _sensor_selection_pub{ORB_ID(sensor_selection)};
+	uORB::Publication<vehicle_attitude_s>          _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
+	uORB::Publication<vehicle_global_position_s>   _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
+	uORB::Publication<vehicle_local_position_s>    _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::Publication<vehicle_odometry_s>          _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
+	uORB::Publication<vehicle_odometry_s>          _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::EKF2_SEL_ERR_RED>) _param_ekf2_sel_err_red,
+		(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate,
+		(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle,
+		(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel,
+		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity
+	)
+};

src/modules/ekf2/ekf2_multi_params.c

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Multi-EKF IMUs
+ *
+ * Maximum number of IMUs to use for Multi-EKF. Set 0 to disable.
+ * Requires SENS_IMU_MODE 0.
+ *
+ * @group EKF2
+ * @reboot_required true
+ * @min 0
+ * @max 3
+ */
+PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0);
+
+/**
+ * Multi-EKF Magnetometers.
+ *
+ * Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable.
+ * Requires SENS_MAG_MODE 0.
+ *
+ * @group EKF2
+ * @reboot_required true
+ * @min 0
+ * @max 4
+ */
+PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0);

src/modules/ekf2/ekf2_params.c

@@ -820,19 +820,6 @@ PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f);
 
-/**
- * Device id of IMU
- *
- * Set to 0 to use system selected (sensor_combined) IMU,
- * otherwise set to the device id of the desired IMU (vehicle_imu).
- *
- * @group EKF2
- * @value 0 System Primary
- * @category Developer
- *
- */
-PARAM_DEFINE_INT32(EKF2_IMU_ID, 0);
-
 /**
  * X position of IMU in body frame (forward axis with origin relative to vehicle centre of gravity)
  *

src/modules/ekf2/ekf2_selector_params.c

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Selector error reduce threshold
+ *
+ * EKF2 instances have to be better than the selected by at least this amount before their relative score can be reduced.
+ *
+ * @group EKF2
+ */
+PARAM_DEFINE_FLOAT(EKF2_SEL_ERR_RED, 0.2f);
+
+/**
+ * Selector angular rate threshold
+ *
+ * EKF2 selector angular rate error threshold for comparing gyros. Angular rate vector differences larger than this will result in accumulated angular error.
+ *
+ * @group EKF2
+ * @unit deg/s
+ */
+PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_RAT, 7.0f);
+
+/**
+ * Selector angular threshold.
+ *
+ * EKF2 selector maximum accumulated angular error threshold for comparing gyros. Accumulated angular error larger than this will result in the sensor being declared faulty.
+ *
+ * @group EKF2
+ * @unit deg
+ */
+PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ANG, 15.0f);
+
+/**
+ * Selector acceleration threshold
+ *
+ * EKF2 selector acceleration error threshold for comparing accelerometers. Acceleration vector differences larger than this will result in accumulated velocity error.
+ *
+ * @group EKF2
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f);
+
+/**
+ * Selector angular threshold.
+ *
+ * EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. Accumulated velocity error larger than this will result in the sensor being declared faulty.
+ *
+ * @group EKF2
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f);

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -42,7 +42,7 @@ using math::radians;
 
 FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))

src/modules/logger/logged_topics.cpp

@@ -55,15 +55,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
 	add_topic("cpuload");
-	add_topic("ekf_gps_drift");
 	add_topic("esc_status", 250);
-	add_topic("estimator_innovation_test_ratios", 200);
-	add_topic("estimator_innovation_variances", 200);
-	add_topic("estimator_innovations", 200);
-	add_topic("estimator_optical_flow_vel", 200);
-	add_topic("estimator_sensor_bias", 1000);
-	add_topic("estimator_states", 1000);
-	add_topic("estimator_status", 200);
 	add_topic("generator_status");
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
@@ -109,24 +101,39 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_status");
 	add_topic("vehicle_status_flags");
 	add_topic("vtol_vehicle_status", 200);
-	add_topic("yaw_estimator_status", 200);
 
 	// multi topics
 	add_topic_multi("actuator_outputs", 100, 2);
 	add_topic_multi("logger_status", 0, 2);
 	add_topic_multi("multirotor_motor_limits", 1000, 2);
 	add_topic_multi("rate_ctrl_status", 200, 2);
-	add_topic_multi("telemetry_status", 1000);
-	add_topic_multi("wind_estimate", 1000);
+	add_topic_multi("telemetry_status", 1000, 4);
+
+	// EKF multi topics (currently max 9 estimators)
+	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4;
+	add_topic("estimator_selector_status", 200);
+	add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_sensor_bias", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
-	add_topic_multi("battery_status", 300, 4);
-	add_topic_multi("differential_pressure", 1000, 3);
+	add_topic_multi("battery_status", 300, 2);
+	add_topic_multi("differential_pressure", 1000, 2);
 	add_topic_multi("distance_sensor", 1000);
-	add_topic_multi("optical_flow", 1000, 2);
+	add_topic_multi("optical_flow", 1000, 1);
 	add_topic_multi("sensor_accel", 1000, 4);
 	add_topic_multi("sensor_baro", 1000, 4);
-	add_topic_multi("sensor_gps", 1000, 3);
+	add_topic_multi("sensor_gps", 1000, 2);
 	add_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("vehicle_imu", 500, 4);
@@ -374,7 +381,6 @@ bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8
 	return true;
 }
 
-
 bool LoggedTopics::initialize_logged_topics(SDLogProfileMask profile)
 {
 	int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt");

src/modules/mavlink/mavlink_messages.cpp

@@ -67,6 +67,7 @@
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
@@ -969,8 +970,9 @@ public:
 
 private:
 	uORB::SubscriptionMultiArray<vehicle_imu_s, 3> _vehicle_imu_subs{ORB_ID::vehicle_imu};
+	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
-	uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
 	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
 	uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
@@ -1010,13 +1012,60 @@ protected:
 			vehicle_magnetometer_s magnetometer{};
 
 			if (_magnetometer_sub.update(&magnetometer)) {
-				/* mark third group dimensions as changed */
+				// mark third group dimensions as changed
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
 
 			} else {
 				_magnetometer_sub.copy(&magnetometer);
 			}
 
+			// find corresponding estimated sensor bias
+			if (_estimator_selector_status_sub.updated()) {
+				estimator_selector_status_s estimator_selector_status;
+
+				if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+					_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+				}
+			}
+
+			Vector3f accel_bias{0.f, 0.f, 0.f};
+			Vector3f gyro_bias{0.f, 0.f, 0.f};
+			Vector3f mag_bias{0.f, 0.f, 0.f};
+
+			{
+				estimator_sensor_bias_s bias;
+
+				if (_estimator_sensor_bias_sub.copy(&bias)) {
+					if ((bias.accel_device_id != 0) && (bias.accel_device_id == imu.accel_device_id)) {
+						accel_bias = Vector3f{bias.accel_bias};
+					}
+
+					if ((bias.gyro_device_id != 0) && (bias.gyro_device_id == imu.gyro_device_id)) {
+						gyro_bias = Vector3f{bias.gyro_bias};
+					}
+
+					if ((bias.mag_device_id != 0) && (bias.mag_device_id == magnetometer.device_id)) {
+						mag_bias = Vector3f{bias.mag_bias};
+
+					} else {
+						// find primary mag
+						uORB::SubscriptionMultiArray<vehicle_magnetometer_s> mag_subs{ORB_ID::vehicle_magnetometer};
+
+						for (int i = 0; i < mag_subs.size(); i++) {
+							if (mag_subs[i].advertised() && mag_subs[i].copy(&magnetometer)) {
+								if (magnetometer.device_id == bias.mag_device_id) {
+									_magnetometer_sub.ChangeInstance(i);
+									break;
+								}
+							}
+
+						}
+					}
+				}
+			}
+
+			const Vector3f mag = Vector3f{magnetometer.magnetometer_ga} - mag_bias;
+
 			vehicle_air_data_s air_data{};
 
 			if (_air_data_sub.update(&air_data)) {
@@ -1037,14 +1086,11 @@ protected:
 				_differential_pressure_sub.copy(&differential_pressure);
 			}
 
-			estimator_sensor_bias_s bias{};
-			_bias_sub.copy(&bias);
-
 			const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
-			Vector3f accel = (Vector3f{imu.delta_velocity} * accel_dt_inv) - Vector3f{bias.accel_bias};
+			const Vector3f accel = (Vector3f{imu.delta_velocity} * accel_dt_inv) - accel_bias;
 
 			const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
-			Vector3f gyro = (Vector3f{imu.delta_angle} * gyro_dt_inv) - Vector3f{bias.gyro_bias};
+			const Vector3f gyro = (Vector3f{imu.delta_angle} * gyro_dt_inv) - gyro_bias;
 
 			mavlink_highres_imu_t msg{};
 
@@ -1055,9 +1101,9 @@ protected:
 			msg.xgyro = gyro(0);
 			msg.ygyro = gyro(1);
 			msg.zgyro = gyro(2);
-			msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_bias[0];
-			msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1];
-			msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2];
+			msg.xmag = mag(0);
+			msg.ymag = mag(1);
+			msg.zmag = mag(2);
 			msg.abs_pressure = air_data.baro_pressure_pa;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
 			msg.pressure_alt = air_data.baro_alt_meter;
@@ -2916,11 +2962,12 @@ public:
 
 	unsigned get_size() override
 	{
-		return _est_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+		return _estimator_status_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
-	uORB::Subscription _est_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
 
 	/* do not allow top copying this class */
 	MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &) = delete;
@@ -2932,9 +2979,20 @@ protected:
 
 	bool send() override
 	{
+		// 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);
+				}
+			}
+		}
+
 		estimator_status_s est;
 
-		if (_est_sub.update(&est)) {
+		if (_estimator_status_sub.update(&est)) {
 			mavlink_estimator_status_t est_msg{};
 			est_msg.time_usec = est.timestamp;
 			est_msg.vel_ratio = est.vel_test_ratio;

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -43,6 +43,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/position_controller_status.h>
@@ -285,6 +286,17 @@ private:
 
 	bool write_estimator_status(mavlink_high_latency2_t *msg)
 	{
+		// 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);
+				}
+			}
+		}
+
 		estimator_status_s estimator_status;
 
 		if (_estimator_status_sub.update(&estimator_status)) {
@@ -618,6 +630,7 @@ private:
 	uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)};
 	uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -53,7 +53,7 @@ using namespace matrix;
 
 MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_vtol(vtol)

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -69,7 +69,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/uORB.h>
 
 using matrix::Dcmf;

src/modules/sensors/sensors.cpp

@@ -536,7 +536,8 @@ void Sensors::InitializeVehicleIMU()
 			if (accel.device_id > 0 && gyro.device_id > 0) {
 				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
 				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
-				const px4::wq_config_t &wq_config = px4::wq_configurations::nav_and_controllers;
+				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
+				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0;
 
 				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -65,7 +65,8 @@ bool VehicleAcceleration::Start()
 	}
 
 	if (!SensorSelectionUpdate(true)) {
-		ScheduleDelayed(10_ms);
+		_selected_sensor_sub_index = 0;
+		_sensor_sub.registerCallback();
 	}
 
 	return true;
@@ -137,6 +138,15 @@ void VehicleAcceleration::CheckFilters()
 
 void VehicleAcceleration::SensorBiasUpdate(bool force)
 {
+	// find corresponding estimated sensor bias
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+		}
+	}
+
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -44,6 +44,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
@@ -56,7 +57,6 @@ namespace sensors
 class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-
 	VehicleAcceleration();
 	~VehicleAcceleration() override;
 
@@ -77,6 +77,7 @@ private:
 
 	uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -137,7 +137,7 @@ void VehicleAirData::Run()
 
 		if (!_advertised[uorb_index]) {
 			// use data's timestamp to throttle advertisement checks
-			if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
+			if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
 				if (_sensor_sub[uorb_index].advertised()) {
 					if (uorb_index > 0) {
 						/* the first always exists, but for each further sensor, add a new validator */
@@ -151,12 +151,17 @@ void VehicleAirData::Run()
 					// force temperature correction update
 					SensorCorrectionsUpdate(true);
 
+					if (_selected_sensor_sub_index < 0) {
+						_sensor_sub[uorb_index].registerCallback();
+					}
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
 			}
+		}
 
-		} else {
+		if (_advertised[uorb_index]) {
 			updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
 
 			if (updated[uorb_index]) {
@@ -204,7 +209,8 @@ void VehicleAirData::Run()
 		_baro_sum_count++;
 
 		if ((_param_sens_baro_rate.get() > 0)
-		    && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) {
+		    && ((_last_publication_timestamp == 0)
+			|| (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) {
 
 			const float pressure = _baro_sum / _baro_sum_count;
 			const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -68,7 +68,8 @@ bool VehicleAngularVelocity::Start()
 	}
 
 	if (!SensorSelectionUpdate(true)) {
-		ScheduleDelayed(10_ms);
+		_selected_sensor_sub_index = 0;
+		_sensor_sub.registerCallback();
 	}
 
 	return true;
@@ -157,6 +158,15 @@ void VehicleAngularVelocity::CheckFilters()
 
 void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 {
+	// find corresponding estimated sensor bias
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+		}
+	}
+
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 
@@ -321,6 +331,7 @@ void VehicleAngularVelocity::PrintStatus()
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
 	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+
 	_calibration.PrintStatus();
 }
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -45,6 +45,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
@@ -58,7 +59,6 @@ namespace sensors
 class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-
 	VehicleAngularVelocity();
 	~VehicleAngularVelocity() override;
 
@@ -80,6 +80,7 @@ private:
 	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -199,7 +199,7 @@ void VehicleMagnetometer::Run()
 
 		if (!_advertised[uorb_index]) {
 			// use data's timestamp to throttle advertisement checks
-			if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
+			if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
 				if (_sensor_sub[uorb_index].advertised()) {
 					if (uorb_index > 0) {
 						/* the first always exists, but for each further sensor, add a new validator */
@@ -217,12 +217,18 @@ void VehicleMagnetometer::Run()
 						}
 					}
 
+					if (_selected_sensor_sub_index < 0) {
+						_sensor_sub[uorb_index].registerCallback();
+					}
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
 			}
 
-		} else {
+		}
+
+		if (_advertised[uorb_index]) {
 			sensor_mag_s report;
 
 			while (_sensor_sub[uorb_index].update(&report)) {
@@ -339,8 +345,8 @@ void VehicleMagnetometer::Run()
 
 void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
 {
-	if ((_param_sens_mag_rate.get() > 0)
-	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
+	if ((_param_sens_mag_rate.get() > 0) && (_last_publication_timestamp[instance] ||
+			(hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())))) {
 
 		const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance];
 		const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance];

src/modules/sensors/voted_sensors_update.cpp

@@ -374,14 +374,19 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 	status.accel_device_id_primary = _selection.accel_device_id;
 	status.gyro_device_id_primary = _selection.gyro_device_id;
 
+	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof(
+			sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
+	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(
+			sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
+
 	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-		if (_accel_device_id[i] != 0) {
+		if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) {
 			status.accel_device_ids[i] = _accel_device_id[i];
 			status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
 			status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
 		}
 
-		if (_gyro_device_id[i] != 0) {
+		if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) {
 			status.gyro_device_ids[i] = _gyro_device_id[i];
 			status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
 			status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
@@ -403,32 +408,50 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
 
 void VotedSensorsUpdate::calcAccelInconsistency()
 {
-	const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
+	Vector3f accel_mean{};
+	Vector3f accel_all[MAX_SENSOR_COUNT] {};
+	uint8_t accel_count = 0;
 
-	// Check each sensor against the primary
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		// check that the sensor we are checking against is not the same as the primary
-		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
-		    && (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
+		if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
+			accel_count++;
+			accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2};
+			accel_mean += accel_all[sensor_index];
+		}
+	}
 
-			const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2};
-			_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
+	if (accel_count > 0) {
+		accel_mean /= accel_count;
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
+				_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean);
+			}
 		}
 	}
 }
 
 void VotedSensorsUpdate::calcGyroInconsistency()
 {
-	const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
+	Vector3f gyro_mean{};
+	Vector3f gyro_all[MAX_SENSOR_COUNT] {};
+	uint8_t gyro_count = 0;
 
-	// Check each sensor against the primary
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		// check that the sensor we are checking against is not the same as the primary
-		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
-		    && (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
+		if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
+			gyro_count++;
+			gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad};
+			gyro_mean += gyro_all[sensor_index];
+		}
+	}
 
-			const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad};
-			_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
+	if (gyro_count > 0) {
+		gyro_mean /= gyro_count;
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
+				_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean);
+			}
 		}
 	}
 }