From a3288ff732ba30fe9c67296a8f21eff4907432fe Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 22 Jun 2022 17:29:20 +0200 Subject: [PATCH] Commander: run ekf checks without grace period after boot --- .../ArmStateMachine/ArmStateMachine.cpp | 3 +-- .../ArmStateMachine/ArmStateMachine.hpp | 2 +- .../ArmStateMachine/ArmStateMachineTest.cpp | 1 - .../Arming/PreFlightCheck/PreFlightCheck.cpp | 21 ++++--------------- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 5 ++--- .../PreFlightCheck/checks/ekf2Check.cpp | 3 +-- src/modules/commander/Commander.cpp | 12 +++-------- 7 files changed, 12 insertions(+), 35 deletions(-) diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index ba956f691f..a9d84c91ef 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/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 diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 62adf81326..4262091b0d 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/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; } diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index 91886a4812..bc0751278c 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/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 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 1d664f0b79..f3860b186d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/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 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index f21e7c1dc9..e52a3ecbf5 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/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); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index ff8b56acda..69b5ec912f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d3fdf38332..f2298b02f1 100644 --- a/src/modules/commander/Commander.cpp +++ b/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()); } }