Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: remove code that prevents attitude updates after mag reset

Browse Source 
This code assumes a vehicle is close to level and will not work for all vehicle types. It will be replaced by a different method.
This commit is contained in:
Paul Riseborough 2016-05-24 09:54:07 +10:00 committed by Andrew Tridgell
parent fe85c68344
commit fe9ddfdfeb
3 changed files with 0 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -43,8 +43,6 @@ void NavEKF2_core::controlMagYawReset()
if (hgtCheckPassed || toiletBowling) { if (hgtCheckPassed || toiletBowling) {
firstMagYawInit = true; firstMagYawInit = true;
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
magFuseTiltInhibit_ms = imuSampleTime_ms;
// Update the yaw angle and earth field states using the magnetic field measurements // Update the yaw angle and earth field states using the magnetic field measurements
Quaternion tempQuat; Quaternion tempQuat;
Vector3f eulerAngles; Vector3f eulerAngles;
@ -64,14 +62,6 @@ void NavEKF2_core::controlMagYawReset()
firstMagYawInit = true; firstMagYawInit = true;
} }
// inhibit the 3-axis mag fusion from modifying the tilt states for the first few seconds after a mag field reset
// to allow the mag states to converge and prevent disturbances in roll and pitch.
if (imuSampleTime_ms - magFuseTiltInhibit_ms < 5000) {
magFuseTiltInhibit = true;
} else {
magFuseTiltInhibit = false;
}
} }
// this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity // this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
@ -627,12 +617,6 @@ void NavEKF2_core::FuseMagnetometer()
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex]; statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
} }
// Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
if (magFuseTiltInhibit) {
stateStruct.angErr.x = 0.0f;
stateStruct.angErr.y = 0.0f;
}
// the first 3 states represent the angular misalignment vector. This is // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr); stateStruct.quat.rotate(stateStruct.angErr);

1
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -240,7 +240,6 @@ void NavEKF2_core::InitialiseVariables()
optFlowFusionDelayed = false; optFlowFusionDelayed = false;
airSpdFusionDelayed = false; airSpdFusionDelayed = false;
sideSlipFusionDelayed = false; sideSlipFusionDelayed = false;
magFuseTiltInhibit = false;
posResetNE.zero(); posResetNE.zero();
velResetNE.zero(); velResetNE.zero();
hgtInnovFiltState = 0.0f; hgtInnovFiltState = 0.0f;

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -758,8 +758,6 @@ private:
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
bool magFuseTiltInhibit; // true when the 3-axis magnetoemter fusion is prevented from changing tilt angle
uint32_t magFuseTiltInhibit_ms; // time in msec that the condition indicated by magFuseTiltInhibit was commenced
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes. Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset