fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-21 22:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support for automatic mag declination setup

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator.cpp
+23 -10
View File
@@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() :
gpsHgt(0.0f), gpsHgt(0.0f),
baroHgt(0.0f), baroHgt(0.0f),
GPSstatus(0), GPSstatus(0),
VtasMeas(0.0f) VtasMeas(0.0f),
magDeclination(0.0f)
{ {
velNED[0] = 0.0f; velNED[0] = 0.0f;
velNED[1] = 0.0f; velNED[1] = 0.0f;
@@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound()
if (StatesNaN(&last_ekf_error)) { if (StatesNaN(&last_ekf_error)) {
ekf_debug("re-initializing dynamic"); ekf_debug("re-initializing dynamic");
InitializeDynamic(velNED); InitializeDynamic(velNED, magDeclination);
return 1; return 1;
} }
@@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound()
return 0; 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 initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch; 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; magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX); initialHdg = atan2f(-magY, magX);
/* true heading is the mag heading minus declination */
initialHdg -= declination;
cosRoll = cosf(initialRoll * 0.5f); cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(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[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; 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 // Clear the init flag
@@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
ZeroVariables(); ZeroVariables();
magDeclination = declination;
// Calculate initial filter quaternion states from raw measurements // Calculate initial filter quaternion states from raw measurements
float initQuat[4]; float initQuat[4];
Vector3f initMagXYZ; Vector3f initMagXYZ;
initMagXYZ = magData - magBias; 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 // Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states // to set initial NED magnetic field states
@@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
magstate.q1 = initQuat[1]; magstate.q1 = initQuat[1];
magstate.q2 = initQuat[2]; magstate.q2 = initQuat[2];
magstate.q3 = initQuat[3]; magstate.q3 = initQuat[3];
magstate.magN = magData.x; magstate.magN = initMagNED.x;
magstate.magE = magData.y; magstate.magE = initMagNED.y;
magstate.magD = magData.z; magstate.magD = initMagNED.z;
magstate.magXbias = magBias.x; magstate.magXbias = magBias.x;
magstate.magYbias = magBias.y; magstate.magYbias = magBias.y;
magstate.magZbias = magBias.z; magstate.magZbias = magBias.z;
@@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f; 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 //store initial lat,long and height
latRef = referenceLat; latRef = referenceLat;
@@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(&last_ekf_error, 0, sizeof(last_ekf_error));
InitializeDynamic(initvelNED); InitializeDynamic(initvelNED, declination);
} }
void AttPosEKF::ZeroVariables() void AttPosEKF::ZeroVariables()
src/modules/ekf_att_pos_estimator/estimator.h
+4 -3
View File
@@ -208,6 +208,7 @@ public:
float innovRng; ///< Range finder innovation float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad) double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m) float hgtRef; // WGS-84 height of reference point (m)
@@ -309,7 +310,7 @@ void OnGroundCheck();
void CovarianceInit(); 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); 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); bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err); void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]); void InitializeDynamic(float (&initvelNED)[3], float declination);
protected: protected:
@@ -342,7 +343,7 @@ bool FilterHealthy();
void ResetHeight(void); 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);
}; };
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+8 -5
View File
@@ -994,11 +994,14 @@ FixedwingEstimator::task_main()
_ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s; _ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f; _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 // Initialize projection
_local_pos.ref_lat = _gps.lat; _local_pos.ref_lat = _gps.lat;
@@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main()
_ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1]; _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);
} }
} }