From 2271b3f12703106d597a0de6c822ca95a58ecb9b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 4 Apr 2018 20:48:44 -0400 Subject: [PATCH] commander preflight checks pass status and status_flags messages --- src/modules/commander/PreflightCheck.cpp | 95 ++++++++++++++++--- src/modules/commander/PreflightCheck.h | 7 +- src/modules/commander/commander.cpp | 19 +--- .../commander/state_machine_helper.cpp | 21 +--- 4 files changed, 91 insertions(+), 51 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index fea703871e..fe241052aa 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -109,6 +109,7 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi break; } } + instance++; } @@ -140,6 +141,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); } + success = false; goto out; } @@ -150,6 +152,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); } + success = false; goto out; } @@ -175,6 +178,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu // Use the difference between IMU's to detect a bad calibration. // If a single IMU is fitted, the value being checked will be zero so this check will always pass. param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); + if (sensors.accel_inconsistency_m_s_s > test_limit) { if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL"); @@ -191,10 +195,12 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec param_get(param_find("COM_ARM_IMU_GYR"), &test_limit); + if (sensors.gyro_inconsistency_rad_s > test_limit) { if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL"); } + success = false; goto out; @@ -215,27 +221,32 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu // get the sensor preflight data int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight)); struct sensor_preflight_s sensors = {}; + if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) { // can happen if not advertised (yet) return true; } + orb_unsubscribe(sensors_sub); // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. // If a single sensor is fitted, the value being checked will be zero so this check will always pass. float test_limit; param_get(param_find("COM_ARM_MAG"), &test_limit); + if (sensors.mag_inconsistency_ga > test_limit) { if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT"); } + return false; } return true; } -static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) +static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, + int &device_id, bool report_fail) { bool success = true; @@ -260,6 +271,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); } + success = false; goto out; } @@ -270,11 +282,13 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret); } + success = false; goto out; } #ifdef __PX4_NUTTX + if (dynamic) { /* check measurement result range */ struct accel_report acc; @@ -288,19 +302,23 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); } + /* this is frickin' fatal */ success = false; goto out; } + } else { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ"); } + /* this is frickin' fatal */ success = false; goto out; } } + #endif out: @@ -333,6 +351,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); } + success = false; goto out; } @@ -343,6 +362,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); } + success = false; goto out; } @@ -403,6 +423,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); } + success = false; goto out; } @@ -412,6 +433,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); } + success = false; goto out; } @@ -426,6 +448,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK"); } + success = false; goto out; } @@ -439,6 +462,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT"); } + success = false; goto out; } @@ -454,8 +478,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre bool success = true; if (!prearm) { - // Ignore power check after arming. - return true; + // Ignore power check after arming. + return true; } else { int system_power_sub = orb_subscribe(ORB_ID(system_power)); @@ -515,66 +539,81 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS"); } + success = false; goto out; } // check vertical position innovation test ratio param_get(param_find("COM_ARM_EKF_HGT"), &test_limit); + if (status.hgt_test_ratio > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HGT ERROR"); } + success = false; goto out; } // check velocity innovation test ratio param_get(param_find("COM_ARM_EKF_VEL"), &test_limit); + if (status.vel_test_ratio > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF VEL ERROR"); } + success = false; goto out; } // check horizontal position innovation test ratio param_get(param_find("COM_ARM_EKF_POS"), &test_limit); + if (status.pos_test_ratio > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HORIZ POS ERROR"); } + success = false; goto out; } // check magnetometer innovation test ratio param_get(param_find("COM_ARM_EKF_YAW"), &test_limit); + if (status.mag_test_ratio > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF YAW ERROR"); } + success = false; goto out; } // check accelerometer delta velocity bias estimates param_get(param_find("COM_ARM_EKF_AB"), &test_limit); - if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) { + + if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit + || fabsf(status.states[15]) > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS"); } + success = false; goto out; } // check gyro delta angle bias estimates param_get(param_find("COM_ARM_EKF_GB"), &test_limit); - if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) { + + if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit + || fabsf(status.states[12]) > test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS"); } + success = false; goto out; } @@ -583,30 +622,36 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ if (enforce_gps_required) { bool ekf_gps_fusion = status.control_mode_flags & (1 << 2); bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; + if (!ekf_gps_fusion) { // The EKF is not using GPS if (report_fail) { if (ekf_gps_check_fail) { // Poor GPS qulaity is the likely cause mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + } else { // Likely cause unknown mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); } } + success = false; goto out; + } else { // The EKF is using GPS so check for bad quality on key performance indicators bool gps_quality_fail = ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0); + + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0); + if (gps_quality_fail) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); } + success = false; goto out; } @@ -618,8 +663,9 @@ out: return success; } -bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot) +bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, + const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm, + const hrt_abstime &time_since_boot) { if (time_since_boot < 2000000) { @@ -627,6 +673,23 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check reportFailures = false; } + const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); + + 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); + + bool checkAirspeed = false; + + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) { + checkAirspeed = true; + } + + reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout); + #ifdef __PX4_QURT // WARNING: Preflight checks are important and should be added back when // all the sensors are supported @@ -673,6 +736,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check if ((reportFailures && !failed)) { mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); } + failed = true; } @@ -696,7 +760,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check bool required = (i < max_mandatory_accel_count); int device_id = -1; - if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures && !accel_fail_reported)) && required) { + if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures + && !accel_fail_reported)) && required) { failed = true; accel_fail_reported = true; } @@ -711,6 +776,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check if ((reportFailures && !failed)) { mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); } + failed = true; } } @@ -743,6 +809,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check if ((reportFailures && !failed)) { mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); } + failed = true; } } @@ -776,6 +843,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check if (reportFailures && !failed) { mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); } + failed = true; } } @@ -796,7 +864,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rc_calibration_check(mavlink_log_pub, reportFailures, isVTOL) != OK) { + if (rc_calibration_check(mavlink_log_pub, reportFailures, status.is_vtol) != OK) { if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); } @@ -816,6 +884,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled int32_t estimator_type; param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); + if (estimator_type == 2) { // 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 * 1000000); diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index e84f17bb0c..4b708ea078 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -40,6 +40,8 @@ */ #include +#include +#include #pragma once @@ -70,8 +72,9 @@ namespace Preflight * @param checkPower * true if the system power should be checked **/ -bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot); +bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, + const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm, + const hrt_abstime &time_since_boot); static constexpr unsigned max_mandatory_gyro_count = 1; static constexpr unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0f347b7b0e..e86f1529ff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -4032,26 +4032,9 @@ void Commander::mission_init() bool Commander::preflight_check(bool report) { - const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); - - const bool checkSensors = !hil_enabled; - const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); - const bool checkDynamic = !hil_enabled; - const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check); - - const bool reportFailures = (report && status_flags.condition_system_hotplug_timeout); - - bool checkAirspeed = false; - - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) { - checkAirspeed = true; - } - bool success = Preflight::preflightCheck(&mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, - status.is_vtol, reportFailures, false, hrt_elapsed_time(&commander_boot_timestamp)); + bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, hrt_elapsed_time(&commander_boot_timestamp)); status_flags.condition_system_sensors_initialized = success; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 353ced0459..eff011ac7b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -75,7 +75,7 @@ static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MA }; // You can index into the array with an arming_state_t in order to get its textual representation -const char * const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { +const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { "INIT", "STANDBY", "ARMED", @@ -120,27 +120,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt bool preflight_check_ret = true; bool prearm_check_ret = true; - const bool checkSensors = !hil_enabled; - const bool checkRC = (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); - const bool checkDynamic = !hil_enabled; - const bool checkPower = (status_flags->condition_power_input_valid - && !status_flags->circuit_breaker_engaged_power_check); - - bool checkAirspeed = false; - - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { - checkAirspeed = true; - } /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { - preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, - checkDynamic, checkPower, status->is_vtol, true, true, time_since_boot); + preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot); if (preflight_check_ret) { status_flags->condition_system_sensors_initialized = true; @@ -156,8 +142,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { - status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, checkSensors, - checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, status->is_vtol, false, false, time_since_boot); + status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot); last_preflight_check = hrt_absolute_time(); }