From 1c57d7de434d09893416137f9c72dca2f225cbc7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Sep 2013 19:00:45 +0200 Subject: using jacobians in fw attitude control --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/lib/ecl/attitude_fw/ecl_roll_controller.cpp') 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) { -- cgit v1.2.3