diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-03 13:26:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-03 13:26:59 +0200 |
commit | 73d7c8e3833211d49ecec0ae9e9fd4d0307d5184 (patch) | |
tree | 1cc84659bb2265d6df338ecf325e7c0892dae60b /src/modules/ekf_att_pos_estimator/estimator_23states.cpp | |
parent | 628477ee2c75d16758688f53c0f8d9f29457f831 (diff) | |
parent | 2a7954b6cf068e36c16dc9170a86970ed46856a9 (diff) | |
download | px4-firmware-73d7c8e3833211d49ecec0ae9e9fd4d0307d5184.tar.gz px4-firmware-73d7c8e3833211d49ecec0ae9e9fd4d0307d5184.tar.bz2 px4-firmware-73d7c8e3833211d49ecec0ae9e9fd4d0307d5184.zip |
Merge branch 'master' of github.com:PX4/Firmware into simon_test
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_23states.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 |
1 files changed, 7 insertions, 7 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d77f4a6e3..393cf4c5a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -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 |