mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-24 16:20:34 +08:00
ekf2: replace _inhibit_ev_yaw_use with local flag
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user