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

Added support for automatic mag declination setup

This commit is contained in:
Lorenz Meier 2014-04-28 18:34:33 +02:00
parent 2c37ec16dc
commit 90569d22bc
3 changed files with 35 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() :
gpsHgt(0.0f),
baroHgt(0.0f),
GPSstatus(0),
VtasMeas(0.0f)
VtasMeas(0.0f),
magDeclination(0.0f)
{
velNED[0] = 0.0f;
velNED[1] = 0.0f;
@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound()
if (StatesNaN(&last_ekf_error)) {
ekf_debug("re-initializing dynamic");
InitializeDynamic(velNED);
InitializeDynamic(velNED, magDeclination);
return 1;
}
@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound()
return 0;
}
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
/* true heading is the mag heading minus declination */
initialHdg -= declination;
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
/* normalize */
float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]);
initQuat[0] /= norm;
initQuat[1] /= norm;
initQuat[2] /= norm;
initQuat[3] /= norm;
}
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
{
// Clear the init flag
@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
ZeroVariables();
magDeclination = declination;
// Calculate initial filter quaternion states from raw measurements
float initQuat[4];
Vector3f initMagXYZ;
initMagXYZ = magData - magBias;
AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat);
AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat);
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
magstate.q1 = initQuat[1];
magstate.q2 = initQuat[2];
magstate.q3 = initQuat[3];
magstate.magN = magData.x;
magstate.magE = magData.y;
magstate.magD = magData.z;
magstate.magN = initMagNED.x;
magstate.magE = initMagNED.y;
magstate.magD = initMagNED.z;
magstate.magXbias = magBias.x;
magstate.magYbias = magBias.y;
magstate.magZbias = magBias.z;
@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f;
}
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt)
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
{
//store initial lat,long and height
latRef = referenceLat;
@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
InitializeDynamic(initvelNED);
InitializeDynamic(initvelNED, declination);
}
void AttPosEKF::ZeroVariables()

7
src/modules/ekf_att_pos_estimator/estimator.h
View File

@ -208,6 +208,7 @@ public:
float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
@ -309,7 +310,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt);
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
@ -334,7 +335,7 @@ void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
@ -342,7 +343,7 @@ bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};

13
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -994,11 +994,14 @@ FixedwingEstimator::task_main()
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt);
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main()
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f);
_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f);
}
}