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,
+        ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
-        amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.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
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
-        print('found estimator_states 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
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
-        print('found estimator_status 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
+        _ = ulog.get_dataset('estimator_innovations', multi_instance).data
-        print('found estimator_innovations 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,
+        in_air_no_ground_effects: InAirDetector, multi_instance: int = 0,
-        amb_thresh: float = 0.5) -> Dict[str, float]:
+        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(
+def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict:
-        ulog: ULog, 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
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
-        print('found estimator_status 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
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
-        print('found estimator_states 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_innovations = ulog.get_dataset('estimator_innovations', multi_instance).data
-        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').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,16 +115,21 @@ 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
+
+    for multi_instance in range(ekf_instances):
+
+        print('\nestimator instance:', multi_instance)
+
         # 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,
+            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)
 
         test_results = create_results_table(
             check_table_filename, master_status, check_status, metrics, airtime_info)
 
         # write metadata to a .csv file
-    with open('{:s}.mdat.csv'.format(filename), "w") as file:
+        with open('{:s}-{:d}.mdat.csv'.format(filename, multi_instance), "w") as file:
 
             file.write("name,value,description\n")
 
@@ -119,11 +139,11 @@ def process_logdata_ekf(
             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))
+        print('Test results written to {:s}-{:d}.mdat.csv'.format(filename, multi_instance))
 
         if plot:
-        create_pdf_report(ulog, '{:s}.pdf'.format(filename))
+            create_pdf_report(ulog, multi_instance, '{:s}-{:d}.pdf'.format(filename, multi_instance))
-        print('Plots saved to {:s}.pdf'.format(filename))
+            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
+uint32[4] accel_device_ids
-float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s).
+float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in (m/s/s).
-bool[3] accel_healthy
+bool[4] accel_healthy
 
 
 uint32 gyro_device_id_primary       # current primary gyro device id for reference
 
-uint32[3] gyro_device_ids
+uint32[4] gyro_device_ids
-float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s).
+float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s).
-bool[3] gyro_healthy
+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", 1728, -13};
-static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 7200, -14};
 
-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 uavcan{"wq:uavcan", 3000, -19};
-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 UART0{"wq:UART0", 1400, -21};
-static constexpr wq_config_t UART3{"wq:UART3", 1400, -20};
+static constexpr wq_config_t UART1{"wq:UART1", 1400, -22};
-static constexpr wq_config_t UART4{"wq:UART4", 1400, -21};
+static constexpr wq_config_t UART2{"wq:UART2", 1400, -23};
-static constexpr wq_config_t UART5{"wq:UART5", 1400, -22};
+static constexpr wq_config_t UART3{"wq:UART3", 1400, -24};
-static constexpr wq_config_t UART6{"wq:UART6", 1400, -23};
+static constexpr wq_config_t UART4{"wq:UART4", 1400, -25};
-static constexpr wq_config_t UART7{"wq:UART7", 1400, -24};
+static constexpr wq_config_t UART5{"wq:UART5", 1400, -26};
-static constexpr wq_config_t UART8{"wq:UART8", 1400, -25};
+static constexpr wq_config_t UART6{"wq:UART6", 1400, -27};
-static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26};
+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.cpp

@@ -37,11 +37,25 @@ using namespace time_literals;
 
 using math::constrain;
 
-EKF2::EKF2(bool replay_mode):
+pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
+static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
+#if !defined(CONSTRAINED_FLASH)
+static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
+#endif // !CONSTRAINED_FLASH
+
+EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool replay_mode):
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	ScheduledWorkItem(MODULE_NAME, config),
-	_replay_mode(replay_mode),
+	_replay_mode(replay_mode && instance < 0),
+	_multi_mode(instance >= 0),
+	_instance(math::constrain(instance, 0, EKF2_MAX_INSTANCES - 1)),
 	_ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")),
+	_attitude_pub(_multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
+	_local_position_pub(_multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
+	_global_position_pub(_multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
+	_odometry_pub(_multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
+	_visual_odometry_aligned_pub(_multi_mode ? ORB_ID(estimator_visual_odometry_aligned) :
+				     ORB_ID(vehicle_visual_odometry_aligned)),
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
 	_param_ekf2_mag_delay(_params->mag_delay_ms),
@@ -153,7 +167,12 @@ EKF2::EKF2(bool replay_mode):
 	_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
 
 	// advertise immediately to ensure consistent uORB instance numbering
-	_att_pub.advertise();
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
+	_global_position_pub.advertise();
+	_odometry_pub.advertise();
+	_visual_odometry_aligned_pub.advertise();
+
 	_ekf2_timestamps_pub.advertise();
 	_ekf_gps_drift_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
@@ -163,65 +182,29 @@ EKF2::EKF2(bool replay_mode):
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
 	_estimator_status_pub.advertise();
-	_vehicle_global_position_pub.advertise();
-	_vehicle_local_position_pub.advertise();
-	_vehicle_odometry_pub.advertise();
-	_vehicle_visual_odometry_aligned_pub.advertise();
 	_wind_pub.advertise();
 	_yaw_est_pub.advertise();
+
+	if (_multi_mode) {
+		_vehicle_imu_sub.ChangeInstance(imu);
+		_magnetometer_sub.ChangeInstance(mag);
+	}
 }
 
 EKF2::~EKF2()
 {
+	if (!_multi_mode) {
 		px4_lockstep_unregister_component(_lockstep_component);
+	}
+
 	perf_free(_ekf_update_perf);
 }
 
-bool EKF2::init()
-{
-	const uint32_t device_id = _param_ekf2_imu_id.get();
-
-	// if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined)
-	if (device_id != 0) {
-		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-			vehicle_imu_s imu;
-
-			if (_vehicle_imu_sub.ChangeInstance(i) && _vehicle_imu_sub.copy(&imu)) {
-				if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) {
-					if (_vehicle_imu_sub.registerCallback()) {
-						PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id);
-						_imu_sub_index = i;
-						_callback_registered = true;
-						return true;
-					}
-				}
-			}
-		}
-
-	} else {
-		_imu_sub_index = -1;
-
-		if (_sensor_combined_sub.registerCallback()) {
-			_callback_registered = true;
-			return true;
-		}
-	}
-
-	PX4_WARN("failed to register callback, retrying in 1 second");
-	ScheduleDelayed(1_s); // retry in 1 second
-
-	return true;
-}
-
 int EKF2::print_status()
 {
-	PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid");
+	PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(),
-	PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid");
+		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
-
-	PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us);
-
 	perf_print_counter(_ekf_update_perf);
-
 	return 0;
 }
 
@@ -238,7 +221,11 @@ void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index)
 		_last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved;
 
 		mag_bias_param.set(_last_valid_mag_cal[axis_index]);
+
+		// save new parameters unless in multi-instance mode
+		if (!_multi_mode) {
 			mag_bias_param.commit_no_notification();
+		}
 
 		_valid_cal_available[axis_index] = false;
 	}
@@ -252,7 +239,11 @@ bool EKF2::update_mag_decl(Param &mag_decl_param)
 
 	if (_ekf.get_mag_decl_deg(&declination_deg)) {
 		mag_decl_param.set(declination_deg);
+
+		if (!_multi_mode) {
 			mag_decl_param.commit_no_notification();
+		}
+
 		return true;
 	}
 
@@ -265,21 +256,30 @@ void EKF2::Run()
 		_sensor_combined_sub.unregisterCallback();
 		_vehicle_imu_sub.unregisterCallback();
 
-		exit_and_cleanup();
 		return;
 	}
 
 	if (!_callback_registered) {
-		init();
+		if (_multi_mode) {
+			_callback_registered = _vehicle_imu_sub.registerCallback();
+
+		} else {
+			_callback_registered = _sensor_combined_sub.registerCallback();
+		}
+
+		if (!_callback_registered) {
+			PX4_WARN("%d failed to register callback, retrying", _instance);
+			ScheduleDelayed(1_s);
 			return;
 		}
+	}
 
 	bool updated = false;
 	imuSample imu_sample_new {};
 
 	hrt_abstime imu_dt = 0; // for tracking time slip later
 
-	if (_imu_sub_index >= 0) {
+	if (_multi_mode) {
 		vehicle_imu_s imu;
 		updated = _vehicle_imu_sub.update(&imu);
 
@@ -357,13 +357,12 @@ void EKF2::Run()
 		}
 
 		// Always update sensor selction first time through if time stamp is non zero
-		if (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0)) {
+		if (!_multi_mode && (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0))) {
 			const sensor_selection_s sensor_selection_prev = _sensor_selection;
 
 			if (_sensor_selection_sub.copy(&_sensor_selection)) {
 				if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
 
-					if (_imu_sub_index < 0) {
 					if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
 						_imu_bias_reset_request = true;
 					}
@@ -372,7 +371,6 @@ void EKF2::Run()
 						_imu_bias_reset_request = true;
 					}
 				}
-				}
 
 				_estimator_status_pub.get().accel_device_id = _sensor_selection.accel_device_id;
 				_estimator_status_pub.get().gyro_device_id = _sensor_selection.gyro_device_id;
@@ -417,22 +415,27 @@ void EKF2::Run()
 				if ((_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) {
 					// the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID
 					// this means we need to reset the learned bias values to zero
+					_param_ekf2_magbias_x.set(0.f);
+					_param_ekf2_magbias_y.set(0.f);
+					_param_ekf2_magbias_z.set(0.f);
+					_param_ekf2_magbias_id.set(magnetometer.device_id);
+
+					if (!_multi_mode) {
 						_param_ekf2_magbias_x.reset();
 						_param_ekf2_magbias_y.reset();
 						_param_ekf2_magbias_z.reset();
-					_param_ekf2_magbias_id.set(magnetometer.device_id);
 						_param_ekf2_magbias_id.commit();
+						PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
+					}
 
 					_invalid_mag_id_count = 0;
-
-					PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
 				}
 
 				magSample mag_sample {};
 				mag_sample.mag(0) = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get();
 				mag_sample.mag(1) = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get();
 				mag_sample.mag(2) = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get();
-				mag_sample.time_us = magnetometer.timestamp;
+				mag_sample.time_us = magnetometer.timestamp_sample;
 
 				_ekf.setMagData(mag_sample);
 				ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
@@ -681,15 +684,15 @@ void EKF2::Run()
 			// only publish position after successful alignment
 			if (control_status.flags.tilt_align) {
 				// generate vehicle local position data
-				vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
+				vehicle_local_position_s &lpos = _local_position_pub.get();
 				lpos.timestamp_sample = imu_sample_new.time_us;
 
 				// Position of body origin in local NED frame
-				Vector3f position = _ekf.getPosition();
+				const Vector3f position = _ekf.getPosition();
 				const float lpos_x_prev = lpos.x;
 				const float lpos_y_prev = lpos.y;
-				lpos.x = (_ekf.local_position_is_valid()) ? position(0) : 0.0f;
+				lpos.x = position(0);
-				lpos.y = (_ekf.local_position_is_valid()) ? position(1) : 0.0f;
+				lpos.y = position(1);
 				lpos.z = position(2);
 
 				// Velocity of body origin in local NED frame (m/s)
@@ -801,7 +804,7 @@ void EKF2::Run()
 
 				// publish vehicle local position data
 				lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
-				_vehicle_local_position_pub.update();
+				_local_position_pub.update();
 
 				// publish vehicle_odometry
 				publish_odometry(now, imu_sample_new, lpos);
@@ -848,12 +851,12 @@ void EKF2::Run()
 					ev_quat_aligned.copyTo(aligned_ev_odom.q);
 					quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
 
-					_vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom);
+					_visual_odometry_aligned_pub.publish(aligned_ev_odom);
 				}
 
 				if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
 					// generate and publish global position data
-					vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
+					vehicle_global_position_s &global_pos = _global_position_pub.get();
 					global_pos.timestamp_sample = imu_sample_new.time_us;
 
 					if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) {
@@ -881,7 +884,7 @@ void EKF2::Run()
 
 					global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
 					global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
-					_vehicle_global_position_pub.update();
+					_global_position_pub.update();
 				}
 			}
 
@@ -909,8 +912,8 @@ void EKF2::Run()
 							status.hgt_test_ratio, status.tas_test_ratio,
 							status.hagl_test_ratio, status.beta_test_ratio);
 
-			status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
+			status.pos_horiz_accuracy = _local_position_pub.get().eph;
-			status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
+			status.pos_vert_accuracy = _local_position_pub.get().epv;
 			_ekf.get_ekf_soln_status(&status.solution_status_flags);
 			_ekf.getImuVibrationMetrics().copyTo(status.vibe);
 			status.time_slip = _last_time_slip_us * 1e-6f;
@@ -922,7 +925,8 @@ void EKF2::Run()
 			status.timestamp = _replay_mode ? now : hrt_absolute_time();
 			_estimator_status_pub.update();
 
-			{
+			// estimator_sensor_bias
+			if (status.filter_fault_flags == 0) {
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
 				estimator_sensor_bias_s bias;
 				bias.timestamp_sample = imu_sample_new.time_us;
@@ -1143,12 +1147,14 @@ void EKF2::Run()
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 
+		if (!_multi_mode) {
 			if (_lockstep_component == -1) {
 				_lockstep_component = px4_lockstep_register_component();
 			}
 
 			px4_lockstep_progress(_lockstep_component);
 		}
+	}
 }
 
 void EKF2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
@@ -1221,13 +1227,13 @@ void EKF2::publish_attitude(const hrt_abstime &timestamp)
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-		_att_pub.publish(att);
+		_attitude_pub.publish(att);
 
 	}  else if (_replay_mode) {
 		// in replay mode we have to tell the replay module not to wait for an update
 		// we do this by publishing an attitude with zero timestamp
 		vehicle_attitude_s att{};
-		_att_pub.publish(att);
+		_attitude_pub.publish(att);
 	}
 }
 
@@ -1293,7 +1299,7 @@ void EKF2::publish_odometry(const hrt_abstime &timestamp, const imuSample &imu,
 
 	// publish vehicle odometry data
 	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-	_vehicle_odometry_pub.publish(odom);
+	_odometry_pub.publish(odom);
 }
 
 void EKF2::publish_yaw_estimator_status(const hrt_abstime &timestamp)
@@ -1383,6 +1389,7 @@ int EKF2::custom_command(int argc, char *argv[])
 
 int EKF2::task_spawn(int argc, char *argv[])
 {
+	bool success = false;
 	bool replay_mode = false;
 
 	if (argc > 1 && !strcmp(argv[1], "-r")) {
@@ -1390,25 +1397,140 @@ int EKF2::task_spawn(int argc, char *argv[])
 		replay_mode = true;
 	}
 
-	EKF2 *instance = new EKF2(replay_mode);
+#if !defined(CONSTRAINED_FLASH)
+	bool multi_mode = false;
+	int32_t imu_instances = 0;
+	int32_t mag_instances = 0;
 
-	if (instance) {
+	int32_t sens_imu_mode = 1;
-		_object.store(instance);
+	param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
-		_task_id = task_id_is_work_queue;
 
-		if (instance->init()) {
+	if (sens_imu_mode == 0) {
-			return PX4_OK;
+		// ekf selector requires SENS_IMU_MODE = 0
+		multi_mode = true;
+
+		// IMUs (1 - 3 supported)
+		param_get(param_find("EKF2_MULTI_IMU"), &imu_instances);
+
+		if (imu_instances < 1 || imu_instances > 3) {
+			const int32_t imu_instances_limited = math::constrain(imu_instances, 1, 3);
+			PX4_WARN("EKF2_MULTI_IMU limited %d -> %d", imu_instances, imu_instances_limited);
+			param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
+			imu_instances = imu_instances_limited;
+		}
+
+		int32_t sens_mag_mode = 1;
+		param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
+
+		if (sens_mag_mode == 0) {
+			param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
+
+			// Mags (1 - 4 supported)
+			if (mag_instances < 1 || mag_instances > 4) {
+				const int32_t mag_instances_limited = math::constrain(mag_instances, 1, 4);
+				PX4_WARN("EKF2_MULTI_MAG limited %d -> %d", mag_instances, mag_instances_limited);
+				param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
+				mag_instances = mag_instances_limited;
 			}
 
 		} else {
-		PX4_ERR("alloc failed");
+			mag_instances = 1;
+		}
 	}
 
-	delete instance;
+	if (multi_mode) {
-	_object.store(nullptr);
+		// Start EKF2Selector if it's not already running
-	_task_id = -1;
+		if (_ekf2_selector.load() == nullptr) {
+			EKF2Selector *inst = new EKF2Selector();
 
-	return PX4_ERROR;
+			if (inst) {
+				_ekf2_selector.store(inst);
+				inst->Start();
+
+			} else {
+				PX4_ERR("Failed to start EKF2 selector");
+			}
+		}
+
+		const hrt_abstime time_started = hrt_absolute_time();
+		const int multi_instances = math::min(imu_instances * mag_instances, (int)EKF2_MAX_INSTANCES);
+		int multi_instances_allocated = 0;
+
+		// allocate EKF2 instances until all found or arming
+		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
+
+		while ((multi_instances_allocated < multi_instances)
+		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
+		       && (hrt_elapsed_time(&time_started) < 30_s)) {
+
+			vehicle_status_sub.update();
+
+			for (uint8_t mag = 0; mag < mag_instances; mag++) {
+				uORB::SubscriptionData<vehicle_magnetometer_s> vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag};
+
+				for (uint8_t imu = 0; imu < imu_instances; imu++) {
+
+					uORB::SubscriptionData<vehicle_imu_s> vehicle_imu_sub{ORB_ID(vehicle_imu), imu};
+					vehicle_mag_sub.update();
+
+					// Mag & IMU data must be valid, first mag can be ignored initially
+					if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
+					    && (vehicle_imu_sub.get().accel_device_id != 0)
+					    && (vehicle_imu_sub.get().gyro_device_id != 0)) {
+
+						const int instance = imu + mag * imu_instances;
+
+						if (_objects[instance].load() == nullptr) {
+							EKF2 *ekf2_inst = new EKF2(instance, px4::ins_instance_to_wq(imu), imu, mag, false);
+
+							if (ekf2_inst) {
+								PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", instance,
+									 imu, vehicle_imu_sub.get().accel_device_id,
+									 mag, vehicle_mag_sub.get().device_id);
+
+								_objects[instance].store(ekf2_inst);
+								ekf2_inst->ScheduleNow();
+								success = true;
+								multi_instances_allocated++;
+
+							} else {
+								PX4_ERR("instance %d alloc failed", instance);
+								px4_usleep(1000000);
+								break;
+							}
+						}
+
+					} else {
+						px4_usleep(50000); // give the sensors extra time to start
+						continue;
+					}
+				}
+			}
+
+			if (multi_instances_allocated < multi_instances) {
+				px4_usleep(100000);
+			}
+		}
+
+	}
+
+#endif // !CONSTRAINED_FLASH
+
+	else {
+		// otherwise launch regular
+		int instance = -1;
+		int imu = 0;
+		int mag = 0;
+		EKF2 *ekf2_inst = new EKF2(instance, px4::wq_configurations::INS0, imu, mag, replay_mode);
+
+		if (ekf2_inst) {
+			_objects[0].store(ekf2_inst);
+			ekf2_inst->ScheduleNow();
+			success = true;
+		}
+	}
+
+	return success ? PX4_OK : PX4_ERROR;
 }
 
 int EKF2::print_usage(const char *reason)
@@ -1439,5 +1561,104 @@ timestamps from the sensor topics.
 
 extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 {
-	return EKF2::main(argc, argv);
+	if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
+		return EKF2::print_usage();
+	}
+
+	if (strcmp(argv[1], "start") == 0) {
+		int ret = 0;
+		EKF2::lock_module();
+
+		ret = EKF2::task_spawn(argc - 1, argv + 1);
+
+		if (ret < 0) {
+			PX4_ERR("start failed (%i)", ret);
+		}
+
+		EKF2::unlock_module();
+		return ret;
+
+	} else if (strcmp(argv[1], "status") == 0) {
+		if (EKF2::trylock_module()) {
+#if !defined(CONSTRAINED_FLASH)
+			if (_ekf2_selector.load()) {
+				_ekf2_selector.load()->PrintStatus();
+			}
+#endif // !CONSTRAINED_FLASH
+
+			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+				if (_objects[i].load()) {
+					PX4_INFO_RAW("\n");
+					_objects[i].load()->print_status();
+				}
+			}
+
+			EKF2::unlock_module();
+
+		} else {
+			PX4_WARN("module locked, try again later");
+		}
+
+		return 0;
+
+	} else if (strcmp(argv[1], "stop") == 0) {
+		EKF2::lock_module();
+
+		if (argc > 2) {
+			int instance = atoi(argv[2]);
+
+			PX4_INFO("stopping %d", instance);
+
+			if (instance > 0 && instance < EKF2_MAX_INSTANCES) {
+				EKF2 *inst = _objects[instance].load();
+
+				if (inst) {
+					inst->request_stop();
+					px4_usleep(20000); // 20 ms
+					delete inst;
+					_objects[instance].store(nullptr);
+				}
+			}
+
+		} else {
+			// otherwise stop everything
+			bool was_running = false;
+
+#if !defined(CONSTRAINED_FLASH)
+			if (_ekf2_selector.load()) {
+				PX4_INFO("stopping ekf2 selector");
+				_ekf2_selector.load()->Stop();
+				delete _ekf2_selector.load();
+				_ekf2_selector.store(nullptr);
+				was_running = true;
+			}
+#endif // !CONSTRAINED_FLASH
+
+			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+				EKF2 *inst = _objects[i].load();
+
+				if (inst) {
+					PX4_INFO("stopping ekf2 instance %d", i);
+					was_running = true;
+					inst->request_stop();
+					px4_usleep(20000); // 20 ms
+					delete inst;
+					_objects[i].store(nullptr);
+				}
+			}
+
+			if (!was_running) {
+				PX4_WARN("not running");
+			}
+		}
+
+		EKF2::unlock_module();
+		return PX4_OK;
+	}
+
+	EKF2::lock_module(); // Lock here, as the method could access _object.
+	int ret = EKF2::custom_command(argc - 1, argv + 1);
+	EKF2::unlock_module();
+
+	return ret;
 }

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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<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::PublicationMulti<estimator_states_s>		_estimator_states_pub{ORB_ID(estimator_states)};
-	uORB::PublicationData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
+	uORB::PublicationMultiData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
-	uORB::Publication<vehicle_attitude_s>			_att_pub{ORB_ID(vehicle_attitude)};
+	uORB::PublicationMulti<yaw_estimator_status_s>		_yaw_est_pub{ORB_ID(yaw_estimator_status)};
-	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<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.cpp

@@ -0,0 +1,628 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "EKF2Selector.hpp"
+
+using namespace time_literals;
+using matrix::Quatf;
+using matrix::Vector2f;
+using math::constrain;
+using math::max;
+using math::radians;
+
+EKF2Selector::EKF2Selector() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers)
+{
+}
+
+EKF2Selector::~EKF2Selector()
+{
+	Stop();
+
+	px4_lockstep_unregister_component(_lockstep_component);
+}
+
+bool EKF2Selector::Start()
+{
+	// default to first instance
+	_selected_instance = 0;
+	_instance[0].estimator_status_sub.registerCallback();
+	_instance[0].estimator_attitude_sub.registerCallback();
+
+	return true;
+}
+
+void EKF2Selector::Stop()
+{
+	for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+		_instance[i].estimator_attitude_sub.unregisterCallback();
+		_instance[i].estimator_status_sub.unregisterCallback();
+	}
+
+	ScheduleClear();
+}
+
+void EKF2Selector::SelectInstance(uint8_t ekf_instance)
+{
+	if (ekf_instance != _selected_instance) {
+
+		// update sensor_selection immediately
+		sensor_selection_s sensor_selection{};
+		sensor_selection.accel_device_id = _instance[ekf_instance].estimator_status.accel_device_id;
+		sensor_selection.gyro_device_id = _instance[ekf_instance].estimator_status.gyro_device_id;
+		sensor_selection.timestamp = hrt_absolute_time();
+		_sensor_selection_pub.publish(sensor_selection);
+
+		if (_selected_instance != INVALID_INSTANCE) {
+			// switch callback registration
+			_instance[_selected_instance].estimator_attitude_sub.unregisterCallback();
+			_instance[_selected_instance].estimator_status_sub.unregisterCallback();
+
+			if (!_instance[_selected_instance].healthy) {
+				PX4_WARN("primary EKF changed %d (unhealthy) -> %d", _selected_instance, ekf_instance);
+			}
+		}
+
+		_instance[ekf_instance].estimator_attitude_sub.registerCallback();
+		_instance[ekf_instance].estimator_status_sub.registerCallback();
+
+		_selected_instance = ekf_instance;
+		_instance_changed_count++;
+		_last_instance_change = sensor_selection.timestamp;
+		_instance[ekf_instance].time_last_selected = _last_instance_change;
+
+		// reset all relative test ratios
+		for (auto &inst : _instance) {
+			inst.relative_test_ratio = 0;
+		}
+
+		// publish new data immediately with resets
+		PublishVehicleAttitude(true);
+		PublishVehicleLocalPosition(true);
+		PublishVehicleGlobalPosition(true);
+	}
+}
+
+bool EKF2Selector::UpdateErrorScores()
+{
+	bool updated = false;
+
+	// first check imu inconsistencies
+	_gyro_fault_detected = false;
+	uint32_t faulty_gyro_id = 0;
+	_accel_fault_detected = false;
+	uint32_t faulty_accel_id = 0;
+
+	if (_sensors_status_imu.updated()) {
+		sensors_status_imu_s sensors_status_imu;
+
+		if (_sensors_status_imu.copy(&sensors_status_imu)) {
+			const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get());
+			const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get());
+			const float accel_threshold = _param_ekf2_sel_imu_accel.get();
+			const float velocity_threshold = _param_ekf2_sel_imu_velocity.get();
+			const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f);
+			_last_update_us = sensors_status_imu.timestamp;
+
+			uint8_t n_gyros = 0;
+			uint8_t n_accels = 0;
+			uint8_t n_gyro_exceedances = 0;
+			uint8_t n_accel_exceedances = 0;
+			float largest_accumulated_gyro_error = 0.0f;
+			float largest_accumulated_accel_error = 0.0f;
+			uint8_t largest_gyro_error_index = 0;
+			uint8_t largest_accel_error_index = 0;
+
+			for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) {
+				// check for gyros with excessive difference to mean using accumulated error
+				if (sensors_status_imu.gyro_device_ids[i] != 0) {
+					n_gyros++;
+					_accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s;
+					_accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f);
+
+					if (_accumulated_gyro_error[i] > angle_threshold) {
+						n_gyro_exceedances++;
+					}
+
+					if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) {
+						largest_accumulated_gyro_error = _accumulated_gyro_error[i];
+						largest_gyro_error_index = i;
+					}
+
+				} else {
+					// no sensor
+					_accumulated_gyro_error[i] = 0.f;
+				}
+
+				// check for accelerometers with excessive difference to mean using accumulated error
+				if (sensors_status_imu.accel_device_ids[i] != 0) {
+					n_accels++;
+					_accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s;
+					_accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f);
+
+					if (_accumulated_accel_error[i] > velocity_threshold) {
+						n_accel_exceedances++;
+					}
+
+					if (_accumulated_accel_error[i] > largest_accumulated_accel_error) {
+						largest_accumulated_accel_error = _accumulated_accel_error[i];
+						largest_accel_error_index = i;
+					}
+
+				} else {
+					// no sensor
+					_accumulated_accel_error[i] = 0.f;
+				}
+			}
+
+			if (n_gyro_exceedances > 0) {
+				if (n_gyros >= 3) {
+					// If there are 3 or more sensors, the one with the largest accumulated error is faulty
+					_gyro_fault_detected = true;
+					faulty_gyro_id = _instance[largest_gyro_error_index].estimator_status.gyro_device_id;
+
+				} else if (n_gyros == 2) {
+					// A fault is present, but the faulty sensor identity cannot be determined
+					_gyro_fault_detected = true;
+				}
+			}
+
+			if (n_accel_exceedances > 0) {
+				if (n_accels >= 3) {
+					// If there are 3 or more sensors, the one with the largest accumulated error is faulty
+					_accel_fault_detected = true;
+					faulty_accel_id = _instance[largest_accel_error_index].estimator_status.accel_device_id;
+
+				} else if (n_accels == 2) {
+					// A fault is present, but the faulty sensor identity cannot be determined
+					_accel_fault_detected = true;
+				}
+			}
+		}
+	}
+
+
+	bool primary_updated = false;
+
+	// calculate individual error scores
+	for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
+		const bool prev_healthy = _instance[i].healthy;
+
+		if (_instance[i].estimator_status_sub.update(&_instance[i].estimator_status)) {
+
+			if ((i + 1) > _available_instances) {
+				_available_instances = i + 1;
+				updated = true;
+			}
+
+			if (i == _selected_instance) {
+				primary_updated = true;
+			}
+
+			const estimator_status_s &status = _instance[i].estimator_status;
+
+			const bool tilt_align = status.control_mode_flags & (1 << estimator_status_s::CS_TILT_ALIGN);
+			const bool yaw_align = status.control_mode_flags & (1 << estimator_status_s::CS_YAW_ALIGN);
+
+			_instance[i].combined_test_ratio = 0.0f;
+
+			if (tilt_align && yaw_align) {
+				_instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio,
+								   0.5f * (status.vel_test_ratio + status.pos_test_ratio));
+				_instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio, status.hgt_test_ratio);
+			}
+
+			_instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0);
+
+		} else if (hrt_elapsed_time(&_instance[i].estimator_status.timestamp) > 20_ms) {
+			_instance[i].healthy = false;
+		}
+
+		// if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay
+		if (_gyro_fault_detected && _instance[i].estimator_status.gyro_device_id == faulty_gyro_id) {
+			_instance[i].healthy = false;
+		}
+
+		// if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay
+		if (_accel_fault_detected && _instance[i].estimator_status.accel_device_id == faulty_accel_id) {
+			_instance[i].healthy = false;
+		}
+
+		if (prev_healthy != _instance[i].healthy) {
+			updated = true;
+		}
+	}
+
+	// update relative test ratios if primary has updated
+	if (primary_updated) {
+		for (uint8_t i = 0; i < _available_instances; i++) {
+			if (i != _selected_instance) {
+
+				const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio;
+
+				// reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes
+				const float threshold = _gyro_fault_detected ? fmaxf(_param_ekf2_sel_err_red.get(), 0.05f) : 0.0f;
+
+				if (error_delta > 0 || error_delta < -threshold) {
+					_instance[i].relative_test_ratio += error_delta;
+					_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim);
+				}
+			}
+		}
+	}
+
+	return (primary_updated || updated);
+}
+
+void EKF2Selector::PublishVehicleAttitude(bool reset)
+{
+	vehicle_attitude_s attitude;
+
+	if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) {
+		if (reset) {
+			// on reset compute deltas from last published data
+			++_quat_reset_counter;
+
+			_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
+
+			// ensure monotonically increasing timestamp_sample through reset
+			attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample);
+
+		} else {
+			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+			if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) {
+				++_quat_reset_counter;
+				_delta_q_reset = Quatf{attitude.delta_q_reset};
+			}
+		}
+
+		// save last primary estimator_attitude
+		_attitude_last = attitude;
+
+		// republish with total reset count and current timestamp
+		attitude.quat_reset_counter = _quat_reset_counter;
+		_delta_q_reset.copyTo(attitude.delta_q_reset);
+
+		attitude.timestamp = hrt_absolute_time();
+		_vehicle_attitude_pub.publish(attitude);
+	}
+}
+
+void EKF2Selector::PublishVehicleLocalPosition(bool reset)
+{
+	// vehicle_local_position
+	vehicle_local_position_s local_position;
+
+	if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) {
+		if (reset) {
+			// on reset compute deltas from last published data
+			++_xy_reset_counter;
+			++_z_reset_counter;
+			++_vxy_reset_counter;
+			++_vz_reset_counter;
+			++_heading_reset_counter;
+
+			_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
+			_delta_z_reset = local_position.z - _local_position_last.z;
+			_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
+			_delta_vz_reset = local_position.vz - _local_position_last.vz;
+			_delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading);
+
+			// ensure monotonically increasing timestamp_sample through reset
+			local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample);
+
+		} else {
+			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+
+			// XY reset
+			if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) {
+				++_xy_reset_counter;
+				_delta_xy_reset = Vector2f{local_position.delta_xy};
+			}
+
+			// Z reset
+			if (local_position.z_reset_counter > _local_position_last.z_reset_counter) {
+				++_z_reset_counter;
+				_delta_z_reset = local_position.delta_z;
+			}
+
+			// VXY reset
+			if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) {
+				++_vxy_reset_counter;
+				_delta_vxy_reset = Vector2f{local_position.delta_vxy};
+			}
+
+			// VZ reset
+			if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) {
+				++_vz_reset_counter;
+				_delta_z_reset = local_position.delta_vz;
+			}
+
+			// heading reset
+			if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) {
+				++_heading_reset_counter;
+				_delta_heading_reset = local_position.delta_heading;
+			}
+		}
+
+		// save last primary estimator_local_position
+		_local_position_last = local_position;
+
+		// publish estimator's local position for system (vehicle_local_position) unless it's stale
+		if (local_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+			// republish with total reset count and current timestamp
+			local_position.xy_reset_counter = _xy_reset_counter;
+			local_position.z_reset_counter = _z_reset_counter;
+			local_position.vxy_reset_counter = _vxy_reset_counter;
+			local_position.vz_reset_counter = _vz_reset_counter;
+			local_position.heading_reset_counter = _heading_reset_counter;
+
+			_delta_xy_reset.copyTo(local_position.delta_xy);
+			local_position.delta_z = _delta_z_reset;
+			_delta_vxy_reset.copyTo(local_position.delta_vxy);
+			local_position.delta_vz = _delta_vz_reset;
+			local_position.delta_heading = _delta_heading_reset;
+
+			local_position.timestamp = hrt_absolute_time();
+			_vehicle_local_position_pub.publish(local_position);
+		}
+	}
+}
+
+void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
+{
+	vehicle_global_position_s global_position;
+
+	if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) {
+		if (reset) {
+			// on reset compute deltas from last published data
+			++_lat_lon_reset_counter;
+			++_alt_reset_counter;
+
+			_delta_lat_reset = global_position.lat - _global_position_last.lat;
+			_delta_lon_reset = global_position.lon - _global_position_last.lon;
+			_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
+
+			// ensure monotonically increasing timestamp_sample through reset
+			global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample);
+
+		} else {
+			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+
+			// lat/lon reset
+			if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) {
+				++_lat_lon_reset_counter;
+
+				// TODO: delta latitude/longitude
+				//_delta_lat_reset = global_position.delta_lat;
+				//_delta_lon_reset = global_position.delta_lon;
+			}
+
+			// alt reset
+			if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) {
+				++_alt_reset_counter;
+				_delta_alt_reset = global_position.delta_alt;
+			}
+		}
+
+		// save last primary estimator_global_position
+		_global_position_last = global_position;
+
+		// publish estimator's global position for system (vehicle_global_position) unless it's stale
+		if (global_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+			// republish with total reset count and current timestamp
+			global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
+			global_position.alt_reset_counter = _alt_reset_counter;
+			global_position.delta_alt = _delta_alt_reset;
+
+			global_position.timestamp = hrt_absolute_time();
+			_vehicle_global_position_pub.publish(global_position);
+		}
+	}
+}
+
+void EKF2Selector::Run()
+{
+	// re-schedule as backup timeout
+	ScheduleDelayed(10_ms);
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	// update combined test ratio for all estimators
+	const bool updated = UpdateErrorScores();
+
+	if (updated) {
+		bool lower_error_available = false;
+		float alternative_error = 0.f; // looking for instances that have error lower than the current primary
+		uint8_t best_ekf_instance = _selected_instance;
+
+		// loop through all available instances to find if an alternative is available
+		for (int i = 0; i < _available_instances; i++) {
+			// Use an alternative instance if  -
+			// (healthy and has updated recently)
+			// AND
+			// (has relative error less than selected instance and has not been the selected instance for at least 10 seconds
+			// OR
+			// selected instance has stopped updating
+			if (_instance[i].healthy && (i != _selected_instance)) {
+				const float relative_error = _instance[i].relative_test_ratio;
+
+				if (relative_error < alternative_error) {
+
+					alternative_error = relative_error;
+					best_ekf_instance = i;
+
+					// relative error less than selected instance and has not been the selected instance for at least 10 seconds
+					if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) {
+						lower_error_available = true;
+					}
+				}
+			}
+		}
+
+		if ((_selected_instance == INVALID_INSTANCE)
+		    || (!_instance[_selected_instance].healthy && (best_ekf_instance == _selected_instance))) {
+
+			// force initial selection
+			uint8_t best_instance = _selected_instance;
+			float best_test_ratio = FLT_MAX;
+
+			for (int i = 0; i < _available_instances; i++) {
+				if (_instance[i].healthy) {
+					const float test_ratio = _instance[i].combined_test_ratio;
+
+					if ((test_ratio > 0) && (test_ratio < best_test_ratio)) {
+						best_instance = i;
+						best_test_ratio = test_ratio;
+					}
+				}
+			}
+
+			SelectInstance(best_instance);
+
+		} else if (best_ekf_instance != _selected_instance) {
+			//  if this instance has a significantly lower relative error to the active primary, we consider it as a
+			// better instance and would like to switch to it even if the current primary is healthy
+			//  switch immediately if the current selected is no longer healthy
+			if ((lower_error_available && hrt_elapsed_time(&_last_instance_change) > 10_s)
+			    || !_instance[_selected_instance].healthy) {
+
+				SelectInstance(best_ekf_instance);
+			}
+		}
+
+		// publish selector status
+		estimator_selector_status_s selector_status{};
+		selector_status.primary_instance = _selected_instance;
+		selector_status.instances_available = _available_instances;
+		selector_status.instance_changed_count = _instance_changed_count;
+		selector_status.last_instance_change = _last_instance_change;
+		selector_status.accel_device_id = _instance[_selected_instance].estimator_status.accel_device_id;
+		selector_status.baro_device_id = _instance[_selected_instance].estimator_status.baro_device_id;
+		selector_status.gyro_device_id = _instance[_selected_instance].estimator_status.gyro_device_id;
+		selector_status.mag_device_id = _instance[_selected_instance].estimator_status.mag_device_id;
+		selector_status.gyro_fault_detected = _gyro_fault_detected;
+		selector_status.accel_fault_detected = _accel_fault_detected;
+
+		for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+			selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
+			selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
+			selector_status.healthy[i] = _instance[i].healthy;
+		}
+
+		for (int i = 0; i < IMU_STATUS_SIZE; i++) {
+			selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
+			selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
+		}
+
+		selector_status.timestamp = hrt_absolute_time();
+		_estimator_selector_status_pub.publish(selector_status);
+	}
+
+	// republish selected estimator data for system
+	if (_selected_instance != INVALID_INSTANCE) {
+		// selected estimator_attitude -> vehicle_attitude
+		if (_instance[_selected_instance].estimator_attitude_sub.updated()) {
+			PublishVehicleAttitude();
+		}
+
+		// selected estimator_local_position -> vehicle_local_position
+		if (_instance[_selected_instance].estimator_local_position_sub.updated()) {
+			PublishVehicleLocalPosition();
+		}
+
+		// selected estimator_global_position -> vehicle_global_position
+		if (_instance[_selected_instance].estimator_global_position_sub.updated()) {
+			PublishVehicleGlobalPosition();
+		}
+
+		// selected estimator_odometry -> vehicle_odometry
+		if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
+			vehicle_odometry_s vehicle_odometry;
+
+			if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) {
+				if (vehicle_odometry.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+					vehicle_odometry.timestamp = hrt_absolute_time();
+					_vehicle_odometry_pub.publish(vehicle_odometry);
+				}
+			}
+		}
+
+		// selected estimator_visual_odometry_aligned -> vehicle_visual_odometry_aligned
+		if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.updated()) {
+			vehicle_odometry_s vehicle_visual_odometry_aligned;
+
+			if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.update(&vehicle_visual_odometry_aligned)) {
+				if (vehicle_visual_odometry_aligned.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+					vehicle_visual_odometry_aligned.timestamp = hrt_absolute_time();
+					_vehicle_visual_odometry_aligned_pub.publish(vehicle_visual_odometry_aligned);
+				}
+			}
+		}
+	}
+
+	if (_lockstep_component == -1) {
+		_lockstep_component = px4_lockstep_register_component();
+	}
+
+	px4_lockstep_progress(_lockstep_component);
+}
+
+void EKF2Selector::PrintStatus()
+{
+	PX4_INFO("available instances: %d", _available_instances);
+
+	if (_selected_instance == INVALID_INSTANCE) {
+		PX4_WARN("selected instance: None");
+	}
+
+	for (int i = 0; i < _available_instances; i++) {
+		const EstimatorInstance &inst = _instance[i];
+
+		PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s",
+			 inst.instance, inst.estimator_status.accel_device_id, inst.estimator_status.gyro_device_id,
+			 inst.estimator_status.mag_device_id,
+			 inst.healthy ? "healthy" : "unhealthy",
+			 (double)inst.combined_test_ratio, (double)inst.relative_test_ratio,
+			 (_selected_instance == i) ? "*" : "");
+	}
+}

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("telemetry_status", 1000, 4);
-	add_topic_multi("wind_estimate", 1000);
+
+	// 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("battery_status", 300, 2);
-	add_topic_multi("differential_pressure", 1000, 3);
+	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.xmag = mag(0);
-			msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1];
+			msg.ymag = mag(1);
-			msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2];
+			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)
+	if ((_param_sens_mag_rate.get() > 0) && (_last_publication_timestamp[instance] ||
-	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
+			(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_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
-		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
+			accel_count++;
-		    && (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
+			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};
+	if (accel_count > 0) {
-			_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
+		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_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
-		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
+			gyro_count++;
-		    && (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
+			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};
+	if (gyro_count > 0) {
-			_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
+		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);
+			}
 		}
 	}
 }

src/modules/sensors/voted_sensors_update.h

@@ -142,12 +142,12 @@ private:
 	bool checkFailover(SensorData &sensor, const char *sensor_name);
 
 	/**
-	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
+	 * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors
 	 */
 	void calcAccelInconsistency();
 
 	/**
-	 * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
+	 * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors
 	 */
 	void calcGyroInconsistency();
 

src/modules/simulator/simulator.h

@@ -143,6 +143,8 @@ private:
 			delete _dist_pubs[i];
 		}
 
+		px4_lockstep_unregister_component(_lockstep_component);
+
 		_instance = nullptr;
 	}
 
@@ -288,6 +290,8 @@ private:
 	px4::atomic<bool> _has_initialized {false};
 #endif
 
+	int _lockstep_component{-1};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,

src/modules/simulator/simulator_mavlink.cpp

@@ -368,6 +368,10 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 
 void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 {
+	if (_lockstep_component == -1) {
+		_lockstep_component = px4_lockstep_register_component();
+	}
+
 	mavlink_hil_sensor_t imu;
 	mavlink_msg_hil_sensor_decode(msg, &imu);
 
@@ -399,6 +403,8 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 	}
 
 #endif
+
+	px4_lockstep_progress(_lockstep_component);
 }
 
 void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
@@ -627,9 +633,11 @@ void Simulator::send()
 			parameters_update(false);
 			check_failure_injections();
 			_vehicle_status_sub.update(&_vehicle_status);
-			send_controls();
+
 			// Wait for other modules, such as logger or ekf2
 			px4_lockstep_wait_for_components();
+
+			send_controls();
 		}
 	}
 }

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -59,7 +59,7 @@ extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]);
 
 UUVAttitudeControl::UUVAttitudeControl():
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -71,7 +71,6 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/uORB.h>
 
 using matrix::Eulerf;

src/systemcmds/top/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN top
 	STACK_MAIN 4096
 	PRIORITY
-		"SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads
+		"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads
 	SRCS
 		top.cpp
 	DEPENDS