diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-12 11:23:48 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-12 11:23:48 +0200 |
commit | 6906dc4edac85113c3217b250c84e89ed1fe26a4 (patch) | |
tree | ff764929c4d9d035c7668dc643741490559b8d00 /src/modules/ekf_att_pos_estimator/estimator.cpp | |
parent | 1b3007aa813829e401849f53552ac5917da71f5b (diff) | |
download | px4-firmware-6906dc4edac85113c3217b250c84e89ed1fe26a4.tar.gz px4-firmware-6906dc4edac85113c3217b250c84e89ed1fe26a4.tar.bz2 px4-firmware-6906dc4edac85113c3217b250c84e89ed1fe26a4.zip |
Minor improvements to estimator
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 11 |
1 files changed, 8 insertions, 3 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e..d05506396 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,6 @@ #include "estimator.h" #include <string.h> +#include <stdarg.h> // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -2119,9 +2120,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { @@ -2483,12 +2482,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) 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; lonRef = referenceLon; hgtRef = referenceHgt; refSet = true; + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); InitializeDynamic(initvelNED, declination); |