mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 10:23:34 +08:00
ekf2/commander: simplify navigation filter preflight checks
- remove commander test ratio "tuning knobs" (COM_ARM_EKF_{HGT,POS,VEL,YAW})
- these are effectively redundant with the actual tuning (noise & gate)
in the estimator, plus most users have no idea why they'd be
adjusting these other than to silence an annoying preflight complaint
- remove ekf2 "PreFlightChecker" with hard coded innovation limits
- ekf2 preflight innovation flags are now simply if any active source
exceeds half the limit preflight
This commit is contained in:
Daniel Agar
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
a42dc2165c
commit
eac14b7db2
@@ -63,6 +63,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
|
||||
bool pre_flt_fail_innov_heading = false;
|
||||
bool pre_flt_fail_innov_vel_horiz = false;
|
||||
bool pre_flt_fail_innov_pos_horiz = false;
|
||||
bool missing_data = false;
|
||||
const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
|
||||
|
||||
@@ -90,6 +91,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
if (_estimator_status_sub.copy(&estimator_status)) {
|
||||
pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
|
||||
pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
|
||||
pre_flt_fail_innov_pos_horiz = estimator_status.pre_flt_fail_innov_pos_horiz;
|
||||
|
||||
checkEstimatorStatus(context, reporter, estimator_status, required_groups);
|
||||
checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
|
||||
@@ -123,7 +125,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
}
|
||||
|
||||
// set mode requirements
|
||||
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
|
||||
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz,
|
||||
lpos, vehicle_gps_position,
|
||||
reporter.failsafeFlags(), reporter);
|
||||
|
||||
|
||||
@@ -166,6 +169,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
|
||||
}
|
||||
|
||||
} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_pos_horiz) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_hor_pos_not_stable"),
|
||||
events::Log::Error, "Horizontal position unstable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable");
|
||||
}
|
||||
|
||||
} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) {
|
||||
/* EVENT
|
||||
*/
|
||||
@@ -208,82 +222,6 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
}
|
||||
}
|
||||
|
||||
// check vertical position innovation test ratio
|
||||
if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_hgt_est_err"),
|
||||
events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check velocity innovation test ratio
|
||||
if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_vel_est_err"),
|
||||
events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check horizontal position innovation test ratio
|
||||
if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_pos_est_err"),
|
||||
events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check magnetometer innovation test ratio
|
||||
if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_yaw_est_err"),
|
||||
events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
|
||||
if (_param_sys_has_gps.get()) {
|
||||
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
|
||||
@@ -767,7 +705,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
|
||||
}
|
||||
|
||||
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
|
||||
bool pre_flt_fail_innov_vel_horiz,
|
||||
bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
|
||||
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags,
|
||||
Report &reporter)
|
||||
{
|
||||
@@ -797,7 +735,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
||||
bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
|
||||
|
||||
if (!context.isArmed()) {
|
||||
if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
|
||||
if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) {
|
||||
xy_valid = false;
|
||||
}
|
||||
|
||||
|
||||
@@ -66,7 +66,9 @@ private:
|
||||
const vehicle_local_position_s &lpos);
|
||||
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
|
||||
void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
|
||||
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
|
||||
|
||||
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
|
||||
bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
|
||||
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
|
||||
failsafe_flags_s &failsafe_flags, Report &reporter);
|
||||
|
||||
@@ -108,10 +110,6 @@ private:
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
|
||||
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_VEL>) _param_com_arm_ekf_vel,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
|
||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
|
||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
|
||||
|
||||
@@ -358,50 +358,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
|
||||
|
||||
/**
|
||||
* Maximum EKF position innovation test ratio that will allow arming
|
||||
*
|
||||
* @group Commander
|
||||
* @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
|
||||
* @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
|
||||
* @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
|
||||
* @min 0.1
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
|
||||
|
||||
/**
|
||||
* Maximum accelerometer inconsistency between IMU units that will allow arming
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user