From aea68f06f926da105f88c19538c6e2b081aa5302 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2020 23:54:05 +0100 Subject: [PATCH] Commander: Streamline preflight and arming checks The checks did previously only report the first failure (to not overload the radio link). As we are moving to buffered messages and higher bandwidth links this design choice is not any more in the best interest of the drone operator. We are now reporting all detected failures. To ensure architectural consistency more checks have been moved from the commander mainloop into the respective classes. --- .../Arming/HealthFlags/HealthFlags.h | 1 + .../Arming/PreFlightCheck/PreFlightCheck.cpp | 66 +++++++------------ .../Arming/PreFlightCheck/PreFlightCheck.hpp | 2 +- .../PreFlightCheck/checks/ekf2Check.cpp | 7 +- .../PreFlightCheck/checks/powerCheck.cpp | 2 +- .../PreFlightCheck/checks/preArmCheck.cpp | 8 ++- src/modules/commander/Commander.cpp | 12 +--- 7 files changed, 37 insertions(+), 61 deletions(-) diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h index a91e1564de..d9a9cd5e5e 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -43,6 +43,7 @@ #include #include +#include void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status); void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 8e9bba4037..9459e07861 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -59,18 +59,15 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot) { - if (time_since_boot < 2_s) { - // the airspeed driver filter doesn't deliver the actual value yet - reportFailures = false; - } - const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); - bool checkSensors = !hil_enabled; + reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout + && !status_flags.condition_calibration_enabled); + + const bool checkSensors = !hil_enabled; const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); const bool checkDynamic = !hil_enabled; const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check); - const bool checkFailureDetector = true; bool checkAirspeed = false; @@ -81,9 +78,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu checkAirspeed = true; } - reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout - && !status_flags.condition_calibration_enabled); - bool failed = false; /* ---- MAG ---- */ @@ -98,12 +92,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t prime_id = -1; param_get(param_find("CAL_MAG_PRIME"), &prime_id); - bool mag_fail_reported = false; - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); - const bool report_fail = (reportFailures && !failed && !mag_fail_reported); + const bool report_fail = (reportFailures); int32_t device_id = -1; @@ -116,15 +108,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } else { if (required) { failed = true; - mag_fail_reported = true; } } } /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { + if (!prime_found && prime_id != 0) { + if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); } @@ -133,7 +124,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } /* mag consistency checks (need to be performed after the individual checks) */ - if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { + if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) { failed = true; } } @@ -145,12 +136,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t prime_id = -1; param_get(param_find("CAL_ACC_PRIME"), &prime_id); - bool accel_fail_reported = false; - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { const bool required = (i < max_mandatory_accel_count); - const bool report_fail = (reportFailures && !failed && !accel_fail_reported); + const bool report_fail = (reportFailures); int32_t device_id = -1; @@ -163,14 +152,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } else { if (required) { failed = true; - accel_fail_reported = true; } } } /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { + if (!prime_found && prime_id != 0) { + if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); } @@ -185,16 +173,12 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t prime_id = -1; param_get(param_find("CAL_GYRO_PRIME"), &prime_id); - bool gyro_fail_reported = false; - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { const bool required = (i < max_mandatory_gyro_count); - const bool report_fail = (reportFailures && !failed && !gyro_fail_reported); - int32_t device_id = -1; - if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) { if ((prime_id > 0) && (device_id == prime_id)) { prime_found = true; @@ -203,14 +187,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } else { if (required) { failed = true; - gyro_fail_reported = true; } } } /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { + if (!prime_found && prime_id != 0) { + if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); } @@ -229,13 +212,12 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1); - const bool report_fail = (reportFailures && !failed && !baro_fail_reported); + const bool report_fail = (reportFailures && !baro_fail_reported); int32_t device_id = -1; if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { if (required) { - failed = true; baro_fail_reported = true; } } @@ -245,7 +227,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- IMU CONSISTENCY ---- */ // To be performed after the individual sensor checks have completed if (checkSensors) { - if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { + if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) { failed = true; } } @@ -255,14 +237,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t optional = 0; param_get(param_find("FW_ARSP_MODE"), &optional); - if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) { + if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures, prearm) && !(bool)optional) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rcCalibrationCheck(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) { + if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) { if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); } @@ -282,7 +264,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- SYSTEM POWER ---- */ if (checkPower) { - if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) { + if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) { failed = true; } } @@ -303,16 +285,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu // don't report ekf failures for the first 10 seconds to allow time for the filter to start bool report_ekf_fail = (time_since_boot > 10_s); - if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) { + if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) { failed = true; } } /* ---- Failure Detector ---- */ - if (checkFailureDetector) { - if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) { - failed = true; - } + if (!failureDetectorCheck(mavlink_log_pub, status, reportFailures, prearm)) { + failed = true; } /* Report status */ diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index fa9c1722d3..cb5b6e9262 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -89,7 +89,7 @@ public: }; static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status, + const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail = true); private: diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index bfb83a9579..aa12bf8a91 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -45,7 +45,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & const bool report_fail, const bool enforce_gps_required) { bool success = true; // start with a pass and change to a fail if any test fails - bool present = true; + bool ahrs_present = true; float test_limit = 1.0f; // pass limit re-used for each test int32_t mag_strength_check_enabled = 1; @@ -60,7 +60,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & const estimator_status_s &status = status_sub.get(); if (status.timestamp == 0) { - present = false; + ahrs_present = false; goto out; } @@ -253,7 +253,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status); + //PX4_INFO("AHRS CHECK: %s", (success && ahrs_present) ? "OK" : "FAIL"); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, ahrs_present, true, success && ahrs_present, vehicle_status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status); return success; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 3a669e6a4f..91b2d4ccb3 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index b58988705c..f2e847b0e2 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -36,9 +36,10 @@ #include #include #include +#include bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status, bool report_fail) + const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail) { bool prearm_ok = true; @@ -60,6 +61,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } // main battery level + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, + status_flags.condition_battery_healthy, status); + + // Only arm if healthy if (!status_flags.condition_battery_healthy) { if (prearm_ok) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); } @@ -89,7 +94,6 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } } - // Arm Requirements: global position if (arm_requirements.global_position) { if (!status_flags.condition_global_position_valid) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a492ae122f..a156c857cb 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3833,13 +3833,10 @@ void Commander::battery_status_check() // All connected batteries are regularly being published (hrt_elapsed_time(&oldest_update) < 5_s) // There is at least one connected battery (in any slot) - && num_connected_batteries > 0 + && (num_connected_batteries > 0) // No currently-connected batteries have any warning && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, - (_battery_warning == battery_status_s::BATTERY_WARNING_NONE), status); - // execute battery failsafe if the state has gotten worse while we are armed if (battery_warning_level_increased_while_armed) { battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning, @@ -3975,13 +3972,6 @@ void Commander::estimator_check() } } - if ((_last_condition_global_position_valid != status_flags.condition_global_position_valid) - && status_flags.condition_global_position_valid) { - // If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa. - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status); - set_health_flags_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, status); - } - check_valid(lpos.timestamp, _param_com_pos_fs_delay.get() * 1_s, lpos.z_valid, &(status_flags.condition_local_altitude_valid), &_status_changed); }