fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 18:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

control: inhibit EV yaw from restarting when GPS aid wants to start

Browse Source
This commit is contained in:
bresch
2021-05-07 16:14:49 +02:00
committed by Paul Riseborough
parent a685987082
commit 48722a2a92
3 changed files with 18 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+13 -6
View File
@@ -190,6 +190,10 @@ void Ekf::controlExternalVisionFusion()
// Check for new external vision data
if (_ev_data_ready) {
if (_inhibit_ev_yaw_use) {
stopEvYawFusion();
}
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
@@ -215,7 +219,7 @@ void Ekf::controlExternalVisionFusion()
}
// external vision yaw aiding selection logic
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
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
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
startEvYawFusion();
@@ -521,13 +525,16 @@ void Ekf::controlGpsFusion()
// If the heading is not aligned, reset the yaw and magnetic field states
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
_control_status.flags.ev_yaw ||
_mag_inhibit_yaw_reset_req;
if (!_control_status.flags.yaw_align
|| _control_status.flags.ev_yaw
|| _mag_inhibit_yaw_reset_req
|| _mag_yaw_reset_req) {
if (want_to_reset_mag_heading) {
_mag_yaw_reset_req = true;
_control_status.flags.ev_yaw = false;
// Stop the vision for yaw fusion and do not allow it to start again
stopEvYawFusion();
_inhibit_ev_yaw_use = true;
} else {
// If the heading is valid start using gps aiding