diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-11 19:29:43 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-11 19:29:43 +0200 |
commit | eeba000b8728e9b7a8daae0c0ad129257ec39403 (patch) | |
tree | 1ecce53983a9dcb21f2be56ce972846649be07fe /src/modules/ekf_att_pos_estimator/estimator.cpp | |
parent | 5581802f0f2f4bb34ac8c43b8aa9c1c555013758 (diff) | |
download | px4-firmware-eeba000b8728e9b7a8daae0c0ad129257ec39403.tar.gz px4-firmware-eeba000b8728e9b7a8daae0c0ad129257ec39403.tar.bz2 px4-firmware-eeba000b8728e9b7a8daae0c0ad129257ec39403.zip |
stupid fix
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 8 |
1 files changed, 5 insertions, 3 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 068d665a5..4239db669 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2004,15 +2004,15 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", min); + ekf_debug("> max: %8.4f, val: %8.4f", max, val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", max); + ekf_debug("< min: %8.4f, val: %8.4f", min, val); } else { ret = val; } - if (!isfinite) { + if (!isfinite(val)) { ekf_debug("constrain: non-finite!"); } @@ -2119,7 +2119,9 @@ 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++) { |