fix : collectGps function move check_gps to ekf2

@@ -55,7 +55,6 @@
 #define MASK_GPS_VDRIFT (1<<6)
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
-static int hold=0;
 bool Ekf::collect_gps(const gps_message &gps)
 {
 	// Run GPS checks always
@@ -141,18 +140,6 @@ bool Ekf::collect_gps(const gps_message &gps)
 	}
 
 	// start collecting GPS if there is a 3D fix and the NED origin has been set
-	if(hold != int(gps.time_usec/1000000))
-	{
-	hold=int(gps.time_usec/1000000);
-		if(hold%2==0)
-		{
-			std::cout <<"----------------------current--------------------------"<<std::endl;
-			std :: cout << "time : "<<gps.time_usec/1000000<<std::endl;
-			std :: cout << "lat : " <<gps.lat <<std::endl;
-			std :: cout << "lon : " <<gps.lon <<std::endl;
-			std :: cout << "alt : " <<gps.alt <<std::endl;
-		}
-	}
 	// print GPS data every 1 seocnds
 	return _NED_origin_initialised && (gps.fix_type >= 3);
 }

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include "EKF2.hpp"
-
+#include <iostream>
 using namespace time_literals;
 using math::constrain;
 using matrix::Eulerf;
@@ -44,7 +44,7 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 #if !defined(CONSTRAINED_FLASH)
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // !CONSTRAINED_FLASH
-
+static int hold=0;
 EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
@@ -169,6 +169,8 @@ EKF2::~EKF2()
 {
 	perf_free(_ecl_ekf_update_perf);
 	perf_free(_ecl_ekf_update_full_perf);
+	perf_free(_imu_missed_perf);
+	perf_free(_mag_missed_perf);
 }
 
 bool EKF2::multi_init(int imu, int mag)
@@ -193,17 +195,18 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
 
-	_vehicle_imu_sub.ChangeInstance(imu);
-	_magnetometer_sub.ChangeInstance(mag);
+	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
 
 	const int status_instance = _estimator_states_pub.get_instance();
 
-	if ((status_instance >= 0)
+	if ((status_instance >= 0) && changed_instance
 	    && (_attitude_pub.get_instance() == status_instance)
 	    && (_local_position_pub.get_instance() == status_instance)
 	    && (_global_position_pub.get_instance() == status_instance)) {
 
 		_instance = status_instance;
+
+		ScheduleNow();
 		return true;
 	}
 
@@ -219,6 +222,12 @@ int EKF2::print_status()
 		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
+	perf_print_counter(_imu_missed_perf);
+
+	if (_device_id_mag != 0) {
+		perf_print_counter(_mag_missed_perf);
+	}
+
 	return 0;
 }
 
@@ -298,7 +307,8 @@ void EKF2::Run()
 		imu_updated = _vehicle_imu_sub.update(&imu);
 
 		if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
-			PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_imu_missed_perf);
+			PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
 				  _vehicle_imu_sub.get_last_generation());
 		}
 
@@ -1498,7 +1508,18 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 					      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
 			};
 			_ekf.setGpsData(gps_msg);
-
+			if(hold != int(gps_msg.time_usec/1000000))
+			{
+				hold=int(gps_msg.time_usec/1000000);
+				if(hold%2==0)
+				{
+					std::cout <<"----------------------current--------------------------"<<std::endl;
+					std :: cout << "time : "<<gps_msg.time_usec/1000000<<std::endl;
+					std :: cout << "lat : " <<gps_msg.lat <<std::endl;
+					std :: cout << "lon : " <<gps_msg.lon <<std::endl;
+					std :: cout << "alt : " <<gps_msg.alt <<std::endl;
+			}
+	}
 			_gps_time_usec = gps_msg.time_usec;
 			_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
 		}
@@ -1513,7 +1534,8 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_magnetometer_sub.update(&magnetometer)) {
 
 		if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_mag_missed_perf);
+			PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
 				  _magnetometer_sub.get_last_generation());
 		}
 
@@ -1738,7 +1760,8 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 		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)) {
+		       && ((hrt_elapsed_time(&time_started) < 30_s)
+			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
 
 			vehicle_status_sub.update();
 
@@ -1763,16 +1786,21 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 								if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
 									_objects[actual_instance].store(ekf2_inst);
-									ekf2_inst->ScheduleNow();
 									success = true;
 									multi_instances_allocated++;
 									ekf2_instance_created[imu][mag] = true;
 
+									if (actual_instance == 0) {
+										// force selector to run immediately if first instance started
+										_ekf2_selector.load()->ScheduleNow();
+									}
+
 									PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
 										 imu, vehicle_imu_sub.get().accel_device_id,
 										 mag, vehicle_mag_sub.get().device_id);
 
-									_ekf2_selector.load()->ScheduleNow();
+									// sleep briefly before starting more instances
+									px4_usleep(10000);
 
 								} else {
 									PX4_ERR("instance numbering problem instance: %d", actual_instance);