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

mag_control: fuse fake heading during leveling fine alignment (#17964)

Browse Source 
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Mathieu Bresciani
2021-07-27 11:50:50 +02:00
committed by GitHub
co-authored by Daniel Agar
parent f2ae8ae814
commit 17ebcd2456
5 changed files with 765 additions and 755 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+18 -18
View File
@@ -41,23 +41,6 @@
void Ekf::controlMagFusion()
{
// handle undefined behaviour
if (_params.mag_fusion_type > MAG_FUSE_TYPE_NONE) {
return;
}
// 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) {
if (noOtherYawAidingThanMag()) {
_is_yaw_fusion_inhibited = true;
fuseHeading();
}
return;
}
checkMagFieldStrength();
// If we are on ground, reset the flight alignment flag so that the mag fields will be
@@ -67,8 +50,25 @@ void Ekf::controlMagFusion()
_num_bad_flight_yaw_events = 0;
}
if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) {
// 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
// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
|| _control_status.flags.mag_fault
|| !_control_status.flags.tilt_align) {
stopMagFusion();
if (noOtherYawAidingThanMag()) {
// TODO: setting _is_yaw_fusion_inhibited to true is required to tell
// fuseHeading to perform a "zero innovation heading fusion"
// We should refactor it to avoid using this flag here
_is_yaw_fusion_inhibited = true;
fuseHeading();
_is_yaw_fusion_inhibited = false;
}
return;
}