diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 56994007c9..08e696d9e1 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -410,6 +410,13 @@ void AP_AHRS::update_DCM(bool skip_ins_update) _dcm_attitude = {roll, pitch, yaw}; } +void AP_AHRS::update_notify_from_filter_status(const nav_filter_status &status) +{ + AP_Notify::flags.gps_fusion = status.flags.using_gps; // Drives AP_Notify flag for usable GPS. + AP_Notify::flags.gps_glitching = status.flags.gps_glitching; + AP_Notify::flags.have_pos_abs = status.flags.horiz_pos_abs; +} + #if HAL_NAVEKF2_AVAILABLE void AP_AHRS::update_EKF2(void) { @@ -482,9 +489,7 @@ void AP_AHRS::update_EKF2(void) _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; nav_filter_status filt_state; EKF2.getFilterStatus(-1,filt_state); - AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS. - AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching; - AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs; + update_notify_from_filter_status(filt_state); } } } @@ -563,9 +568,7 @@ void AP_AHRS::update_EKF3(void) _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; nav_filter_status filt_state; EKF3.getFilterStatus(-1,filt_state); - AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS. - AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching; - AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs; + update_notify_from_filter_status(filt_state); } } } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 87fdc0f207..c55628ff4e 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -618,6 +618,9 @@ private: */ bool fly_forward; // true if we can assume the vehicle will be flying forward on its X axis + // poke AP_Notify based on values from status + void update_notify_from_filter_status(const nav_filter_status &status); + #if HAL_NMEA_OUTPUT_ENABLED class AP_NMEA_Output* _nmea_out; #endif