Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: factor out an update_notify_from_filter_status

This commit is contained in:
Peter Barker 2021-08-27 23:22:05 +10:00 committed by Peter Barker
parent ca6d214bb3
commit 0792caab83
2 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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);
}
}
}

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -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