diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-09-29 19:00:45 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 17:39:12 +0200 |
| commit | 1c57d7de434d09893416137f9c72dca2f225cbc7 (patch) | |
| tree | 1220475a819ff7e24ca3ecf9afad76f818253ff8 /src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | |
| parent | 8193382ec245ff82ffb30e2d9038c253bc93a099 (diff) | |
| download | px4-firmware-1c57d7de434d09893416137f9c72dca2f225cbc7.tar.gz px4-firmware-1c57d7de434d09893416137f9c72dca2f225cbc7.tar.bz2 px4-firmware-1c57d7de434d09893416137f9c72dca2f225cbc7.zip | |
using jacobians in fw attitude control
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 |
1 files changed, 3 insertions, 2 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..59f4b10be 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -56,7 +56,7 @@ ECL_PitchController::ECL_PitchController() : { } -float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -108,7 +108,8 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; + float theta_dot_setpoint = pitch_error / _tc; + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian /* add turn offset */ _rate_setpoint += turn_offset; |