Blimp: remove get_filter_status from AP_InertialNav

This commit is contained in:
Peter Barker 2025-05-27 19:11:17 +10:00 committed by Peter Barker
parent 876e9a9db5
commit a43049d7fb
4 changed files with 30 additions and 26 deletions

View File

@ -59,8 +59,8 @@ bool AP_Arming_Blimp::barometer_checks(bool display_failure)
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = blimp.inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
const auto &ahrs = AP::ahrs();
const bool using_baro_ref = !ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_REL) && ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_ABS);
if (using_baro_ref) {
if (fabsf(blimp.inertial_nav.get_position_z_up_cm() - blimp.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
check_failed(Check::BARO, display_failure, "Altitude disparity");
@ -195,10 +195,7 @@ bool AP_Arming_Blimp::gps_checks(bool display_failure)
// check ekf attitude is acceptable
bool AP_Arming_Blimp::pre_arm_ekf_attitude_check()
{
// get ekf filter status
nav_filter_status filt_status = blimp.inertial_nav.get_filter_status();
return filt_status.flags.attitude;
return AP::ahrs().has_status(AP_AHRS::Status::ATTITUDE_VALID);
}
// performs mandatory gps checks. returns true if passed

View File

@ -161,8 +161,7 @@ void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason)
void Blimp::gpsglitch_check()
{
// get filter status
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool gps_glitching = filt_status.flags.gps_glitching;
const bool gps_glitching = AP::ahrs().has_status(AP_AHRS::Status::GPS_GLITCHING);
// log start or stop of gps glitch. AP_Notify update is handled from within AP_AHRS
if (ap.gps_glitching != gps_glitching) {

View File

@ -13,7 +13,7 @@ void Blimp::read_inertia()
current_loc.lng = loc.lng;
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
if (!ahrs.has_status(AP_AHRS::Status::VERT_POS)) {
return;
}

View File

@ -142,16 +142,21 @@ bool Blimp::ekf_has_absolute_position() const
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal position
if (!motors->armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
} else {
// once armed we require a good absolute position and EKF must not be in const_pos_mode
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
if (ahrs.has_status(AP_AHRS::Status::HORIZ_POS_ABS)) {
return true;
}
if (ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_ABS)) {
return true;
}
return false;
}
// once armed we require a good absolute position and EKF must not be in const_pos_mode
if (ahrs.has_status(AP_AHRS::Status::CONST_POS_MODE)) {
return false;
}
return ahrs.has_status(AP_AHRS::Status::HORIZ_POS_ABS);
}
// ekf_has_relative_position - returns true if the EKF can provide a position estimate relative to it's starting position
@ -168,15 +173,14 @@ bool Blimp::ekf_has_relative_position() const
return false;
}
// get filter status from EKF
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal relative position
if (!motors->armed()) {
return (filt_status.flags.pred_horiz_pos_rel);
} else {
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
return ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_REL);
}
if (ahrs.has_status(AP_AHRS::Status::CONST_POS_MODE)) {
return false;
}
return ahrs.has_status(AP_AHRS::Status::HORIZ_POS_REL);
}
// returns true if the ekf has a good altitude estimate (required for modes which do AltHold)
@ -187,11 +191,15 @@ bool Blimp::ekf_alt_ok() const
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// require both vertical velocity and position
return (filt_status.flags.vert_vel && filt_status.flags.vert_pos);
if (!ahrs.has_status(AP_AHRS::Status::VERT_VEL)) {
return false;
}
if (!ahrs.has_status(AP_AHRS::Status::VERT_POS)) {
return false;
}
return true;
}
// update_auto_armed - update status of auto_armed flag