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_roll_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_roll_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 |
1 files changed, 3 insertions, 2 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc02..c42e1856a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -58,7 +58,7 @@ ECL_RollController::ECL_RollController() : } -float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -79,7 +79,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; - _rate_setpoint = roll_error / _tc; + float phi_dot_setpoint = roll_error / _tc; + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian /* limit the rate */ if (_max_rate > 0.01f) { |