fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 11:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: magless fixes and improvements

Browse Source 
- periodically fuse yaw estimator yaw if valid and no other heading
sources available
 - initial yaw align from yaw estimator also include GPS vel/pos reset
 - fix resetYawToEKFGSF() logic to prevent excessive resets
 - GPS fusion timeout use lower threshold for yaw estimator heading
reset if no other heading source available
This commit is contained in:
Daniel Agar
2022-04-21 11:31:46 -04:00
parent 4084beaa53
commit fd9a1a8b1c
2 changed files with 28 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -1779,8 +1779,8 @@ bool Ekf::resetYawToEKFGSF()
// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
// to improve its estimate if the previous reset was not successful.
if (!isYawEmergencyEstimateAvailable()
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
|| !isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)
|| _ekfgsf_yaw_reset_count > _params.EKFGSF_reset_count_limit) {
return false;
}
src/modules/ekf2/EKF/gps_control.cpp
+26 -6
View File
@@ -56,14 +56,25 @@ void Ekf::controlGpsFusion()
if (gps_checks_passing && !gps_checks_failing && isYawEmergencyEstimateAvailable()) {
bool perform_yaw_align = !_control_status.flags.yaw_align && _control_status.flags.tilt_align;
bool no_yaw_aid_src = !yawAidingAvailable();
bool yaw_error = isYawError(math::radians(10.f)); // 10 degrees yaw error
if (perform_yaw_align || (no_yaw_aid_src && yaw_error)) {
if (perform_yaw_align) {
if (resetYawToEKFGSF()) {
ECL_INFO("Yaw aligned using IMU and GPS");
resetVelocityToGps(_gps_sample_delayed);
resetHorizontalPositionToGps(_gps_sample_delayed);
}
}
// periodically fuse yaw estimate if necessary
if (isYawEmergencyEstimateAvailable()
&& _control_status.flags.yaw_align
&& isTimedOut(_ekfgsf_yaw_reset_time, 1'000'000)
&& isTimedOut(_time_last_heading_fuse, (uint64_t)1'000'000)) {
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yawEstimator.getYaw());
float obs_var = _yawEstimator.getYawVar();
fuseYaw(innovation, obs_var);
}
}
// Determine if we should use GPS aiding for velocity and horizontal position
@@ -89,13 +100,22 @@ void Ekf::controlGpsFusion()
* because the timeout could have been caused by bad GPS.
* The total number of resets allowed per boot cycle is limited.
*/
if (isYawFailure()
if (isYawEmergencyEstimateAvailable()
&& !yawAidingAvailable()
&& isYawError(math::radians(10.f))
&& _control_status.flags.in_air
&& !was_gps_signal_lost
&& resetYawToEKFGSF()) {
ECL_WARN("GPS emergency yaw reset");
} else if (isYawFailure()
&& _control_status.flags.in_air
&& !was_gps_signal_lost
&& resetYawToEKFGSF()) {
ECL_WARN("GPS emergency yaw reset");
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
@@ -113,7 +133,6 @@ void Ekf::controlGpsFusion()
}
} else {
_warning_events.flags.gps_fusion_timout = true;
ECL_WARN("GPS fusion timeout - resetting");
// use GPS velocity data to check and correct yaw angle if a FW vehicle
@@ -123,6 +142,7 @@ void Ekf::controlGpsFusion()
}
}
_warning_events.flags.gps_fusion_timout = true;
resetVelocityToGps(_gps_sample_delayed);
resetHorizontalPositionToGps(_gps_sample_delayed);
}
@@ -173,7 +193,7 @@ bool Ekf::shouldResetGpsFusion() const
|| isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
* navigation casued by a bad yaw estimate.
* navigation caused by a bad yaw estimate.
* A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
* innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,