fix(controller): remove ekf2 readiness delay in simulation

Previously, there was an unnecessary 1-second delay imposed on the Commander
module specifically when running in simulation (QEMU) environments. This
delay has been removed to allow for a faster startup and smoother simulation
experience.

Additionally, the EstimatorChecks module had conditional compilation flags
for QEMU that erroneously bypassed important position and velocity validity
checks. These checks are now applied uniformly across all platforms, ensuring
better behavior and consistency in simulation.

The changes also refactor the EstimatorChecks code to improve readability and
maintainability, as well as correct a minor issue with angular velocity
validation that could cause false failures in simulation.

By addressing these issues, simulation runs on QEMU are expected to be more
stable and better represent the behavior of the system in real-world conditions.

apps/controller/commander/Commander.cpp

@@ -1577,6 +1577,9 @@ void Commander::Run() {
 
  arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
 
+ // Wait 1s until the ekf2 is ready in the simulation environment
+ rt_thread_mdelay(1000);
+
  while (!should_exit()) {
  perf_begin(_loop_perf);
 

apps/controller/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -719,24 +719,25 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
  }
  }
 
-// TODO: need fix
-#ifdef BSP_USING_QEMU
- failsafe_flags.global_position_invalid = false;
- failsafe_flags.local_position_invalid = false;
- failsafe_flags.local_position_invalid_relaxed = false;
- failsafe_flags.local_velocity_invalid = false;
- failsafe_flags.local_altitude_invalid = false;
-#else
- failsafe_flags.global_position_invalid = !checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
- _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
- failsafe_flags.local_position_invalid = !checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
- _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
- failsafe_flags.local_position_invalid_relaxed = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
- _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
- failsafe_flags.local_velocity_invalid = !checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
- _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);
- failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s));
-#endif /* BSP_USING_QEMU */
+ failsafe_flags.global_position_invalid =
+ !checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
+ _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
+
+ failsafe_flags.local_position_invalid =
+ !checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
+ _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
+
+ failsafe_flags.local_position_invalid_relaxed =
+ !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
+ _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
+
+ failsafe_flags.local_velocity_invalid =
+ !checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
+ _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);
+
+
+ // altitude
+ failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s));
 
  // attitude
  vehicle_attitude_s attitude;
@@ -761,9 +762,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
  // angular velocity
  vehicle_angular_velocity_s angular_velocity{};
  _vehicle_angular_velocity_sub.copy(&angular_velocity);
- const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0 && (now < angular_velocity.timestamp + 1_s);
- const bool condition_angular_velocity_finite = matrix::Vector3f(angular_velocity.xyz).isAllFinite();
- const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
+ const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0
+ && (now < angular_velocity.timestamp + 1_s);
+ const bool condition_angular_velocity_finite = matrix::Vector3f(angular_velocity.xyz).isAllFinite();
+ const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
  || !condition_angular_velocity_finite;
 
  if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) {

apps/estimator/ekf2_fake/ekf2_fake.hpp

@@ -16,6 +16,8 @@ using namespace nextpilot;
 using namespace time_literals;
 using namespace nextpilot::global_params;
 
+#define EKF2_FAKE_PERIOD_MS 5
+
 class Ekf2Fake {
 public:
  Ekf2Fake();
@@ -24,14 +26,13 @@ class Ekf2Fake {
  void Run();
 
 private:
- static constexpr int EKF2_FAKE_PERIOD_MS = 5;
- uORB::Subscription _sensor_gyro_sub{ORB_ID(sensor_gyro)};
- uORB::Subscription _sensor_acc_sub{ORB_ID(sensor_accel)};
- uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
- uORB::Subscription _v_ang_vel_groundtruth_sub{ORB_ID(vehicle_angular_velocity_groundtruth)};
- uORB::Subscription _v_att_groundtruth_sub{ORB_ID(vehicle_attitude_groundtruth)};
- uORB::Subscription _v_loc_pos_groundtruth_sub{ORB_ID(vehicle_local_position_groundtruth)};
- uORB::Subscription _v_glob_pos_groundtruth_sub{ORB_ID(vehicle_global_position_groundtruth)};
+ uORB::Subscription _sensor_gyro_sub{ORB_ID(sensor_gyro)};
+ uORB::Subscription _sensor_acc_sub{ORB_ID(sensor_accel)};
+ uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
+ uORB::Subscription _v_ang_vel_groundtruth_sub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+ uORB::Subscription _v_att_groundtruth_sub{ORB_ID(vehicle_attitude_groundtruth)};
+ uORB::Subscription _v_loc_pos_groundtruth_sub{ORB_ID(vehicle_local_position_groundtruth)};
+ uORB::Subscription _v_glob_pos_groundtruth_sub{ORB_ID(vehicle_global_position_groundtruth)};
 
  sensor_gyro_s _s_gyro{};
  sensor_accel_s _s_accel{};

apps/estimator/ekf2_fake/ekf2_fake_main.cpp

@@ -21,7 +21,7 @@ static void ekf2_fake_run(void *parameter) {
  _instance->init();
  while (1) {
  _instance->Run();
- rt_thread_mdelay(5);
+ rt_thread_mdelay(EKF2_FAKE_PERIOD_MS);
  }
 }