diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.h')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 62 |
1 files changed, 58 insertions, 4 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index fe0abb956..d5aaa617f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -35,6 +35,15 @@ * @file ecl_yaw_controller.h * Definition of a simple orthogonal coordinated turn yaw PID controller. * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. */ #ifndef ECL_YAW_CONTROLLER_H #define ECL_YAW_CONTROLLER_H @@ -42,24 +51,69 @@ #include <stdbool.h> #include <stdint.h> -class __EXPORT ECL_YawController +class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); - float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, - float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + float control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint); + + float control_bodyrate( float roll, float pitch, + float pitch_rate, float yaw_rate, + float pitch_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator); + + void reset_integrator(); + + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + void set_k_roll_ff(float roll_ff) { + _roll_ff = roll_ff; + } + + float get_rate_error() { + return _rate_error; + } float get_desired_rate() { return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; - float _rate_setpoint; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate; + float _roll_ff; float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; |