diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index 3850dc19a2..d82ee30e9f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco index e1a434b62a..4541937873 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4072_draco +++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index 07826f212c..fefc1c02ad 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index e07ccf7fd9..ae1c9de06e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -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 diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index 1f4e92ca1a..6999bea7a4 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -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 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index e11202fdaf..134ccd8dd1 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -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; } } diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index ff6418d5c1..1add8ba0d8 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -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); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index de72b5f5f1..33e673f5f7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -39,9 +39,11 @@ #include #include #include -#include +#include #include +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_sub{ORB_ID(estimator_selector_status)}; uORB::SubscriptionData 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_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_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; } } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ab682f9c2f..29afa848bc 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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 * diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 98d9bbb44d..4958a529b1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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;