diff options
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 7 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 4 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Matrix.hpp | 6 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Vector.hpp | 6 |
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"); |