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') 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 From cb5e5e914351754356c85e61f2394d1cf91db71f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 18:34:13 +0200 Subject: fw att control: also transform rate estimate --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++++++++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 11 +++++++++-- 2 files changed, 17 insertions(+), 5 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 59f4b10be..9b86d6971 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -107,14 +107,19 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc turn_offset = -turn_offset; float pitch_error = pitch_setpoint - pitch; - /* rate setpoint from current error and time constant */ + /* /* Apply P controller: rate setpoint from current error and time constant */ float theta_dot_setpoint = pitch_error / _tc; - _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* add turn offset */ _rate_setpoint += turn_offset; - _rate_error = _rate_setpoint - pitch_rate; + /* Transform estimation to body angular rates */ + float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _rate_error = _rate_setpoint - pitch_rate_body; float ilimit_scaled = 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 c42e1856a..36186ce68 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -79,8 +79,12 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; + + /* Apply P controller */ float phi_dot_setpoint = roll_error / _tc; - _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* limit the rate */ if (_max_rate > 0.01f) { @@ -88,8 +92,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } - _rate_error = _rate_setpoint - roll_rate; + /* Transform estimation to body angular rates */ + float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - roll_rate_body; //body angular rate error float ilimit_scaled = 0.0f; -- cgit v1.2.3 From 17e0c5053ece4cbf53e659b5f60d640beaab7d50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 19:05:23 +0200 Subject: wip, fw att ctrl: coordinated turn --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 24 ++++++++++++----- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 30 +++------------------- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 3 files changed, 23 insertions(+), 33 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index b786acf24..9601fa544 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,12 +47,10 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _last_error(0.0f), _last_output(0.0f), - _last_rate_hp_out(0.0f), - _last_rate_hp_in(0.0f), - _k_d_last(0.0f), - _integrator(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) + { } @@ -66,7 +64,21 @@ float ECL_YawController::control(float roll, float yaw_rate, float accel_y, floa float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - return 0.0f; +// float psi_dot = 0.0f; +// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// if(denumerator != 0.0f) { +// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body)) +// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// } + + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _last_output = 0.0f; + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator; + } + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 66b227918..fe0abb956 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,39 +50,17 @@ public: 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)); - void reset_integrator(); - void set_k_side(float k_a) { - _k_side = k_a; - } - 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_integrator_max(float max) { - _integrator_max = max; + float get_desired_rate() { + return _rate_setpoint; } private: uint64_t _last_run; - float _k_side; - float _k_i; - float _k_d; - float _k_roll_ff; - float _integrator_max; - - float _last_error; + float _rate_setpoint; float _last_output; - float _last_rate_hp_out; - float _last_rate_hp_in; - float _k_d_last; - float _integrator; + float _max_deflection_rad; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c8a59356c..8cb515dcc 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -661,7 +661,7 @@ FixedwingAttitudeControl::task_main() vehicle_rates_setpoint_s rates_sp; rates_sp.roll = _roll_ctrl.get_desired_rate(); rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + rates_sp.yaw = _yaw_ctrl.get_desired_rate(); rates_sp.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From b21b9078e2d719bfd7af9580ac3b9b5957ef1d57 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 22:00:56 +0200 Subject: wip, fw attitude ctrl: split into attitude and rate, compiles, untested --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 75 ++++++++++++------- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 17 ++++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 42 +++++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 16 +++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 86 ++++++++++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 62 +++++++++++++++- src/modules/fw_att_control/fw_att_control_main.cpp | 84 ++++++++++++++++----- 7 files changed, 298 insertions(+), 84 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 9b86d6971..30a46b0e7 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -56,29 +56,9 @@ ECL_PitchController::ECL_PitchController() : { } -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) +float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, 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 / 1000000; - - /* lock integral for long intervals */ - if (dt_micros > 500000) - lock_integrator = true; - - 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 */ - 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; - } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -106,20 +86,61 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc if (inverted) turn_offset = -turn_offset; + /* Calculate the error */ float pitch_error = pitch_setpoint - pitch; - /* /* Apply P controller: rate setpoint from current error and time constant */ - float theta_dot_setpoint = pitch_error / _tc; - /* Transform setpoint to body angular rates */ - _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate? + /* Apply P controller: rate setpoint from current error and time constant */ + float _rate_setpoint = pitch_error / _tc; /* add turn offset */ _rate_setpoint += turn_offset; + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint; + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; + } + + } + + return _rate_setpoint; +} + +float ECL_PitchController::control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = dt_micros / 1000000; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + 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 */ + 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; + } + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian + /* Transform estimation to body angular rates */ - float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian - _rate_error = _rate_setpoint - pitch_rate_body; + _rate_error = _bodyrate_setpoint - pitch_bodyrate; float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 41aa48677..ef6129d02 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal pitch PID controller. * * @author Lorenz Meier + * @author Thomas Gubler * * Acknowledgements: * @@ -51,13 +52,18 @@ #include #include -class __EXPORT ECL_PitchController +class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); - 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)); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); + + + float control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -94,6 +100,10 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; @@ -109,6 +119,7 @@ private: float _integrator; float _rate_error; float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 36186ce68..3afda3176 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -53,18 +53,38 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } -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) +float ECL_RollController::control_attitude(float roll_setpoint, float roll) +{ + + /* Calculate error */ + float roll_error = roll_setpoint - roll; + + /* Apply P controller */ + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ //XXX: move to body angluar rates + 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; + } + + return _rate_setpoint; +} + +float ECL_RollController::control_bodyrate(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* 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 k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); @@ -78,25 +98,15 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra airspeed = airspeed_min; } - float roll_error = roll_setpoint - roll; - - /* Apply P controller */ - float phi_dot_setpoint = roll_error / _tc; /* Transform setpoint to body angular rates */ - _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate? - - /* 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; - } + _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian /* Transform estimation to body angular rates */ - float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian + float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - roll_rate_body; //body angular rate error + _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index e19812ea8..f94db0dc7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal roll PID controller. * * @author Lorenz Meier + * @author Thomas Gubler * * Acknowledgements: * @@ -51,13 +52,17 @@ #include #include -class __EXPORT ECL_RollController +class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); - 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)); + float control_attitude(float roll_setpoint, float roll); + + float control_bodyrate(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -90,6 +95,10 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; float _tc; @@ -102,6 +111,7 @@ private: float _integrator; float _rate_error; float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 9601fa544..d2cd9cf04 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,37 +47,95 @@ ECL_YawController::ECL_YawController() : _last_run(0), + _tc(0.1f), _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } -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) +float ECL_YawController::control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint) +{ + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = 0.0f; + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + } + + /* limit the rate */ //XXX: move to body angluar rates + 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; + } + + return _rate_setpoint; +} + +float ECL_YawController::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) { /* 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 psi_dot = 0.0f; -// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); -// if(denumerator != 0.0f) { -// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body)) -// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); -// } + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; - /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _last_output = 0.0f; - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison - _last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator; + /* 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; } + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint * cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + + /* Transform estimation to body angular rates */ + float yaw_bodyrate = -sinf(roll) * pitch_rate * cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * 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 = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } 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 + * @author Thomas Gubler + * + * 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 #include -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; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8cb515dcc..422e94ba1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -37,6 +37,7 @@ * Implementation of a generic attitude controller based on classic orthogonal PIDs. * * @author Lorenz Meier + * @author Thomas Gubler * */ @@ -62,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -106,26 +108,28 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -226,6 +230,11 @@ private: */ void vehicle_setpoint_poll(); + /** + * Check for global position updates. + */ + void global_pos_poll(); + /** * Shim for calling task_main from task_create. */ @@ -261,6 +270,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), + _global_pos_sub(-1), /* publications */ _rate_sp_pub(-1), @@ -375,7 +385,7 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ - _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); + _yaw_ctrl.set_k_p(math::radians(_parameters.y_p)); _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); @@ -452,6 +462,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } } +void +FixedwingAttitudeControl::global_pos_poll() +{ + /* check if there is a new setpoint */ + bool global_pos_updated; + orb_check(_global_pos_sub, &global_pos_updated); + + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -476,6 +498,7 @@ FixedwingAttitudeControl::task_main() _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -551,6 +574,8 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); + global_pos_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -635,16 +660,41 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_w = 0.0f; + if(_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[0][1] * _global_pos.vy + _att.R[0][2] * _global_pos.vz; +// speed_body_v = _att.R[1][0] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[1][2] * _global_pos.vz; + speed_body_w = _att.R[2][0] * _global_pos.vx + _att.R[2][1] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + } else { + warnx("Did not get a valid R\n"); + } + + /* Run attitude controllers */ + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(roll_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u,speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_rad = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + float pitch_rad = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + float yaw_rad = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; /* throttle passed through */ -- cgit v1.2.3 From a0ea0901b555b4c7732dbc2da22339d82f581e48 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Oct 2013 20:29:54 +0200 Subject: wip, minor bugfixes in fw att control --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- src/modules/fw_att_control/fw_att_control_params.c | 4 +++- 3 files changed, 8 insertions(+), 6 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 30a46b0e7..e1f4d3eef 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -90,7 +90,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl float pitch_error = pitch_setpoint - pitch; /* Apply P controller: rate setpoint from current error and time constant */ - float _rate_setpoint = pitch_error / _tc; + _rate_setpoint = pitch_error / _tc; /* add turn offset */ _rate_setpoint += turn_offset; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 422e94ba1..bc9d6771b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -673,7 +673,7 @@ FixedwingAttitudeControl::task_main() /* Run attitude controllers */ _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(roll_sp, _att.roll, _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude @@ -709,9 +709,9 @@ FixedwingAttitudeControl::task_main() * only once available */ vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = _yaw_ctrl.get_desired_rate(); + rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); + rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); + rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); rates_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 97aa275de..717608159 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -44,6 +44,8 @@ #include +//XXX resolve unclear naming of paramters: FW_P_D --> FW_PR_P + /* * Controller parameters, accessible via MAVLink * @@ -130,7 +132,7 @@ PARAM_DEFINE_FLOAT(FW_Y_P, 0); PARAM_DEFINE_FLOAT(FW_Y_I, 0); PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); PARAM_DEFINE_FLOAT(FW_Y_D, 0); -PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); -- cgit v1.2.3 From 8f74aab468d565101eaa1e0f104b0297343fe2ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 14:14:44 +0200 Subject: fw att control: bugfixes for integrator --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 +++----- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 13 +++++----- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 30 ++++++++++++++-------- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 +++ src/modules/fw_att_control/fw_att_control_main.cpp | 18 ++++++++----- src/modules/fw_att_control/fw_att_control_params.c | 1 + 6 files changed, 48 insertions(+), 29 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 e1f4d3eef..5fe55163f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -116,8 +116,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - - float dt = dt_micros / 1000000; + float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) @@ -142,11 +141,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _rate_error = _bodyrate_setpoint - pitch_bodyrate; - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -164,9 +161,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* 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 * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 3afda3176..1aac82792 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,6 +45,7 @@ #include #include #include +#include ECL_RollController::ECL_RollController() : _last_run(0), @@ -85,7 +86,7 @@ float ECL_RollController::control_bodyrate(float pitch, /* 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 > 500000) ? 0.0f : (float)dt_micros * 1e-6f; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -108,11 +109,9 @@ float ECL_RollController::control_bodyrate(float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -130,9 +129,11 @@ float ECL_RollController::control_bodyrate(float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d2cd9cf04..7dfed3379 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,6 +44,7 @@ #include #include #include +#include ECL_YawController::ECL_YawController() : _last_run(0), @@ -53,7 +54,8 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _max_deflection_rad(math::radians(45.0f)), + _coordinated(1.0f) { @@ -63,11 +65,18 @@ float ECL_YawController::control_attitude(float roll, float pitch, float speed_body_u, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { +// static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison - _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + if (_coordinated > 0.1) { + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + } + +// if(counter % 20 == 0) { +// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch)); +// } } /* limit the rate */ //XXX: move to body angluar rates @@ -76,6 +85,9 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } + +// counter++; + return _rate_setpoint; } @@ -87,7 +99,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* 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 > 500000) ? 0.0f : (float)dt_micros * 1e-6f; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -110,11 +122,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -132,9 +142,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index d5aaa617f..0250fcb35 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -85,6 +85,9 @@ public: void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } + void set_coordinated(float coordinated) { + _coordinated = coordinated; + } float get_rate_error() { @@ -115,6 +118,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _max_deflection_rad; + float _coordinated; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7f3e7893e..91981b08a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -155,6 +155,7 @@ private: float y_d; float y_roll_feedforward; float y_integrator_max; + float y_coordinated; float airspeed_min; float airspeed_trim; @@ -181,6 +182,7 @@ private: param_t y_d; param_t y_roll_feedforward; param_t y_integrator_max; + param_t y_coordinated; param_t airspeed_min; param_t airspeed_trim; @@ -289,25 +291,27 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + _parameter_handles.y_coordinated = param_find("FW_Y_COORD"); + /* fetch initial parameter values */ parameters_update(); } @@ -361,6 +365,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_d, &(_parameters.y_d)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -390,6 +395,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + _yaw_ctrl.set_coordinated(_parameters.y_coordinated); return OK; } @@ -709,9 +715,9 @@ FixedwingAttitudeControl::task_main() * only once available */ vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); - rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); - rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = _yaw_ctrl.get_desired_rate(); rates_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 717608159..efe40b1ef 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,3 +136,4 @@ PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); +PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); -- cgit v1.2.3 From ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 19:12:03 +0200 Subject: consistent and safer fix for dt calculation --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 +++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 ++++++- 3 files changed, 12 insertions(+), 3 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 5fe55163f..eb1031cd3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -116,7 +116,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* 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 : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1aac82792..ab3ac0a9d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -86,7 +86,11 @@ float ECL_RollController::control_bodyrate(float pitch, /* 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 : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7dfed3379..e56a8d08d 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -99,7 +99,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* 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 : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; -- cgit v1.2.3 From 1e89f30120f30f0cac9c4bdd7b81ee2da96bcc8f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:57:21 +0200 Subject: constrain integrator part in control output until startup detection is available for safety reasons --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 5 +++-- src/modules/fw_att_control/fw_att_control_params.c | 6 +++--- 4 files changed, 12 insertions(+), 9 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 eb1031cd3..4b43e73e6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -161,9 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_roll_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d..658a0a501 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -133,11 +133,12 @@ float ECL_RollController::control_bodyrate(float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d..b6938a59a 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -147,9 +147,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index efe40b1ef..39b78e20a 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_P_IMAX, 0.2f); // @DisplayName Roll feedforward gain. // @Description This compensates during turns and ensures the nose stays level. @@ -119,7 +119,7 @@ PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_R_IMAX, 0.2f); // @DisplayName Maximum Roll Rate // @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. @@ -130,7 +130,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); PARAM_DEFINE_FLOAT(FW_Y_P, 0); PARAM_DEFINE_FLOAT(FW_Y_I, 0); -PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_Y_IMAX, 0.2f); PARAM_DEFINE_FLOAT(FW_Y_D, 0); PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); -- cgit v1.2.3 From feb75f08cba0d18267f9d463db49f7c1db310596 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 22:07:27 +0200 Subject: wip, clean up pid in fw att --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 ++++++++------- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 14 +++++++------- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 16 +++++++++------- 3 files changed, 24 insertions(+), 21 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 eb1031cd3..5b8897eaa 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -122,8 +122,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; /* input conditioning */ if (!isfinite(airspeed)) { @@ -141,13 +141,12 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -162,8 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d..01a114d59 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -92,8 +92,8 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; //xxx: param /* input conditioning */ if (!isfinite(airspeed)) { @@ -113,13 +113,12 @@ float ECL_RollController::control_bodyrate(float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -136,8 +135,9 @@ float ECL_RollController::control_bodyrate(float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d..5c9cc820f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -106,8 +106,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; + /* input conditioning */ if (!isfinite(airspeed)) { @@ -127,13 +128,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -148,8 +148,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } -- cgit v1.2.3 From 3c14483df247fb842cabdbb02206912f7f4350cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 00:35:20 +0200 Subject: fw att ctrl: rename some variables --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 +-- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 4 files changed, 4 insertions(+), 7 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 5b8897eaa..8b9ba9c62 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -163,8 +163,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables (k_d <--> k_p) - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 01a114d59..f3909593a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -136,8 +136,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5c9cc820f..84d9ebd88 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -150,8 +150,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables (k_d <--> k_p) - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index efe40b1ef..d61d3dd4f 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -44,7 +44,7 @@ #include -//XXX resolve unclear naming of paramters: FW_P_D --> FW_PR_P +//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P /* * Controller parameters, accessible via MAVLink -- cgit v1.2.3 From 146279b20fc65705e31e03298a1d3eea8911d0f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 15:03:11 +0200 Subject: wip fw ctrl, several small bugfixes, set limit to 1 --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 +++-- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 1 - src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 - src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 9 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 - src/modules/fw_att_control/fw_att_control_main.cpp | 49 ++++++++++++---------- src/modules/fw_att_control/fw_att_control_params.c | 1 + 8 files changed, 44 insertions(+), 44 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 8b9ba9c62..531512493 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,8 +51,7 @@ ECL_PitchController::ECL_PitchController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _rate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _rate_setpoint(0.0f) { } @@ -148,10 +147,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -163,9 +162,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } 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 ef6129d02..ba8ed3e6f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -120,7 +120,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_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 f3909593a..e5bd7b4f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -54,8 +54,7 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _bodyrate_setpoint(0.0f) { } @@ -96,6 +95,7 @@ float ECL_RollController::control_bodyrate(float pitch, float k_ff = 0; //xxx: param /* input conditioning */ +// warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (airspeed_min + airspeed_max); @@ -120,10 +120,10 @@ float ECL_RollController::control_bodyrate(float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -133,12 +133,13 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + warnx("roll: _last_output %.4f", (double)_last_output); - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } 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 f94db0dc7..dd7d1bf53 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -112,7 +112,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _max_deflection_rad; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 84d9ebd88..bc036e082 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -54,7 +54,6 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)), _coordinated(1.0f) { @@ -135,10 +134,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -150,9 +149,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 0250fcb35..5c00fa873 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -117,7 +117,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _max_deflection_rad; float _coordinated; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 91981b08a..1b30462a8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -156,6 +156,7 @@ private: float y_roll_feedforward; float y_integrator_max; float y_coordinated; + float y_rmax; float airspeed_min; float airspeed_trim; @@ -183,6 +184,7 @@ private: param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated; + param_t y_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -305,6 +307,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -366,6 +369,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); + param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -373,29 +377,30 @@ FixedwingAttitudeControl::parameters_update() /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); - _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); - _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); - _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); - _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_k_p(_parameters.p_p); + _pitch_ctrl.set_k_i(_parameters.p_i); + _pitch_ctrl.set_k_d(_parameters.p_d); + _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); - _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); - _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); - _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); - _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_k_p(_parameters.r_p); + _roll_ctrl.set_k_i(_parameters.r_i); + _roll_ctrl.set_k_d(_parameters.r_d); + _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ - _yaw_ctrl.set_k_p(math::radians(_parameters.y_p)); - _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); - _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); - _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); - _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + _yaw_ctrl.set_k_p(_parameters.y_p); + _yaw_ctrl.set_k_i(_parameters.y_i); + _yaw_ctrl.set_k_d(_parameters.y_d); + _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); + _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated(_parameters.y_coordinated); + _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; } @@ -437,6 +442,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); return true; } @@ -596,9 +602,6 @@ FixedwingAttitudeControl::task_main() if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale from radians to normalized -1 .. 1 range */ - const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); - /* scale around tuning airspeed */ float airspeed; @@ -685,23 +688,23 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above */ - float roll_rad = _roll_ctrl.control_bodyrate(_att.pitch, + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; - float pitch_rad = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + _actuators.control[1] = (isfinite(roll_u)) ? roll_u : 0.0f; - float yaw_rad = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d61d3dd4f..c48f3a5e3 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,4 +136,5 @@ PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); -- cgit v1.2.3 From a5046fdfa02999f153502168682ade9bd5c15842 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 20:10:31 +0200 Subject: fix wrong operation in yaw controller --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 +++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ++++-- 3 files changed, 9 insertions(+), 5 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 531512493..626812288 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,6 +45,7 @@ #include #include #include +#include ECL_PitchController::ECL_PitchController() : _last_run(0), @@ -163,7 +164,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - +// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); +// warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index e5bd7b4f7..9e2007131 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -133,11 +133,11 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); +// warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - warnx("roll: _last_output %.4f", (double)_last_output); +// warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index bc036e082..bd1f318a7 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -119,10 +119,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint * cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian /* Transform estimation to body angular rates */ - float yaw_bodyrate = -sinf(roll) * pitch_rate * cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error @@ -150,6 +150,8 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + return math::constrain(_last_output, -1.0f, 1.0f); } -- cgit v1.2.3 From 9fce5aa40226d4018794b7520759a6960965ce78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 21:56:04 +0100 Subject: change _update_height_demand for test --- src/lib/external_lgpl/tecs/tecs.cpp | 60 +++++++++++++++++++++++-------------- src/lib/external_lgpl/tecs/tecs.h | 2 +- 2 files changed, 38 insertions(+), 24 deletions(-) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d..af596dc5e 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -2,6 +2,7 @@ #include "tecs.h" #include +#include using namespace math; @@ -199,33 +200,46 @@ void TECS::_update_speed_demand(void) _TAS_dem_last = _TAS_dem; } -void TECS::_update_height_demand(float demand) +void TECS::_update_height_demand(float demand, float state) { - // Apply 2 point moving average to demanded height - // This is required because height demand is only updated at 5Hz - _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); - _hgt_dem_in_old = _hgt_dem; - - // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, - // _maxClimbRate); - +// // Apply 2 point moving average to demanded height +// // This is required because height demand is only updated at 5Hz +// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); +// _hgt_dem_in_old = _hgt_dem; +// +// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, +// // _maxClimbRate); +// +// // Limit height rate of change +// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; +// +// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; +// } +// +// _hgt_dem_prev = _hgt_dem; +// +// // Apply first order lag to height demand +// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; +// _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; +// _hgt_dem_adj_last = _hgt_dem_adj; +// +// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, +// // _hgt_rate_dem); + + _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + + _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter // Limit height rate of change - if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + if (_hgt_rate_dem > _maxClimbRate) { + _hgt_rate_dem = _maxClimbRate; - } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } else if (_hgt_rate_dem < -_maxSinkRate) { + _hgt_rate_dem = -_maxSinkRate; } - _hgt_dem_prev = _hgt_dem; - - // Apply first order lag to height demand - _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; - _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; - _hgt_dem_adj_last = _hgt_dem_adj; - - // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, - // _hgt_rate_dem); + warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) @@ -500,7 +514,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar _update_speed_demand(); // Calculate the height demand - _update_height_demand(hgt_dem); + _update_height_demand(hgt_dem, baro_altitude); // Detect underspeed condition _detect_underspeed(); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4a98c8e97..224f35366 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -327,7 +327,7 @@ private: void _update_speed_demand(void); // Update the demanded height - void _update_height_demand(float demand); + void _update_height_demand(float demand, float state); // Detect an underspeed condition void _detect_underspeed(void); -- cgit v1.2.3 From 5eea669d62b8a6a85b79c8ad3b5b8c08bb54987f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 22:03:49 +0100 Subject: add missing copy of variable --- src/lib/external_lgpl/tecs/tecs.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index af596dc5e..51a621f62 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -229,6 +229,7 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + _hgt_dem_adj_last = _hgt_dem_adj; _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter // Limit height rate of change -- cgit v1.2.3 From 947207e68da08b7cd14e7da0b44d000b1c435b11 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 12:15:14 +0100 Subject: change calc of hgt_rate_dem to use the filtered height setpoint --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 51a621f62..baf893dd2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -231,7 +231,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter + _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; -- cgit v1.2.3 From d944eeabf08620413c4d99c26de339fd6f6e7e8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 13:32:42 +0100 Subject: remove filtering of height setpoint --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index baf893dd2..5cbc19dbb 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -228,7 +228,7 @@ void TECS::_update_height_demand(float demand, float state) // // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, // // _hgt_rate_dem); - _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter -- cgit v1.2.3 From c0100b5e17b87b1c721a115bdbe535ab606bd58e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 14:20:30 +0100 Subject: heightrate p as parameter --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 5 +++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++ 4 files changed, 16 insertions(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5cbc19dbb..a94a06dda 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -231,7 +231,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 224f35366..d07bd00dd 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -178,6 +178,10 @@ public: _indicated_airspeed_max = airspeed; } + void set_heightrate_p(float heightrate_p) { + _heightrate_p = heightrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -201,6 +205,7 @@ private: float _vertAccLim; float _rollComp; float _spdWeight; + float _heightrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7..dbdab5961 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -206,6 +206,8 @@ private: float throttle_land_max; float loiter_hold_radius; + + float heightrate_p; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +242,8 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t heightrate_p; } _parameter_handles; /**< handles for interesting parameters */ @@ -370,6 +374,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -435,6 +440,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -453,6 +460,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 3bb872405..a6aa45258 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -111,3 +111,5 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); -- cgit v1.2.3 From 89b8acd7a8dd82a7adbd38fb7fea4b422dea64e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 22:04:32 +0100 Subject: fix initialization of attitude controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++++++++++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 +++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 11 +++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 2 -- 4 files changed, 22 insertions(+), 8 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 4e27de45c..ccaed14ce 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -49,10 +49,19 @@ ECL_PitchController::ECL_PitchController() : _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate_pos(0.0f), + _max_rate_neg(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _rate_setpoint(0.0f) + _rate_setpoint(0.0f), + _bodyrate_setpoint(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 0772f88bc..4b0bfc6c4 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -50,13 +50,17 @@ ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { - } float ECL_RollController::control_attitude(float roll_setpoint, float roll) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0de0eb439..013bc191a 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -48,16 +48,19 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated(1.0f) - + _coordinated(0.0f) { - } float ECL_YawController::control_attitude(float roll, float pitch, diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 5c00fa873..154471caf 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -104,8 +104,6 @@ public: private: uint64_t _last_run; - - float _tc; float _k_p; float _k_i; float _k_d; -- cgit v1.2.3 From 014e856c1f789b6c27d886bb55617aa7d235f4f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:14:25 +0100 Subject: fw: make att controller more robust against invalid (nan) setpoints --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 5 ++ src/modules/fw_att_control/fw_att_control_main.cpp | 75 ++++++++++++++-------- 2 files changed, 52 insertions(+), 28 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 013bc191a..04293efd6 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -90,6 +90,11 @@ float ECL_YawController::control_attitude(float roll, float pitch, // counter++; + if(!isfinite(_rate_setpoint)){ + warnx("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + return _rate_setpoint; } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 78952cb8d..ffad69135 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,8 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { @@ -643,6 +644,7 @@ FixedwingAttitudeControl::task_main() */ roll_sp = _manual.roll * 0.75f; pitch_sp = _manual.pitch * 0.75f; + warnx("copy(2) _att_sp.roll_body %.4f", _att_sp.roll_body); throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -681,33 +683,50 @@ FixedwingAttitudeControl::task_main() } /* Run attitude controllers */ - _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); - _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u,speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - - /* Run attitude RATE controllers which need the desired attitudes from above */ - float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, - _att.rollspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; - - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; - - float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; - - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u,speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + if (!isfinite(roll_u)) { + warnx("roll_u %.4f", roll_u); + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + if (!isfinite(pitch_u)) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", + pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + } + + float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + if (!isfinite(yaw_u)) { + warnx("yaw_u %.4f", yaw_u); + } + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + warnx("throttle_sp %.4f", throttle_sp); + } + } else { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + } // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], -- cgit v1.2.3 From a1b80ec3f356aa19544eaa318bc188d57877f16f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 21:27:00 +0100 Subject: fw: att fix initialization and add parameter to disable coordinated turns at low speed --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 +++--- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 8 +++---- src/modules/fw_att_control/fw_att_control_main.cpp | 28 +++++++++++++++------- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 4 files changed, 29 insertions(+), 16 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 04293efd6..a4ecc48a2 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -59,21 +59,22 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated(0.0f) + _coordinated_min_speed(1.0f) { } float ECL_YawController::control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_w, + float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - if (_coordinated > 0.1) { + if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); if(denumerator != 0.0f) { //XXX: floating point comparison _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; +// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } // if(counter % 20 == 0) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 154471caf..b5ae1e6af 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -57,7 +57,7 @@ public: ECL_YawController(); float control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_w, + 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, @@ -85,8 +85,8 @@ public: void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } - void set_coordinated(float coordinated) { - _coordinated = coordinated; + void set_coordinated_min_speed(float coordinated_min_speed) { + _coordinated_min_speed = coordinated_min_speed; } @@ -115,7 +115,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _coordinated; + float _coordinated_min_speed; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c88b29056..a5f3f1d91 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -155,7 +155,7 @@ private: float y_d; float y_roll_feedforward; float y_integrator_max; - float y_coordinated; + float y_coordinated_min_speed; float y_rmax; float airspeed_min; @@ -183,7 +183,7 @@ private: param_t y_d; param_t y_roll_feedforward; param_t y_integrator_max; - param_t y_coordinated; + param_t y_coordinated_min_speed; param_t y_rmax; param_t airspeed_min; @@ -287,6 +287,17 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false), _airspeed_valid(false) { + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + accel_report _accel = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _vcontrol_mode = {0}; + actuator_controls_s _actuators = {0}; + vehicle_global_position_s _global_pos = {0}; + + _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_d = param_find("FW_PR_D"); @@ -313,7 +324,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); - _parameter_handles.y_coordinated = param_find("FW_Y_COORD"); + _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); /* fetch initial parameter values */ parameters_update(); @@ -368,7 +379,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_d, &(_parameters.y_d)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -399,7 +410,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_d(_parameters.y_d); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); - _yaw_ctrl.set_coordinated(_parameters.y_coordinated); + _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; @@ -672,10 +683,11 @@ FixedwingAttitudeControl::task_main() /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; + float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; -// speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; + speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; } else { warnx("Did not get a valid R\n"); @@ -686,7 +698,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.control_attitude(roll_sp, _att.roll); _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u,speed_body_w, + speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above */ @@ -709,7 +721,7 @@ FixedwingAttitudeControl::task_main() pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); } - float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41e37e61f..796ab3f90 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -137,4 +137,4 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); -PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); +PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); -- cgit v1.2.3 From 38172497c116548320c74696d795f9198e0bf4e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 13 Nov 2013 11:05:22 +0100 Subject: reintroduce feedforward --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 10 ++++++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 10 ++++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 13 ++++++++++++- src/modules/fw_att_control/fw_att_control_main.cpp | 16 ++++++++++++++++ src/modules/fw_att_control/fw_att_control_params.c | 6 +++++- 8 files changed, 59 insertions(+), 5 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 ccaed14ce..d7dbbebd4 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -53,6 +53,7 @@ ECL_PitchController::ECL_PitchController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate_pos(0.0f), _max_rate_neg(0.0f), @@ -173,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index ba8ed3e6f..2ca0490fd 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -73,21 +73,30 @@ public: 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_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; } + void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; } + void set_roll_ff(float roll_ff) { _roll_ff = roll_ff; } @@ -111,6 +120,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate_pos; float _max_rate_neg; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 4b0bfc6c4..bd6c9da71 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -53,6 +53,7 @@ ECL_RollController::ECL_RollController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _last_output(0.0f), @@ -141,7 +142,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index dd7d1bf53..efc7b8944 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -71,18 +71,27 @@ public: _tc = time_constant; } } + 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_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; } @@ -105,6 +114,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _last_output; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a4ecc48a2..7c366aaf2 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _roll_ff(0.0f), @@ -159,7 +160,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index b5ae1e6af..f15645fcf 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -69,22 +69,32 @@ public: 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_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; } @@ -107,6 +117,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _roll_ff; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a5f3f1d91..53f89c7c4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -141,6 +141,7 @@ private: float p_p; float p_d; float p_i; + float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; @@ -148,11 +149,13 @@ private: float r_p; float r_d; float r_i; + float r_ff; float r_integrator_max; float r_rmax; float y_p; float y_i; float y_d; + float y_ff; float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; @@ -169,6 +172,7 @@ private: param_t p_p; param_t p_d; param_t p_i; + param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; @@ -176,11 +180,13 @@ private: param_t r_p; param_t r_d; param_t r_i; + param_t r_ff; param_t r_integrator_max; param_t r_rmax; param_t y_p; param_t y_i; param_t y_d; + param_t y_ff; param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; @@ -302,6 +308,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_d = param_find("FW_PR_D"); _parameter_handles.p_i = param_find("FW_PR_I"); + _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); @@ -310,12 +317,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_d = param_find("FW_RR_D"); _parameter_handles.r_i = param_find("FW_RR_I"); + _parameter_handles.r_ff = param_find("FW_RR_FF"); _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); _parameter_handles.y_d = param_find("FW_YR_D"); + _parameter_handles.y_ff = param_find("FW_YR_FF"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); @@ -363,6 +372,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); @@ -371,12 +381,15 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); @@ -391,6 +404,7 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_d(_parameters.p_d); + _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); @@ -401,6 +415,7 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_d(_parameters.r_d); + _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); @@ -408,6 +423,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_d(_parameters.y_d); + _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 796ab3f90..240749fed 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. +f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without @@ -138,3 +138,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); + +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f); -- cgit v1.2.3 From fefaab91cf3a67a9d2f66e94fdb2708709635095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Nov 2013 13:41:14 +0100 Subject: l1: change a max to min in wp switch-distance calculation --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959..31d7cecb7 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::max(wp_radius, _L1_distance); + return math::min(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) -- cgit v1.2.3 From cc96edfe014b0990fdd489ef1e38df2fad456189 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 16:37:48 +0100 Subject: tecs: fix wrong != 0 check --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/lib') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a94a06dda..510b8ed9c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -314,7 +314,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time - if (fabsf(_throttle_slewrate) < 0.01f) { + if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; _throttle_dem = constrain(_throttle_dem, -- cgit v1.2.3