aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Prevent pre-arm baro drift affecting mag field reset height

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-21 00:23:56 +10:00 committed by Randy Mackay
parent c2e6fdb56c
commit 68b225de4d
2 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4760,6 +4760,10 @@ void NavEKF::performArmingChecks()
getEulerAngles(eulerAngles); getEulerAngles(eulerAngles);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
} }
// store vertical position at arming to use as a reference for ground relative cehcks
if (vehicleArmed) {
posDownAtArming = state.position.z;
}
// zero stored velocities used to do dead-reckoning // zero stored velocities used to do dead-reckoning
heldVelNE.zero(); heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation // reset the flag that indicates takeoff for use by optical flow navigation
@ -4835,14 +4839,14 @@ void NavEKF::performArmingChecks()
StoreStatesReset(); StoreStatesReset();
} }
} else if (vehicleArmed && !firstMagYawInit && state.position.z < -1.5f && !assume_zero_sideslip()) { } else if (vehicleArmed && !firstMagYawInit && (state.position.z - posDownAtArming) < -1.5f && !assume_zero_sideslip()) {
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff) // Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
Vector3f eulerAngles; Vector3f eulerAngles;
getEulerAngles(eulerAngles); getEulerAngles(eulerAngles);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
firstMagYawInit = true; firstMagYawInit = true;
} else if (vehicleArmed && !secondMagYawInit && state.position.z < -5.0f && !assume_zero_sideslip()) { } else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip()) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff) // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
Vector3f eulerAngles; Vector3f eulerAngles;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -640,6 +640,7 @@ private:
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
// Used by smoothing of state corrections // Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement