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 | 74 |
1 files changed, 59 insertions, 15 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 66b227918..f15645fcf 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,47 +51,82 @@ #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_v, 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_side(float k_a) { - _k_side = k_a; + 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_k_roll_ff(float k_roll_ff) { - _k_roll_ff = k_roll_ff; + + void set_k_ff(float k_ff) { + _k_ff = k_ff; } + 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; + } + + void set_coordinated_min_speed(float coordinated_min_speed) { + _coordinated_min_speed = coordinated_min_speed; + } + + + 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 _k_side; + float _k_p; float _k_i; float _k_d; - float _k_roll_ff; + float _k_ff; float _integrator_max; - - float _last_error; + float _max_rate; + float _roll_ff; float _last_output; - float _last_rate_hp_out; - float _last_rate_hp_in; - float _k_d_last; float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; + float _coordinated_min_speed; }; |