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

EKF: Add support for use of GPS heading data.

Browse Source 
Heading data is assumed to be from a  dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
This commit is contained in:
Paul Riseborough
2018-09-19 06:32:43 +10:00
committed by Paul Riseborough
parent af60e6bbc8
commit 91f886cb5e
8 changed files with 499 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+38
View File
@@ -511,6 +511,35 @@ void Ekf::controlGpsFusion()
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
// GPS yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_GPSYAW)
&& isfinite(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align
&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
&& (_time_last_imu - _time_last_gps < 2 * GPS_MAX_INTERVAL)) {
if (resetGpsAntYaw()) {
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all other yaw fusion
_control_status.flags.gps_yaw = true;
_control_status.flags.ev_yaw = false;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
ECL_INFO("EKF commencing GPS yaw fusion");
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
}
}
// fuse the yaw observation
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
@@ -1310,6 +1339,15 @@ void Ekf::controlDragFusion()
void Ekf::controlMagFusion()
{
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
// do not use the magnetomer and deactivate magnetic field states
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
return;
}
// If we are on ground, store the local position and time to use as a reference
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
if (!_control_status.flags.in_air) {