Commander: run ekf checks without grace period after boot

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -42,7 +42,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
 		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
 		orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
-		const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason)
+		arm_disarm_reason_t calling_reason)
 {
 	// Double check that our static arrays are still valid
 	static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
@@ -69,7 +69,6 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 			if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
 							    true, // report_failures
-							    time_since_boot,
 							    safety_button_available, safety_off,
 							    true)) { // is_arm_attempt
 				feedback_provided = true; // Preflight checks report error messages

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -58,7 +58,7 @@ public:
 				const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
 				actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
 				vehicle_status_flags_s &status_flags,
-				const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
+				arm_disarm_reason_t calling_reason);
 
 	// Getters
 	uint8_t getArmState() const { return _arm_state; }

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -269,7 +269,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 						     true /* enable pre-arm checks */,
 						     nullptr /* no mavlink_log_pub */,
 						     status_flags,
-						     2e6, /* 2 seconds after boot, everything should be checked */
 						     arm_disarm_reason_t::unit_test);
 
 		// Validate result of transition

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -52,7 +52,7 @@ static constexpr unsigned max_mandatory_baro_count = 1;
 
 bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				    vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				    bool report_failures, const hrt_abstime &time_since_boot,
+				    bool report_failures,
 				    const bool safety_button_available, const bool safety_off,
 				    const bool is_arm_attempt)
 {
@@ -193,23 +193,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}
 
 	if (estimator_type == 2) {
+		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) &&
+					 ekf2CheckSensorBias(mavlink_log_pub, report_failures);
 
-		const bool in_grace_period = time_since_boot < 10_s;
-		const bool do_report_ekf2_failures = report_failures && (!in_grace_period);
-		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, do_report_ekf2_failures) &&
-					 ekf2CheckSensorBias(mavlink_log_pub, do_report_ekf2_failures);
-
-		// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
-		// as not present.
-		// After that or if we're forced to report, we'll set the flags as is.
-
-		if (!ekf_healthy && !do_report_ekf2_failures) {
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
-
-		} else {
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
-		}
-
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
 
 		if (control_mode.flag_control_attitude_enabled
 		    || control_mode.flag_control_velocity_enabled

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -64,7 +64,7 @@ public:
 	**/
 	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				   vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				   bool reportFailures, const hrt_abstime &time_since_boot,
+				   bool reportFailures,
 				   const bool safety_button_available, const bool safety_off,
 				   const bool is_arm_attempt = false);
 
@@ -93,8 +93,7 @@ private:
 				  const float arming_max_airspeed_allowed);
 	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail);
 	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
-	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
-			      const bool report_fail);
+	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
 	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
 	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -44,8 +44,7 @@
 
 using namespace time_literals;
 
-bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
-			       const bool report_fail)
+bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail)
 {
 	bool success = true; // start with a pass and change to a fail if any test fails
 

src/modules/commander/Commander.cpp

@@ -299,7 +299,6 @@ int Commander::custom_command(int argc, char *argv[])
 		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
 					   vehicle_control_mode,
 					   true, // report_failures
-					   30_s,
 					   false, // safety_buttton_available not known
 					   false); // safety_off not known
 		PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
@@ -496,7 +495,7 @@ bool Commander::shutdown_if_allowed()
 			_safety.isButtonAvailable(), _safety.isSafetyOff(),
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
 			_actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
-			hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
+			arm_disarm_reason_t::shutdown);
 }
 
 static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
@@ -748,7 +747,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
 					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
 					 vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks,
-					 &_mavlink_log_pub, _vehicle_status_flags, hrt_elapsed_time(&_boot_timestamp),
+					 &_mavlink_log_pub, _vehicle_status_flags,
 					 calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
@@ -793,7 +792,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
 					 vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false,
 					 &_mavlink_log_pub, _vehicle_status_flags,
-					 hrt_elapsed_time(&_boot_timestamp), calling_reason);
+					 calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
@@ -853,7 +852,6 @@ Commander::Commander() :
 	// run preflight immediately to find all relevant parameters, but don't report
 	PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
 				       false, // report_failures
-				       hrt_elapsed_time(&_boot_timestamp),
 				       false, // safety_buttton_available not known
 				       false); // safety_off not known
 }
@@ -1408,7 +1406,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						_safety.isButtonAvailable(), _safety.isSafetyOff(),
 						vehicle_status_s::ARMING_STATE_INIT, _actuator_armed,
 						false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
-						30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
 						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
 				   ) {
 
@@ -2459,7 +2456,6 @@ Commander::run()
 					_safety.isButtonAvailable(), _safety.isSafetyOff(),
 					vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed,
 					true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
-					hrt_elapsed_time(&_boot_timestamp),
 					arm_disarm_reason_t::transition_to_standby);
 		}
 
@@ -3000,7 +2996,6 @@ Commander::run()
 						_vehicle_status_flags,
 						_vehicle_control_mode,
 						false, // report_failures
-						hrt_elapsed_time(&_boot_timestamp),
 						_safety.isButtonAvailable(), _safety.isSafetyOff());
 				perf_end(_preflight_check_perf);
 
@@ -3630,7 +3625,6 @@ void Commander::data_link_check()
 						// make sure to report preflight check failures to a connecting GCS
 						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
 									       true, // report_failures
-									       hrt_elapsed_time(&_boot_timestamp),
 									       _safety.isButtonAvailable(), _safety.isSafetyOff());
 					}
 				}