From 8398de7515671b52df19a032162ff2064d68772a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 12:53:39 +0200 Subject: WIP on controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 ++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ++++++ 3 files changed, 13 insertions(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0faba287d..c1c2bc5af 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,6 +51,12 @@ ECL_PitchController::ECL_PitchController() : float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + return 0.0f; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b28ecdabe..cdb221b26 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -61,7 +61,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - + float integrator_limit_scaled = 0.0f; return 0.0f; } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0d8a0513f..6dd90f676 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -54,6 +54,12 @@ ECL_YawController::ECL_YawController() : float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + return 0.0f; } -- cgit v1.2.3 From 056fe1c0b93094e86fa80de4102b12f2d406de5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 13:50:38 +0200 Subject: WIP --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 63 ++++++++++++++++++++++-- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 55 +++++++++++++++++++-- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 +- 4 files changed, 116 insertions(+), 12 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index c1c2bc5af..0e1a0d7f6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -44,20 +44,75 @@ ECL_PitchController::ECL_PitchController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _desired_rate(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, - bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float dt = dt_micros / 1000000; - return 0.0f; + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* calculate the offset in the rate resulting from rolling */ + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll)) * _roll; + + float pitch_error = pitch_setpoint - pitch; + /* rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* add turn offset */ + _rate_setpoint += turn_offset; + + _rate_error = _rate_setpoint - pitch_rate; + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 391f90b9d..53134556d 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -49,7 +49,7 @@ public: ECL_PitchController(); float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, - bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); @@ -100,7 +100,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index cdb221b26..1152b2964 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -46,13 +46,14 @@ ECL_RollController::ECL_RollController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, - float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -60,10 +61,56 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float k_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; - float integrator_limit_scaled = 0.0f; + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } - return 0.0f; + float roll_error = roll_setpoint - roll; + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + _rate_error = _rate_setpoint - roll_rate; + + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index bba9ae163..011820765 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -49,7 +49,7 @@ public: ECL_RollController(); float control(float roll_setpoint, float roll, float roll_rate, - float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + 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(); @@ -93,7 +93,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_ROLL_CONTROLLER_H -- cgit v1.2.3 From 9b48fe36229f27242ce8d80d2807f0849b2b55e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:04:39 +0200 Subject: Compiling attitude control, ready for tests --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 22 ++++++++++++++-------- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 19 ++++++++++++------- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 5 +++++ 5 files changed, 33 insertions(+), 17 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0e1a0d7f6..77ec15c53 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -38,13 +38,19 @@ */ #include "ecl_pitch_controller.h" +#include +#include +#include +#include +#include +#include ECL_PitchController::ECL_PitchController() : _last_run(0), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f), + _rate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } @@ -62,7 +68,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc if (dt_micros > 500000) lock_integrator = true; - float k_roll_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; /* input conditioning */ @@ -75,7 +81,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc /* calculate the offset in the rate resulting from rolling */ float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll)) * _roll; + tanf(roll) * sinf(roll)) * _roll_ff; float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ @@ -88,7 +94,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * k_i_rate * dt * scaler; @@ -98,21 +104,21 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ - id = max(id, 0.0f); + id = math::max(id, 0.0f); } else if (_last_output > _max_deflection_rad) { /* only allow motion to center: decrease value */ - id = min(id, 0.0f); + id = math::min(id, 0.0f); } _integrator += id; } /* integrator limit */ - _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); /* store non-limited output */ _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; - return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 53134556d..02ca62dad 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -83,7 +83,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1152b2964..6de30fc33 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -39,6 +39,11 @@ #include "../ecl.h" #include "ecl_roll_controller.h" +#include +#include +#include +#include +#include ECL_RollController::ECL_RollController() : _last_run(0), @@ -61,7 +66,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - float k_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; /* input conditioning */ @@ -86,7 +91,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * k_i_rate * dt * scaler; @@ -96,21 +101,21 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ - id = max(id, 0.0f); + id = math::max(id, 0.0f); } else if (_last_output > _max_deflection_rad) { /* only allow motion to center: decrease value */ - id = min(id, 0.0f); + id = math::min(id, 0.0f); } _integrator += id; } /* integrator limit */ - _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; - return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 011820765..7fbcdf4b8 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -79,7 +79,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 6dd90f676..a0f901e71 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -38,6 +38,11 @@ */ #include "ecl_yaw_controller.h" +#include +#include +#include +#include +#include ECL_YawController::ECL_YawController() : _last_run(0), -- cgit v1.2.3