fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: allow yaw estimator to perform yaw alignment

Browse Source 
This can be useful if using a full EV + GNSS setup and you start indoors, then fly outside. Once GPS is good and the only missing requirement is yaw alignment the yaw estimator reset is performed and EV yaw will automatically stop itself.
This commit is contained in:
Daniel Agar
2023-07-25 10:58:45 -04:00
committed by GitHub
parent 6d4ec4e623
commit 19d6e69b0b
4 changed files with 88 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+12 -20
View File
@@ -1160,37 +1160,29 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
}
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _time_delayed_us;
_control_status.flags.yaw_align = true;
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
} else if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw_fault = true;
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
}
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_yaw) {
_inhibit_ev_yaw_use = true;
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
_information_events.flags.yaw_aligned_to_imu_gps = true;
return true;
}