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

EKF: Don't use magnetometer with optical flow only nav if GPS checks are failing

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-15 09:48:09 +10:00
committed by Lorenz Meier
parent 84516760c0
commit 0160aaa568
4 changed files with 29 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+19 -1
View File
@@ -472,10 +472,11 @@ 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
if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) {
if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) {
_control_status.flags.yaw_align = false;
_control_status.flags.ev_yaw = false;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_mag_inhibit_yaw_reset_req = false;
}
// If the heading is valid start using gps aiding
@@ -1391,6 +1392,23 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_dec = false;
}
// If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating
// indoors and the magnetometer is unreliable.
if (_control_status.flags.opt_flow
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos
&& ((_last_gps_fail_us - _time_last_imu) < 5E6)) {
_mag_use_inhibit = true;
} else {
_mag_use_inhibit = false;
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
}
// If magnetomer use has been inhibited continuously then a yaw reset is required for a valid heading
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
_mag_inhibit_yaw_reset_req = true;
}
// fuse magnetometer data using the selected methods
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();