diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-30 16:55:08 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-30 16:55:08 +0200 |
| commit | 7a4049b12a8be77bb67906c666cddf93286d39fc (patch) | |
| tree | 0d925b7521ac3c8e4564469efca676dbbeb3ca27 /src/modules/ekf_att_pos_estimator | |
| parent | 918c87912d60544ab70bedccc19d7d6043b0c840 (diff) | |
| download | px4-firmware-7a4049b12a8be77bb67906c666cddf93286d39fc.tar.gz px4-firmware-7a4049b12a8be77bb67906c666cddf93286d39fc.tar.bz2 px4-firmware-7a4049b12a8be77bb67906c666cddf93286d39fc.zip | |
Fix use of declination in estimator, remove bogus measurement value reset on reinit
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 4 |
1 files changed, 1 insertions, 3 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a29421045..86b7efafb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2396,7 +2396,7 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initialHdg = atan2f(-magY, magX); /* true heading is the mag heading minus declination */ - initialHdg -= declination; + initialHdg += declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2528,8 +2528,6 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - magBias.zero(); - magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { |