5 files changed, 16 insertions, 12 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index d876f1a39..2eb58abd6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-
-	float dt = dt_micros / 1000000;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	if (dt_micros > 500000)
@@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 
 	_rate_error = _rate_setpoint - pitch_rate;
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index b9a73fc02..0b1ffa7a4 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
 
 	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
 	float k_i_rate = _k_i * _tc;
@@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	_rate_error = _rate_setpoint - roll_rate;
 
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 4a98c8e97..f8f832ed7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -54,7 +54,9 @@ public:
 		_SPE_est(0.0f),
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
-		_SKEdot(0.0f) {
+		_SKEdot(0.0f),
+		_vel_dot(0.0f),
+		_STEdotErrLast(0.0f) {
 
 	}
 
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..1945bb02d 100644
--- a/src/lib/mathlib/math/arm/Matrix.hpp
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
@@ -121,10 +121,10 @@ public:
 		for (size_t i = 0; i < getRows(); i++) {
 			for (size_t j = 0; j < getCols(); j++) {
 				float sig;
-				int exp;
+				int exponent;
 				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
+				float2SigExp(num, sig, exponent);
+				printf("%6.3fe%03d ", (double)sig, exponent);
 			}
 
 			printf("\n");
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
index 4155800e8..52220fc15 100644
--- a/src/lib/mathlib/math/arm/Vector.hpp
+++ b/src/lib/mathlib/math/arm/Vector.hpp
@@ -109,10 +109,10 @@ public:
 	inline void print() const {
 		for (size_t i = 0; i < getRows(); i++) {
 			float sig;
-			int exp;
+			int exponent;
 			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
+			float2SigExp(num, sig, exponent);
+			printf("%6.3fe%03d ", (double)sig, exponent);
 		}
 
 		printf("\n");