commander: Add preflight checking for EKF health and IMU sensor consi…

…stency

src/modules/commander/PreflightCheck.cpp

@@ -65,7 +65,8 @@
 
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_gps_position.h>
-
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_preflight.h>
 
 #include "PreflightCheck.h"
 
@@ -160,6 +161,55 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
  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;
 }

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 */

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