mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2025-12-10 00:28:42 -06:00
Blimp: remove get_filter_status from AP_InertialNav
This commit is contained in:
parent
876e9a9db5
commit
a43049d7fb
@ -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
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user