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

Merge branch 'master' of github.com:PX4/Firmware into simon_test

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-03 13:26:59 +02:00
parent 628477ee2c 2a7954b6cf
commit 73d7c8e383
101 changed files with 2073 additions and 1258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+7 -7
View File
@@ -2201,7 +2201,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt
posNED[2] = -(hgt - hgtReference);
}
void AttPosEKF::calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + (double)posNED[0] * earthRadiusInv;
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
@@ -2831,12 +2831,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
Mat3f DCM;
quat2Tbn(DCM, initQuat);
quat2Tbn(Tbn, initQuat);
Tnb = Tbn.transpose();
Vector3f initMagNED;
initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
magstate.q0 = initQuat[0];
magstate.q1 = initQuat[1];
@@ -2849,7 +2849,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
magstate.magYbias = magBias.y;
magstate.magZbias = magBias.z;
magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM = DCM;
magstate.DCM = Tbn;
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
src/modules/ekf_att_pos_estimator/estimator_23states.h
+2 -2
View File
@@ -252,7 +252,7 @@ int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
@@ -264,7 +264,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);