dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mag control: select mag mode before doing the reset

This commit is contained in:
bresch 2021-05-11 12:06:32 +02:00 committed by Paul Riseborough
parent fe943cf104
commit 2cc023038b
1 changed files with 15 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
EKF/mag_control.cpp
View File

@ -77,23 +77,11 @@ void Ekf::controlMagFusion()
_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req; _mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
if (noOtherYawAidingThanMag() && _mag_data_ready) { if (noOtherYawAidingThanMag() && _mag_data_ready) {
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
runInAirYawReset();
runVelPosReset();
} else {
runOnGroundYawReset();
}
if (!_control_status.flags.yaw_align) {
// Having the yaw aligned is mandatory to continue
return;
}
// Determine if we should use simple magnetic heading fusion which works better when // Determine if we should use simple magnetic heading fusion which works better when
// there are large external disturbances or the more accurate 3-axis fusion // there are large external disturbances or the more accurate 3-axis fusion
switch (_params.mag_fusion_type) { switch (_params.mag_fusion_type) {
default:
/* fallthrough */
case MAG_FUSE_TYPE_AUTO: case MAG_FUSE_TYPE_AUTO:
selectMagAuto(); selectMagAuto();
break; break;
@ -107,10 +95,20 @@ void Ekf::controlMagFusion()
case MAG_FUSE_TYPE_3D: case MAG_FUSE_TYPE_3D:
startMag3DFusion(); startMag3DFusion();
break; break;
}
default: if (_control_status.flags.in_air) {
selectMagAuto(); checkHaglYawResetReq();
break; runInAirYawReset();
runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only
} else {
runOnGroundYawReset();
}
if (!_control_status.flags.yaw_align) {
// Having the yaw aligned is mandatory to continue
return;
} }
checkMagDeclRequired(); checkMagDeclRequired();