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

GPS Yaw: Always reset Yaw when GPS Yaw fusion is starting

Browse Source 
Also avoid fusing fake mag data when an other source of yaw aiding
is active, even if in MAG_TYPE_NONE mode.
This commit is contained in:
bresch
2020-06-23 08:43:48 +02:00
committed by Mathieu Bresciani
parent ff8b5ec69d
commit 3c6790f5d5
6 changed files with 45 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_control.cpp
+11 -9
View File
@@ -46,12 +46,15 @@ void Ekf::controlMagFusion()
return;
}
// When operating without a magnetometer, yaw fusion is run selectively to prevent
// enable yaw gyro bias learning hen stationary on ground and to prevent uncontrolled
// yaw variance growth
// When operating without a magnetometer and no other source of yaw aiding is active,
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
// ground and to prevent uncontrolled yaw variance growth
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
_yaw_use_inhibit = true;
fuseHeading();
if (noOtherYawAidingThanMag())
{
_yaw_use_inhibit = true;
fuseHeading();
}
return;
}
@@ -70,7 +73,7 @@ void Ekf::controlMagFusion()
return;
}
if (canRunMagFusion()) {
if (noOtherYawAidingThanMag() && _mag_data_ready) {
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
runInAirYawReset();
@@ -116,11 +119,10 @@ void Ekf::updateMagFilter()
}
}
bool Ekf::canRunMagFusion() const
bool Ekf::noOtherYawAidingThanMag() const
{
// check for new magnetometer data that has fallen behind the fusion time horizon
// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready;
return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw;
}
void Ekf::checkHaglYawResetReq()