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_pitch_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp') 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; -- cgit v1.2.3