forked from Archive/PX4-Autopilot
estimated IMU bias preflight checks
- commander preflightcheck use estimator_sensor_bias message instead of EKF state index magic number - ekf2 publish estimated bias limits in estimator_sensor_bias - preflightcheck only error if bias estimate exceeds half of configured limit (delete COM_ARM_EKF_AB and COM_ARM_EKF_GB parameters)
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
7c95e99156
commit
48f125f150
|
|
@ -98,7 +98,6 @@ then
|
|||
param set BAT_V_DIV 10.14
|
||||
param set BAT_A_PER_V 18.18
|
||||
#param set CBRK_IO_SAFETY 22027
|
||||
param set COM_ARM_EKF_AB 0.005
|
||||
param set COM_DISARM_LAND 2
|
||||
|
||||
# Filter settings
|
||||
|
|
|
|||
|
|
@ -77,7 +77,6 @@ then
|
|||
|
||||
param set BAT_SOURCE 0
|
||||
param set CBRK_IO_SAFETY 22027
|
||||
#param set COM_ARM_EKF_AB 0.005
|
||||
#param set COM_DISARM_LAND 3
|
||||
|
||||
# Filter settings
|
||||
|
|
|
|||
|
|
@ -107,7 +107,6 @@ then
|
|||
param set BAT_V_DIV 10.14
|
||||
param set BAT_A_PER_V 18.18
|
||||
#param set CBRK_IO_SAFETY 22027
|
||||
param set COM_ARM_EKF_AB 0.005
|
||||
param set COM_DISARM_LAND 2
|
||||
|
||||
# Filter settings
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@ then
|
|||
# sensor calibration
|
||||
param set CAL_MAG_SIDES 63
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set COM_ARM_EKF_AB 0.0032
|
||||
|
||||
# circuit breakers
|
||||
param set CBRK_IO_SAFETY 22027
|
||||
|
|
|
|||
|
|
@ -3,22 +3,25 @@
|
|||
# scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available).
|
||||
#
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
# In-run bias estimates (subtract from uncorrected data)
|
||||
|
||||
uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s)
|
||||
uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s)
|
||||
float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s)
|
||||
float32[3] gyro_bias_variance
|
||||
bool gyro_bias_valid
|
||||
|
||||
uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2)
|
||||
uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2)
|
||||
float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2)
|
||||
float32[3] accel_bias_variance
|
||||
bool accel_bias_valid
|
||||
|
||||
uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss)
|
||||
uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss)
|
||||
float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss)
|
||||
float32[3] mag_bias_variance
|
||||
bool mag_bias_valid
|
||||
|
|
|
|||
|
|
@ -232,7 +232,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
|
|||
failed = true;
|
||||
}
|
||||
|
||||
if (!ekf2CheckStates(mavlink_log_pub, report_failures && report_ekf_fail)) {
|
||||
if (!ekf2CheckSensorBias(mavlink_log_pub, report_failures && report_ekf_fail)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ private:
|
|||
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
|
||||
const bool report_fail, const bool enforce_gps_required);
|
||||
|
||||
static bool ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail);
|
||||
static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
|
||||
|
||||
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
|
||||
const bool prearm);
|
||||
|
|
|
|||
|
|
@ -39,9 +39,11 @@
|
|||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_states.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
|
||||
const bool report_fail, const bool enforce_gps_required)
|
||||
{
|
||||
|
|
@ -69,7 +71,6 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
|
|||
// Get estimator status data if available and exit with a fail recorded if not
|
||||
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
|
||||
status_sub.update();
|
||||
const estimator_status_s &status = status_sub.get();
|
||||
|
||||
if (status.timestamp == 0) {
|
||||
|
|
@ -231,52 +232,55 @@ out:
|
|||
return success;
|
||||
}
|
||||
|
||||
bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail)
|
||||
bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail)
|
||||
{
|
||||
float ekf_ab_test_limit = 1.0f; // pass limit re-used for each test
|
||||
param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit);
|
||||
|
||||
float ekf_gb_test_limit = 1.0f; // pass limit re-used for each test
|
||||
param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);
|
||||
|
||||
// Get estimator states data if available and exit with a fail recorded if not
|
||||
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::Subscription states_sub{ORB_ID(estimator_states), estimator_selector_status_sub.get().primary_instance};
|
||||
uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
|
||||
const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
|
||||
|
||||
estimator_states_s states;
|
||||
if (hrt_elapsed_time(&bias.timestamp) < 30_s) {
|
||||
|
||||
if (states_sub.copy(&states)) {
|
||||
// check accelerometer bias estimates
|
||||
if (bias.accel_bias_valid) {
|
||||
const float ekf_ab_test_limit = 0.5f * bias.accel_bias_limit;
|
||||
|
||||
// check accelerometer delta velocity bias estimates
|
||||
for (uint8_t index = 13; index < 16; index++) {
|
||||
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
|
||||
// adjust test threshold by 3-sigma
|
||||
float test_uncertainty = 3.0f * sqrtf(fmaxf(states.covariances[index], 0.0f));
|
||||
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
|
||||
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
|
||||
// adjust test threshold by 3-sigma
|
||||
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
|
||||
|
||||
if (fabsf(states.states[index]) > ekf_ab_test_limit + test_uncertainty) {
|
||||
if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
|
||||
if (report_fail) {
|
||||
PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
|
||||
(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
|
||||
}
|
||||
|
||||
if (report_fail) {
|
||||
PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)ekf_ab_test_limit,
|
||||
(double)test_uncertainty);
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check gyro delta angle bias estimates
|
||||
// check gyro bias estimates
|
||||
if (bias.gyro_bias_valid) {
|
||||
const float ekf_gb_test_limit = 0.5f * bias.gyro_bias_limit;
|
||||
|
||||
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
|
||||
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
|
||||
// adjust test threshold by 3-sigma
|
||||
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
|
||||
|
||||
if (fabsf(states.states[10]) > ekf_gb_test_limit
|
||||
|| fabsf(states.states[11]) > ekf_gb_test_limit
|
||||
|| fabsf(states.states[12]) > ekf_gb_test_limit) {
|
||||
if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
|
||||
if (report_fail) {
|
||||
PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
|
||||
(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
|
||||
}
|
||||
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -556,31 +556,6 @@ PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
|
||||
|
||||
/**
|
||||
* Maximum value of EKF accelerometer delta velocity bias estimate that will allow arming.
|
||||
* Note: ekf2 will limit the delta velocity bias estimate magnitude to be less than EKF2_ABL_LIM * FILTER_UPDATE_PERIOD_MS * 0.001 so this parameter must be less than that to be useful.
|
||||
*
|
||||
* @group Commander
|
||||
* @unit m/s
|
||||
* @min 0.001
|
||||
* @max 0.01
|
||||
* @decimal 4
|
||||
* @increment 0.0001
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 0.0022f);
|
||||
|
||||
/**
|
||||
* Maximum value of EKF gyro delta angle bias estimate that will allow arming
|
||||
*
|
||||
* @group Commander
|
||||
* @unit rad
|
||||
* @min 0.0001
|
||||
* @max 0.0017
|
||||
* @decimal 4
|
||||
* @increment 0.0001
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_ARM_EKF_GB, 0.0011f);
|
||||
|
||||
/**
|
||||
* Maximum accelerometer inconsistency between IMU units that will allow arming
|
||||
*
|
||||
|
|
|
|||
|
|
@ -884,6 +884,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
if (_device_id_gyro != 0) {
|
||||
bias.gyro_device_id = _device_id_gyro;
|
||||
gyro_bias.copyTo(bias.gyro_bias);
|
||||
bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
|
||||
_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
|
||||
bias.gyro_bias_valid = true;
|
||||
|
||||
|
|
@ -893,6 +894,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
|
||||
bias.accel_device_id = _device_id_accel;
|
||||
accel_bias.copyTo(bias.accel_bias);
|
||||
bias.accel_bias_limit = _params->acc_bias_lim;
|
||||
_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
|
||||
bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
|
||||
|
||||
|
|
@ -902,6 +904,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
if (_device_id_mag != 0) {
|
||||
bias.mag_device_id = _device_id_mag;
|
||||
mag_bias.copyTo(bias.mag_bias);
|
||||
bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
|
||||
_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
|
||||
bias.mag_bias_valid = _mag_cal_available;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue