diff --git a/Blimp/AP_Arming_Blimp.cpp b/Blimp/AP_Arming_Blimp.cpp index c39b9c107e..89471e4423 100644 --- a/Blimp/AP_Arming_Blimp.cpp +++ b/Blimp/AP_Arming_Blimp.cpp @@ -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 diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 6abbb0c82a..d91e3dba03 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -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) { diff --git a/Blimp/inertia.cpp b/Blimp/inertia.cpp index b46e03978d..c2b69e1207 100644 --- a/Blimp/inertia.cpp +++ b/Blimp/inertia.cpp @@ -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; } diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 9c1df237c8..d827498045 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -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