mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +08:00
commander: Add preflight checking for EKF health and IMU sensor consistency
This commit is contained in:
Paul Riseborough
committed by
Lorenz Meier
Lorenz Meier
parent
55bf6b4974
commit
983cfb8fdd
@@ -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 @@ 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;
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user