diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-07 20:16:41 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-07 20:16:41 +0200 |
| commit | 2219dd3fc6586e7da8a1bc7b7514b97261ff8b23 (patch) | |
| tree | a179953054c5cce21259868c42e691748e343b85 /src | |
| parent | 33f98abf8046506e071845a78d6341b6c8749cf0 (diff) | |
| download | px4-firmware-2219dd3fc6586e7da8a1bc7b7514b97261ff8b23.tar.gz px4-firmware-2219dd3fc6586e7da8a1bc7b7514b97261ff8b23.tar.bz2 px4-firmware-2219dd3fc6586e7da8a1bc7b7514b97261ff8b23.zip | |
Undo hacking
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_21states.cpp | 3 |
1 files changed, 1 insertions, 2 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp index 0f5a17a33..67bfec4ea 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -61,8 +61,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - correctedDelAng.z = 0; + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) |