diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-05 09:20:07 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-05 09:20:07 +0100 |
commit | d3b267c06e53aaf119b66717ac33e78834ea0d69 (patch) | |
tree | e786003fef6936449d0e148a338e77ad310be031 /src | |
parent | 1358d4cb88e70370014410353faf3e51afb1ceb6 (diff) | |
download | px4-firmware-d3b267c06e53aaf119b66717ac33e78834ea0d69.tar.gz px4-firmware-d3b267c06e53aaf119b66717ac33e78834ea0d69.tar.bz2 px4-firmware-d3b267c06e53aaf119b66717ac33e78834ea0d69.zip |
Integral fixes, last parts
Diffstat (limited to 'src')
-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 |
2 files changed, 7 insertions, 5 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) { |