fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 23:34:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
EKF/control.cpp
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();

3
EKF/ekf.h
View File

@ -322,6 +322,9 @@ private:
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation moaneouvre was detected
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetomer use was inhibited
bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix

8
EKF/mag_fusion.cpp
View File

@ -675,7 +675,13 @@ void Ekf::fuseHeading()
measured_hdg = wrap_pi(measured_hdg);
// calculate the innovation
_heading_innov = predicted_hdg - measured_hdg;
if (_mag_use_inhibit) {
// The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly
// conditoned covariance matrix developing over time.
_heading_innov = 0.0f;
} else {
_heading_innov = predicted_hdg - measured_hdg;
}
// wrap the innovation to the interval between +-pi
_heading_innov = wrap_pi(_heading_innov);

1
EKF/optflow_fusion.cpp
View File

@ -500,7 +500,6 @@ void Ekf::fuseOptFlow()
fuse(gain, _flow_innov[obs_index]);
_time_last_of_fuse = _time_last_imu;
_gps_check_fail_status.value = 0;
}
}
}