ekf2 selector fix SITL lockstep

- ekf2 selector don't register lockstep until first attitude publication
 - sensors module register lockstep component
 - enable multi-EKF2 in SITL

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -142,10 +142,10 @@ then
 	param set EKF2_REQ_GPS_H 0.5
 
 	# Multi-EKF
-	#param set EKF2_MULTI_IMU 3
+	param set EKF2_MULTI_IMU 3
-	#param set SENS_IMU_MODE 0
+	param set SENS_IMU_MODE 0
-	#param set EKF2_MULTI_MAG 2
+	param set EKF2_MULTI_MAG 2
-	#param set SENS_MAG_MODE 0
+	param set SENS_MAG_MODE 0
 
 	# By default log from boot until first disarm.
 	param set SDLOG_MODE 1

src/modules/ekf2/EKF2Selector.cpp

@@ -57,8 +57,17 @@ bool EKF2Selector::Start()
 {
 	// default to first instance
 	_selected_instance = 0;
-	_instance[0].estimator_status_sub.registerCallback();
+
-	_instance[0].estimator_attitude_sub.registerCallback();
+	if (!_instance[0].estimator_status_sub.registerCallback()) {
+		PX4_ERR("estimator status callback registration failed");
+	}
+
+	if (!_instance[0].estimator_attitude_sub.registerCallback()) {
+		PX4_ERR("estimator attitude callback registration failed");
+	}
+
+	// backup schedule
+	ScheduleDelayed(10_ms);
 
 	return true;
 }
@@ -316,6 +325,11 @@ void EKF2Selector::PublishVehicleAttitude(bool reset)
 
 		attitude.timestamp = hrt_absolute_time();
 		_vehicle_attitude_pub.publish(attitude);
+
+		// register lockstep component on first attitude publish
+		if (_lockstep_component == -1) {
+			_lockstep_component = px4_lockstep_register_component();
+		}
 	}
 }
 
@@ -588,10 +602,6 @@ void EKF2Selector::Run()
 		}
 	}
 
-	if (_lockstep_component == -1) {
-		_lockstep_component = px4_lockstep_register_component();
-	}
-
 	px4_lockstep_progress(_lockstep_component);
 }
 

src/modules/sensors/sensors.cpp

@@ -178,6 +178,7 @@ private:
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
+	int _lockstep_component{-1};
 
 	/**
 	 * Update our local parameter cache.
@@ -297,6 +298,8 @@ Sensors::~Sensors()
 	}
 
 	perf_free(_loop_perf);
+
+	px4_lockstep_unregister_component(_lockstep_component);
 }
 
 bool Sensors::init()
@@ -639,6 +642,12 @@ void Sensors::Run()
 		parameter_update_poll();
 	}
 
+	if (_lockstep_component == -1) {
+		_lockstep_component = px4_lockstep_register_component();
+	}
+
+	px4_lockstep_progress(_lockstep_component);
+
 	perf_end(_loop_perf);
 }