stupid fix

2 files changed, 6 insertions, 4 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++) {
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 997ea1759..9065063ce 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -1379,7 +1379,7 @@ FixedwingEstimator::print_status()
 	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-	printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec);
+	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
 	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
 	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);