diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-19 19:12:03 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 17:47:36 +0200 |
| commit | ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4 (patch) | |
| tree | aa31b839b7948892336e33c565757357a14a13c1 /src/lib/ecl/attitude_fw | |
| parent | 8f74aab468d565101eaa1e0f104b0297343fe2ed (diff) | |
| download | px4-firmware-ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4.tar.gz px4-firmware-ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4.tar.bz2 px4-firmware-ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4.zip | |
consistent and safer fix for dt calculation
Diffstat (limited to 'src/lib/ecl/attitude_fw')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 | ||||
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 | ||||
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 |
3 files changed, 12 insertions, 3 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 5fe55163f..eb1031cd3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -116,7 +116,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1aac82792..ab3ac0a9d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -86,7 +86,11 @@ float ECL_RollController::control_bodyrate(float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* 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; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7dfed3379..e56a8d08d 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -99,7 +99,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* 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; |