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); }