Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues 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

15
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -197,6 +197,14 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @User: advanced // @User: advanced
AP_GROUPINFO("EAS_GATE", 21, NavEKF, _tasInnovGate, 10), 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 AP_GROUPEND
}; };
@ -759,6 +767,7 @@ void NavEKF::UpdateStrapdownEquationsNED()
// calculate a magnitude of the filtered nav acceleration (required for GPS // calculate a magnitude of the filtered nav acceleration (required for GPS
// variance estimation) // variance estimation)
accNavMag = velDotNEDfilt.length(); accNavMag = velDotNEDfilt.length();
accNavMagHoriz = sqrtf(sq(velDotNEDfilt.x) + sq(velDotNEDfilt.y));
// If calculating position save previous velocity // If calculating position save previous velocity
Vector3f lastVelocity = state.velocity; Vector3f lastVelocity = state.velocity;
@ -2380,9 +2389,15 @@ void NavEKF::OnGroundCheck()
uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f); 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 lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f)); uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// 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 // Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2); onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
} }
}
void NavEKF::CovarianceInit(float roll, float pitch, float yaw) 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 _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 _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 _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 // Tuning parameters
AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot 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 Vector3f prevDelAng; // previous delta angle use for INS coning error compensation
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation 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 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 earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f dVelIMU1; // delta velocity vector in XYZ body axes measured by IMU1 (m/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) Vector3f dVelIMU2; // delta velocity vector in XYZ body axes measured by IMU2 (m/s)