ekf2: replace _inhibit_ev_yaw_use with local flag

This commit is contained in:
Daniel Agar
2022-03-21 15:41:31 -04:00
parent cdf9e6d35a
commit 2f0c2fa126
4 changed files with 4 additions and 11 deletions
+4 -2
View File
@@ -201,7 +201,9 @@ void Ekf::controlExternalVisionFusion()
reset = true;
}
if (_inhibit_ev_yaw_use) {
bool inhibit_ev_yaw_use = _control_status.flags.gps || _control_status.flags.gps_yaw;
if (inhibit_ev_yaw_use) {
stopEvYawFusion();
}
@@ -232,7 +234,7 @@ void Ekf::controlExternalVisionFusion()
}
// external vision yaw aiding selection logic
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
if (!inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
&& _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
-1
View File
@@ -371,7 +371,6 @@ private:
Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m)
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-7
View File
@@ -1579,10 +1579,6 @@ void Ekf::stopGpsFusion()
if (_control_status.flags.gps_yaw) {
stopGpsYawFusion();
}
// We do not need to know the true North anymore
// EV yaw can start again
_inhibit_ev_yaw_use = false;
}
void Ekf::stopGpsPosFusion()
@@ -1767,9 +1763,6 @@ bool Ekf::resetYawToEKFGSF()
} else if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw_fault = true;
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
} else if (_control_status.flags.ev_yaw) {
_inhibit_ev_yaw_use = true;
}
_ekfgsf_yaw_reset_time = _time_last_imu;
-1
View File
@@ -130,7 +130,6 @@ void Ekf::controlGpsFusion()
// Stop the vision for yaw fusion and do not allow it to start again
stopEvYawFusion();
_inhibit_ev_yaw_use = true;
} else {
startGpsFusion();