mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-13 22:11:35 +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;
|
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();
|
stopEvYawFusion();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -232,7 +234,7 @@ void Ekf::controlExternalVisionFusion()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// external vision yaw aiding selection logic
|
// 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) {
|
&& _control_status.flags.tilt_align) {
|
||||||
|
|
||||||
// don't start using EV data unless data is arriving frequently
|
// 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)
|
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
|
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
|
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
|
// 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
|
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) {
|
if (_control_status.flags.gps_yaw) {
|
||||||
stopGpsYawFusion();
|
stopGpsYawFusion();
|
||||||
}
|
}
|
||||||
|
|
||||||
// We do not need to know the true North anymore
|
|
||||||
// EV yaw can start again
|
|
||||||
_inhibit_ev_yaw_use = false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::stopGpsPosFusion()
|
void Ekf::stopGpsPosFusion()
|
||||||
@@ -1767,9 +1763,6 @@ bool Ekf::resetYawToEKFGSF()
|
|||||||
} else if (_control_status.flags.gps_yaw) {
|
} else if (_control_status.flags.gps_yaw) {
|
||||||
_control_status.flags.gps_yaw_fault = true;
|
_control_status.flags.gps_yaw_fault = true;
|
||||||
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = 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;
|
_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
|
// Stop the vision for yaw fusion and do not allow it to start again
|
||||||
stopEvYawFusion();
|
stopEvYawFusion();
|
||||||
_inhibit_ev_yaw_use = true;
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
startGpsFusion();
|
startGpsFusion();
|
||||||
|
|||||||
Reference in New Issue
Block a user