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

AP_NavEKF : Add low speed magnetometer calibration option

Browse Source
This commit is contained in:
Paul Riseborough 2014-03-06 17:13:22 +11:00 committed by priseborough
parent 88b0364fcd
commit 52fabc822f
2 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -197,6 +197,14 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @User: advanced
AP_GROUPINFO("EAS_GATE", 21, NavEKF, _tasInnovGate, 10),
// @Param: MAG_CAL
// @DisplayName: Turns on magnetometer calibration mode
// @Description: Setting this parameter to 1 forces magnetic field state calibration to be active all the time the vehicle is manoeuvring regardless of its speed and altitude. This parameter should be set to 0 for aircraft use. This parameter can be set to 1 to enable in-flight compass calibration on Copter and Rover vehicles.
// @Range: 0 - 1
// @Increment: 1
// @User: advanced
AP_GROUPINFO("MAG_CAL", 22, NavEKF, _magCal, 0),
AP_GROUPEND
};
@ -759,6 +767,7 @@ void NavEKF::UpdateStrapdownEquationsNED()
// calculate a magnitude of the filtered nav acceleration (required for GPS
// variance estimation)
accNavMag = velDotNEDfilt.length();
accNavMagHoriz = sqrtf(sq(velDotNEDfilt.x) + sq(velDotNEDfilt.y));
// If calculating position save previous velocity
Vector3f lastVelocity = state.velocity;
@ -2380,8 +2389,14 @@ void NavEKF::OnGroundCheck()
uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f);
uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
// inhibit onGround mode if magnetomter calibration at low speed and height is enabled.
// A static check is still applied to prevent excessive compass drift when sitting stationary
if ((_magCal == 1) && (accNavMagHoriz > 0.5f)) {
onGround = false;
} else {
// Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
}
}
void NavEKF::CovarianceInit(float roll, float pitch, float yaw)

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -304,6 +304,7 @@ private:
AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check
AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
AP_Int8 _magCal; // Forces magentic field states to be always active to aid magnetometer calibration
// Tuning parameters
AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot
@ -347,6 +348,7 @@ private:
Vector3f prevDelAng; // previous delta angle use for INS coning error compensation
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f dVelIMU1; // delta velocity vector in XYZ body axes measured by IMU1 (m/s)
Vector3f dVelIMU2; // delta velocity vector in XYZ body axes measured by IMU2 (m/s)