diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 7931acdc40..ba36de31a3 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -65,7 +65,8 @@ #include #include - +#include +#include #include "PreflightCheck.h" @@ -160,6 +161,55 @@ out: return success; } +static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool checkAcc, bool checkGyro, bool report_status) +{ + // get the sensor preflight data + int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight)); + struct sensor_preflight_s sensors = {}; + orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors); + px4_close(sensors_sub); + + // 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. + bool success = true; + float test_limit; + param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); + if (checkAcc) { + if (sensors.accel_inconsistency_m_s_s > test_limit) { + if (report_status) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL SENSORS INCONSISTENT - CHECK CALIBRATION"); + } + success = false; + goto out; + + } else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.5f) { + if (report_status) { + mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCEL SENSORS INCONSISTENT - CHECK CALIBRATION"); + + } + } + } + // 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 (checkGyro) { + if (sensors.gyro_inconsistency_rad_s > test_limit) { + if (report_status) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO SENSORS INCONSISTENT - CHECK CALIBRATION"); + } + success = false; + goto out; + + } else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) { + if (report_status) { + mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYRO SENSORS INCONSISTENT - CHECK CALIBRATION"); + + } + } + } + +out: + return success; +} + static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -382,6 +432,84 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) return success; } +static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail) +{ + // Get estimator status data if available and exit with a fail recorded if not + int sub = orb_subscribe(ORB_ID(estimator_status)); + bool updated; + orb_check(sub,&updated); + struct estimator_status_s status; + orb_copy(ORB_ID(estimator_status), sub, &status); + orb_unsubscribe(sub); + px4_close(sub); + + bool success = true; // start with a pass and change to a fail if any test fails + float test_limit; // pass limit re-used for each test + + // 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.hgt_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_IMU_AB"), &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_IMU_GB"), &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; + } + +out: + return success; +} + bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures) { @@ -518,6 +646,9 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, } } + /* ---- IMU CONSISTENCY ---- */ + imuConsistencyCheck(mavlink_log_pub, checkAcc, checkGyro, reportFailures); + /* ---- AIRSPEED ---- */ if (checkAirspeed) { if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) { @@ -542,6 +673,16 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, } } + /* ---- Navigation EKF ---- */ + // 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 && checkGNSS) { + if (!ekf2Check(mavlink_log_pub, true, reportFailures)) { + failed = true; + } + } + /* Report status */ return !failed; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 27aca67c86..63531451ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,6 +180,18 @@ static systemlib::Hysteresis auto_disarm_hysteresis(false); static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; +/* pre-flight EKF checks */ +static float max_ekf_pos_ratio = 0.5f; +static float max_ekf_vel_ratio = 0.5f; +static float max_ekf_hgt_ratio = 0.5f; +static float max_ekf_yaw_ratio = 0.5f; +static float max_ekf_dvel_bias = 2.0e-3f; +static float max_ekf_dang_bias = 3.5e-4f; + +/* pre-flight IMU consistency checks */ +static float max_imu_acc_diff = 0.7f; +static float max_imu_gyr_diff = 0.09f; + static struct vehicle_status_s status = {}; static struct vehicle_roi_s _roi = {}; static struct battery_status_s battery = {}; @@ -309,7 +321,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 40, - 3100, + 3200, commander_thread_main, (char * const *)&argv[0]); @@ -1299,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_fmode_5 = param_find("COM_FLTMODE5"); param_t _param_fmode_6 = param_find("COM_FLTMODE6"); + /* pre-flight EKF checks */ + param_t _param_max_ekf_pos_ratio = param_find("COM_ARM_EKF_POS"); + param_t _param_max_ekf_vel_ratio = param_find("COM_ARM_EKF_VEL"); + param_t _param_max_ekf_hgt_ratio = param_find("COM_ARM_EKF_HGT"); + param_t _param_max_ekf_yaw_ratio = param_find("COM_ARM_EKF_YAW"); + param_t _param_max_ekf_dvel_bias = param_find("COM_ARM_EKF_AB"); + param_t _param_max_ekf_dang_bias = param_find("COM_ARM_EKF_GB"); + + /* pre-flight IMU consistency checks */ + param_t _param_max_imu_acc_diff = param_find("COM_ARM_IMU_ACC"); + param_t _param_max_imu_gyr_diff = param_find("COM_ARM_IMU_GYR"); + // These are too verbose, but we will retain them a little longer // until we are sure we really don't need them. @@ -1780,6 +1804,18 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_fmode_5, &_flight_mode_slots[4]); param_get(_param_fmode_6, &_flight_mode_slots[5]); + /* pre-flight EKF checks */ + param_get(_param_max_ekf_pos_ratio, &max_ekf_pos_ratio); + param_get(_param_max_ekf_vel_ratio, &max_ekf_vel_ratio); + param_get(_param_max_ekf_hgt_ratio, &max_ekf_hgt_ratio); + param_get(_param_max_ekf_yaw_ratio, &max_ekf_yaw_ratio); + param_get(_param_max_ekf_dvel_bias, &max_ekf_dvel_bias); + param_get(_param_max_ekf_dang_bias, &max_ekf_dang_bias); + + /* pre-flight IMU consistency checks */ + param_get(_param_max_imu_acc_diff, &max_imu_acc_diff); + param_get(_param_max_imu_gyr_diff, &max_imu_gyr_diff); + param_init_forced = false; /* Set flag to autosave parameters if necessary */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a8f91cb91a..0a495d23a8 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -472,3 +472,99 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1); * @value 12 Follow Me */ PARAM_DEFINE_INT32(COM_FLTMODE6, -1); + +/** + * Maximum EKF position innovation test ratio that will allow arming + * + * @group Commander + * @unit m + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f); + +/** + * Maximum EKF velocity innovation test ratio that will allow arming + * + * @group Commander + * @unit m/s + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f); + +/** + * Maximum EKF height innovation test ratio that will allow arming + * + * @group Commander + * @unit m + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f); + +/** + * Maximum EKF yaw innovation test ratio that will allow arming + * + * @group Commander + * @unit rad + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f); + +/** + * Maximum value of EKF accelerometer delta velocity bias estimate that will allow arming + * + * @group Commander + * @unit m/s + * @min 0.001 + * @max 0.004 + * @decimal 4 + * @increment 0.0005 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 2.0e-3f); + +/** + * Maximum value of EKF gyro delta angle bias estimate that will allow arming + * + * @group Commander + * @unit rad + * @min 0.0001 + * @max 0.0007 + * @decimal 5 + * @increment 0.00005 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_GB, 3.5e-4f); + +/** + * Maximum accelerometer inconsistency between IMU units that will allow arming + * + * @group Commander + * @unit m/s/s + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_IMU_ACC, 0.7f); + +/** + * Maximum rate gyro inconsistency between IMU units that will allow arming + * + * @group Commander + * @unit rad/s + * @min 0.02 + * @max 0.2 + * @decimal 3 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.09f);