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 +++-- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- 4 files changed, 8 insertions(+), 6 deletions(-) (limited to 'src/lib/ecl/attitude_fw') 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; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 1e6cec6a1..41aa48677 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_PitchController public: ECL_PitchController(); - float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); 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) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0d4ea9333..e19812ea8 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_RollController public: ECL_RollController(); - float control(float roll_setpoint, float roll, float roll_rate, + float control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); -- cgit v1.2.3