diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-15 18:34:13 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 17:39:27 +0200 |
| commit | cb5e5e914351754356c85e61f2394d1cf91db71f (patch) | |
| tree | e6af69311c892c65008c3a667c4c2b448701476d /src/lib | |
| parent | 1c57d7de434d09893416137f9c72dca2f225cbc7 (diff) | |
| download | px4-firmware-cb5e5e914351754356c85e61f2394d1cf91db71f.tar.gz px4-firmware-cb5e5e914351754356c85e61f2394d1cf91db71f.tar.bz2 px4-firmware-cb5e5e914351754356c85e61f2394d1cf91db71f.zip | |
fw att control: also transform rate estimate
Diffstat (limited to 'src/lib')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 | ||||
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 11 |
2 files changed, 17 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 59f4b10be..9b86d6971 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -107,14 +107,19 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc turn_offset = -turn_offset; float pitch_error = pitch_setpoint - pitch; - /* rate setpoint from current error and time constant */ + /* /* Apply P controller: rate setpoint from current error and time constant */ float theta_dot_setpoint = pitch_error / _tc; - _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* add turn offset */ _rate_setpoint += turn_offset; - _rate_error = _rate_setpoint - pitch_rate; + /* Transform estimation to body angular rates */ + float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _rate_error = _rate_setpoint - pitch_rate_body; float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index c42e1856a..36186ce68 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -79,8 +79,12 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; + + /* Apply P controller */ float phi_dot_setpoint = roll_error / _tc; - _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* limit the rate */ if (_max_rate > 0.01f) { @@ -88,8 +92,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } - _rate_error = _rate_setpoint - roll_rate; + /* Transform estimation to body angular rates */ + float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - roll_rate_body; //body angular rate error float ilimit_scaled = 0.0f; |