From 1c57d7de434d09893416137f9c72dca2f225cbc7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Sep 2013 19:00:45 +0200 Subject: [PATCH 01/63] 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 +- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39b..59f4b10bee 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 1e6cec6a18..41aa486774 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 b9a73fc029..c42e1856a8 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 0d4ea9333a..e19812ea8e 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(); 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 eb67874db4..c8a59356ca 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,11 +635,11 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + 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); _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, airspeed_scaling, + 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); _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; From cb5e5e914351754356c85e61f2394d1cf91db71f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 18:34:13 +0200 Subject: [PATCH 02/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 59f4b10bee..9b86d69715 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 c42e1856a8..36186ce68b 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; From 17e0c5053ece4cbf53e659b5f60d640beaab7d50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 19:05:23 +0200 Subject: [PATCH 03/63] wip, fw att ctrl: coordinated turn --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 24 +++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 30 +++---------------- .../fw_att_control/fw_att_control_main.cpp | 2 +- 3 files changed, 23 insertions(+), 33 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index b786acf24f..9601fa5447 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 66b2279182..fe0abb956b 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 c8a59356ca..8cb515dcca 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(); From b21b9078e2d719bfd7af9580ac3b9b5957ef1d57 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 22:00:56 +0200 Subject: [PATCH 04/63] wip, fw attitude ctrl: split into attitude and rate, compiles, untested --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 77 +++++++++++------ .../ecl/attitude_fw/ecl_pitch_controller.h | 17 +++- .../ecl/attitude_fw/ecl_roll_controller.cpp | 42 +++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 16 +++- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 86 ++++++++++++++++--- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 62 ++++++++++++- .../fw_att_control/fw_att_control_main.cpp | 84 ++++++++++++++---- 7 files changed, 299 insertions(+), 85 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 9b86d69715..30a46b0e7b 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; - /* Transform estimation to body angular rates */ - float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + /* 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; + } - _rate_error = _rate_setpoint - pitch_rate_body; + } + + 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_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _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 41aa486774..ef6129d026 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 36186ce68b..3afda31765 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 e19812ea8e..f94db0dc77 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 9601fa5447..d2cd9cf041 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 fe0abb956b..d5aaa617fe 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 8cb515dcca..422e94ba18 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 */ From a0ea0901b555b4c7732dbc2da22339d82f581e48 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Oct 2013 20:29:54 +0200 Subject: [PATCH 05/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 30a46b0e7b..e1f4d3eef0 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 422e94ba18..bc9d6771b6 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 97aa275de2..717608159a 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); From b9ef3636f5210588d0aa219a163d3ef5edd6a204 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:43:50 +0200 Subject: [PATCH 06/63] change wrong comment --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 bc9d6771b6..7f3e7893ef 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -465,7 +465,7 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() void FixedwingAttitudeControl::global_pos_poll() { - /* check if there is a new setpoint */ + /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); From 8f74aab468d565101eaa1e0f104b0297343fe2ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 14:14:44 +0200 Subject: [PATCH 07/63] fw att control: bugfixes for integrator --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 11 +++---- .../ecl/attitude_fw/ecl_roll_controller.cpp | 13 ++++---- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 30 ++++++++++++------- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 +++ .../fw_att_control/fw_att_control_main.cpp | 18 +++++++---- .../fw_att_control/fw_att_control_params.c | 1 + 6 files changed, 48 insertions(+), 29 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index e1f4d3eef0..5fe55163f3 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 3afda31765..1aac827923 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 d2cd9cf041..7dfed3379f 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 d5aaa617fe..0250fcb352 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 7f3e7893ef..91981b08a0 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 717608159a..efe40b1efe 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); From ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 19:12:03 +0200 Subject: [PATCH 08/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 5fe55163f3..eb1031cd30 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 1aac827923..ab3ac0a9d3 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 7dfed3379f..e56a8d08d4 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; From 1e89f30120f30f0cac9c4bdd7b81ee2da96bcc8f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:57:21 +0200 Subject: [PATCH 09/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd30..4b43e73e6b 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 ab3ac0a9d3..658a0a5016 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 e56a8d08d4..b6938a59a2 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 efe40b1efe..39b78e20a8 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); From feb75f08cba0d18267f9d463db49f7c1db310596 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 22:07:27 +0200 Subject: [PATCH 10/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd30..5b8897eaad 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 ab3ac0a9d3..01a114d59f 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 e56a8d08d4..5c9cc820fd 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); } From 3c14483df247fb842cabdbb02206912f7f4350cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 00:35:20 +0200 Subject: [PATCH 11/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 5b8897eaad..8b9ba9c626 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 01a114d59f..f3909593a4 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 5c9cc820fd..84d9ebd88c 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 efe40b1efe..d61d3dd4f6 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 From 146279b20fc65705e31e03298a1d3eea8911d0f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 15:03:11 +0200 Subject: [PATCH 12/63] wip fw ctrl, several small bugfixes, set limit to 1 --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++--- .../ecl/attitude_fw/ecl_pitch_controller.h | 1 - .../ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++--- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 - .../ecl/attitude_fw/ecl_yaw_controller.cpp | 9 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 - .../fw_att_control/fw_att_control_main.cpp | 49 ++++++++++--------- .../fw_att_control/fw_att_control_params.c | 1 + 8 files changed, 44 insertions(+), 44 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 8b9ba9c626..531512493b 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 ef6129d026..ba8ed3e6f2 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 f3909593a4..e5bd7b4f7f 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 f94db0dc77..dd7d1bf534 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 84d9ebd88c..bc036e0821 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 0250fcb352..5c00fa8731 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 91981b08a0..1b30462a88 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 d61d3dd4f6..c48f3a5e3d 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); From 273bc52acc73cb43f31acf5869d0e536def0eec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 19:22:08 +0200 Subject: [PATCH 13/63] fw att ctrl: transpose R --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 7f3e7893ef..b59a97136e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -664,9 +664,9 @@ FixedwingAttitudeControl::task_main() 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; + 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_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"); } From 61fafcbc785525641b137389e3e5cd96b72cfbc1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 16:23:32 +0200 Subject: [PATCH 14/63] fw ctrl: rename parameters to more consistent names --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++-------- .../fw_att_control/fw_att_control_params.c | 30 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) 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 1b30462a88..2772593305 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -288,25 +288,25 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _airspeed_valid(false) { _parameter_handles.tconst = param_find("FW_ATT_TC"); - _parameter_handles.p_p = param_find("FW_P_P"); - _parameter_handles.p_d = param_find("FW_P_D"); - _parameter_handles.p_i = param_find("FW_P_I"); + _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_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_IMAX"); + _parameter_handles.p_integrator_max = param_find("FW_PR_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_IMAX"); + _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_integrator_max = param_find("FW_RR_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_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_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); 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 c48f3a5e3d..07fac49890 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -50,7 +50,7 @@ * Controller parameters, accessible via MAVLink * */ - +//xxx: update descriptions // @DisplayName Attitude Time Constant // @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. // @Range 0.4 to 1.0 seconds, in tens of seconds @@ -59,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); // @DisplayName Proportional gain. // @Description This defines how much the elevator input will be commanded dependend on the current pitch error. // @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f); // @DisplayName Damping gain. // @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. // @Range 0.0 to 10.0, 0.1 increments -PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove // @DisplayName Integrator gain. // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0 to 50.0 -PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f); // @DisplayName Maximum positive / up pitch rate. // @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); // @DisplayName Maximum negative / down pitch rate. // @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); // @DisplayName Pitch Integrator Anti-Windup // @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_PR_IMAX, 1000.0f); // @DisplayName Roll feedforward gain. // @Description This compensates during turns and ensures the nose stays level. @@ -99,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); // @Range 10.0 200.0 // @Increment 10.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f); // @DisplayName Damping Gain // @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. // @Range 0.0 10.0 // @Increment 1.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove // @DisplayName Integrator Gain // @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. // @Range 0.0 100.0 // @Increment 5.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f); // @DisplayName Roll Integrator Anti-Windup // @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_RR_IMAX, 1000.0f); // @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. @@ -128,10 +128,10 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); 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_D, 0); +PARAM_DEFINE_FLOAT(FW_YR_P, 0.5); +PARAM_DEFINE_FLOAT(FW_YR_I, 0.05); +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 1000.0f); +PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); From a5046fdfa02999f153502168682ade9bd5c15842 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 20:10:31 +0200 Subject: [PATCH 15/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 531512493b..6268122882 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 e5bd7b4f7f..9e20071311 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 bc036e0821..bd1f318a72 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); } From 00825aa3cf7a04de148f763b96ee69f977d2656f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 17:36:49 +0200 Subject: [PATCH 16/63] fix small bug where roll instead of pitch was used --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 2772593305..c29cf4cc30 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -442,7 +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); +// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); return true; } @@ -698,7 +698,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(roll_u)) ? roll_u : 0.0f; + _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, From 9fce5aa40226d4018794b7520759a6960965ce78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 21:56:04 +0100 Subject: [PATCH 17/63] change _update_height_demand for test --- src/lib/external_lgpl/tecs/tecs.cpp | 56 ++++++++++++++++++----------- src/lib/external_lgpl/tecs/tecs.h | 2 +- 2 files changed, 36 insertions(+), 22 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d2..af596dc5e7 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; +// // 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); - // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, - // _maxClimbRate); + _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 4a98c8e974..224f353664 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); From 5eea669d62b8a6a85b79c8ad3b5b8c08bb54987f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 22:03:49 +0100 Subject: [PATCH 18/63] add missing copy of variable --- src/lib/external_lgpl/tecs/tecs.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index af596dc5e7..51a621f62b 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 From 947207e68da08b7cd14e7da0b44d000b1c435b11 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 12:15:14 +0100 Subject: [PATCH 19/63] 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(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 51a621f62b..baf893dd20 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; From d944eeabf08620413c4d99c26de339fd6f6e7e8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 13:32:42 +0100 Subject: [PATCH 20/63] remove filtering of height setpoint --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index baf893dd20..5cbc19dbba 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 From c0100b5e17b87b1c721a115bdbe535ab606bd58e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 14:20:30 +0100 Subject: [PATCH 21/63] 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(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5cbc19dbba..a94a06dda2 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 224f353664..d07bd00dd6 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 ffa7915a7d..dbdab5961a 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 3bb872405e..a6aa45258c 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); From 1214cbe71f57e980e26d520dda693c089265e1e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 17:15:55 +0100 Subject: [PATCH 22/63] wip towards glide slope tracking for autoland --- .../fw_pos_control_l1_main.cpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) 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 ffa7915a7d..000fa5fa30 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 @@ -759,10 +759,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; - } +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// } _att_sp.roll_body = _l1_control.nav_roll(); @@ -777,6 +777,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = _parameters.airspeed_min; float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param + float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad); + if (altitude_error > -4.0f) { /* land with minimal speed */ @@ -798,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - /* minimize speed to approach speed */ + /* minimize speed to approach speed, stay on glide slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -808,9 +811,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - /* normal cruise speed */ + /* normal cruise speed + * intersect glide slope: if current position higher than slope sink to glide slope but continue horizontally if below slope */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired) { + altitude_desired = landing_slope_alt_desired; + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 18c5d901889e33167f0967f612fb7e57b3b4a018 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 17:53:03 +0100 Subject: [PATCH 23/63] change logic to avoid overshoot over landing spot --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 000fa5fa30..be6cc0e8ce 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 @@ -799,10 +799,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + } else if (wp_distance < 60.0f || altitude_error > -20.0f) { /* minimize speed to approach speed, stay on glide slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, From 97a54d1b5e684be36819816c03a54fe8bfd1792f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 18:19:16 +0100 Subject: [PATCH 24/63] fix missing conversion to absolute altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 be6cc0e8ce..97b659af8d 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 @@ -778,7 +778,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad); + float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad) + _global_triplet.current.altitude; if (altitude_error > -4.0f) { From a6b85cfbf76dbe0e829e503a09660ffb11f003d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 14:02:55 +0100 Subject: [PATCH 25/63] add some comments for clarification --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 97b659af8d..a68de45255 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 @@ -729,8 +729,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < 15.0f || land_noreturn) { @@ -758,16 +758,16 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* normal navigation */ _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + + /* Vertical landing control */ // /* do not go down too early */ // if (wp_distance > 50.0f) { // altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; // } - - - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param From 116ee348a228481512965778f444dd1c5f373939 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 14:37:29 +0100 Subject: [PATCH 26/63] landing: make altitude handling before helper waypoint L more clever --- .../fw_pos_control_l1_main.cpp | 31 ++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) 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 a68de45255..a908995289 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 @@ -279,6 +279,11 @@ private: */ void vehicle_setpoint_poll(); + /** + * Get Altitude on the landing glide slope + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + /** * Control position. */ @@ -624,6 +629,11 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +{ + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) @@ -778,7 +788,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad) + _global_triplet.current.altitude; + const float landingslope_length = 64.0f; + const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); if (altitude_error > -4.0f) { @@ -799,7 +812,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (wp_distance < 60.0f || altitude_error > -20.0f) { + } else if (wp_distance < L_wp_distance) { /* minimize speed to approach speed, stay on glide slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), @@ -811,10 +824,20 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* normal cruise speed - * intersect glide slope: if current position higher than slope sink to glide slope but continue horizontally if below slope */ + * intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally + * if current position is below altitude at L, climb to altitude of L */ float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired) { + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ altitude_desired = landing_slope_alt_desired; + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + /* continue horizontally */ + altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection + } else { + /* climb to L_altitude */ + altitude_desired = L_altitude; } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), From e539a89e225c1d6ab127d7953bceec7efecefbdf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 17:35:09 +0100 Subject: [PATCH 27/63] autoland slope parameters, variable rename for bugfix and code clarity, printfs --- .../fw_pos_control_l1_main.cpp | 41 ++++++++++++++----- .../fw_pos_control_l1_params.c | 3 ++ 2 files changed, 33 insertions(+), 11 deletions(-) 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 a908995289..a4b2778885 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,9 @@ private: float throttle_land_max; float loiter_hold_radius; + + float land_slope_angle; + float land_slope_length; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +243,9 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t land_slope_angle; + param_t land_slope_length; } _parameter_handles; /**< handles for interesting parameters */ @@ -363,6 +369,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -440,6 +449,11 @@ 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.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + + _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)); @@ -673,7 +687,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ math::Vector2f prev_wp; @@ -711,7 +725,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -726,7 +740,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + _l1_control.navigate_loiter(curr_wp, current_position, global_triplet.current.loiter_radius, global_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -741,7 +755,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < 15.0f || land_noreturn) { @@ -766,7 +780,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -787,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = _parameters.airspeed_min; float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - const float landingslope_length = 64.0f; - const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float landingslope_length = _parameters.land_slope_length; + float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); @@ -812,15 +826,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + warnx("Landing: land with minimal speed"); + } else if (wp_distance < L_wp_distance) { - /* minimize speed to approach speed, stay on glide slope */ + /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - + warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); } else { /* normal cruise speed @@ -832,12 +848,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; + warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { /* continue horizontally */ altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); } else { /* climb to L_altitude */ altitude_desired = L_altitude; + warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), @@ -849,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); 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 3bb872405e..a3140fe48f 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,6 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); From 4b2e08a85083f3925e951ec3de3ac41575f11985 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 13:53:30 +0100 Subject: [PATCH 28/63] autoland, fix flare and max landing throttle --- .../fw_pos_control_l1_main.cpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) 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 a4b2778885..af43097cc0 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 @@ -799,7 +799,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = _parameters.airspeed_min; - float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; @@ -807,22 +807,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - if (altitude_error > -4.0f) { + if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ _tecs.set_speed_weight(2.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, _parameters.throttle_max, throttle_land, - math::radians(-10.0f), math::radians(15.0f)); - /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + math::radians(flare_angle_rad), math::radians(15.0f)); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -839,8 +839,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); } else { - /* normal cruise speed - * intersect glide slope: + /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally * if current position is below altitude at L, climb to altitude of L */ @@ -859,7 +858,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 5fec5fecdc3980c973cb8c6ad23a013c8214ace7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 15:13:03 +0100 Subject: [PATCH 29/63] change flare pitch setpoint --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 af43097cc0..08f9d3c15b 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 @@ -795,7 +795,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float flare_angle_rad = -math::radians(5.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = _parameters.airspeed_min; From 937b502d4c3fd582f7be736240f5971e8c0f7c2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 20:33:56 +0100 Subject: [PATCH 30/63] increase landing speed to v_min * 1.3 for more safety --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 08f9d3c15b..9060eac90d 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 @@ -798,7 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = -math::radians(5.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.airspeed_min; + float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); From 9d5f422d245de4ead6b37193e7862ba771febb83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 20:44:12 +0100 Subject: [PATCH 31/63] fix land_noreturn logic --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 9060eac90d..1a263b741d 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 @@ -766,7 +766,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) + if (!land_noreturn) //set target_bearing in first occurrence target_bearing = _att.yaw; //} @@ -774,8 +774,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -5.0f) - land_noreturn = true; + land_noreturn = true; } else { From 89b8acd7a8dd82a7adbd38fb7fea4b422dea64e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 22:04:32 +0100 Subject: [PATCH 32/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 4e27de45c9..ccaed14cea 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 0772f88bc7..4b0bfc6c42 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 0de0eb439f..013bc191ac 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 5c00fa8731..154471cafb 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; From 3b1086b879a78eb48a04c36d0c1a9ad2e7eac6bf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 22:04:32 +0100 Subject: [PATCH 33/63] 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(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 4e27de45c9..ccaed14cea 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 0772f88bc7..4b0bfc6c42 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 0de0eb439f..013bc191ac 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 5c00fa8731..154471cafb 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; From 014e856c1f789b6c27d886bb55617aa7d235f4f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:14:25 +0100 Subject: [PATCH 34/63] fw: make att controller more robust against invalid (nan) setpoints --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 5 ++ .../fw_att_control/fw_att_control_main.cpp | 67 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 013bc191ac..04293efd6b 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 78952cb8de..ffad691350 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 + 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; + /* 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; + 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; + 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; + /* 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], From 303ceab6ed7aca8cedd2d7703e6766c0477d7ab6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:14:25 +0100 Subject: [PATCH 35/63] fw: make att controller more robust against invalid (nan) setpoints --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 5 ++ .../fw_att_control/fw_att_control_main.cpp | 67 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 013bc191ac..04293efd6b 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 78952cb8de..ffad691350 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 + 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; + /* 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; + 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; + 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; + /* 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], From 22dbc03c011ea86f3a500949109c85676583db32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:18:20 +0100 Subject: [PATCH 36/63] remove unnecessary printf --- src/modules/fw_att_control/fw_att_control_main.cpp | 1 - 1 file changed, 1 deletion(-) 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 ffad691350..c88b29056c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -644,7 +644,6 @@ 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; From c4b994d4ca67b21ce77cb8bc87e1096305ad4fad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:18:20 +0100 Subject: [PATCH 37/63] remove unnecessary printf --- src/modules/fw_att_control/fw_att_control_main.cpp | 1 - 1 file changed, 1 deletion(-) 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 ffad691350..c88b29056c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -644,7 +644,6 @@ 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; From dbee6763672f431b386f59e3e9113cc036547347 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 5 Nov 2013 16:49:54 +0100 Subject: [PATCH 38/63] experiment with landing slope hight for more exact landing --- .../fw_pos_control_l1_main.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) 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 a51eff5124..3aa3acb1a7 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 @@ -292,7 +292,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); /** * Control position. @@ -648,9 +648,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative } bool @@ -807,16 +807,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; + float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out + if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ - /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ - _tecs.set_speed_weight(2.0f); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); From a34252d18f08e37153a777a6e2d79272b789b0c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 5 Nov 2013 16:49:54 +0100 Subject: [PATCH 39/63] experiment with landing slope hight for more exact landing --- .../fw_pos_control_l1_main.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) 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 5363801298..45493b95f8 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 @@ -288,7 +288,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); /** * Control position. @@ -642,9 +642,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative } bool @@ -801,16 +801,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; + float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out + if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ - /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ - _tecs.set_speed_weight(2.0f); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); From bdbe64026b0663489f31841fbc894befd839f732 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 13:10:14 +0100 Subject: [PATCH 40/63] introduce experimental flare trajectory --- .../fw_pos_control_l1_main.cpp | 92 ++++++++++++------- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 61 insertions(+), 33 deletions(-) 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 3aa3acb1a7..9de6f83660 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 @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -292,7 +295,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); /** * Control position. @@ -353,7 +356,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -374,7 +380,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -648,9 +654,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } bool @@ -762,7 +768,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -771,15 +778,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) //set target_bearing in first occurrence + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn = true; + land_noreturn_horizontal = true; } else { @@ -791,6 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { @@ -806,13 +814,19 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float landingslope_length = _parameters.land_slope_length; - float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") - float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out + if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -820,18 +834,34 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, throttle_max, throttle_land, - math::radians(flare_angle_rad), math::radians(15.0f)); + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - warnx("Landing: land with minimal speed"); + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; } else if (wp_distance < L_wp_distance) { @@ -841,26 +871,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally - * if current position is below altitude at L, climb to altitude of L */ + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { /* continue horizontally */ - altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); - } else { - /* climb to L_altitude */ - altitude_desired = L_altitude; - warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -957,7 +983,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { 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 b905c66c3b..61d665b172 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 @@ -115,4 +115,4 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); From 7ad907e638103065051b2b857874588b41766ca8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 13:10:14 +0100 Subject: [PATCH 41/63] introduce experimental flare trajectory --- .../fw_pos_control_l1_main.cpp | 92 ++++++++++++------- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 61 insertions(+), 33 deletions(-) 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 45493b95f8..413474a6e5 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 @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -288,7 +291,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); /** * Control position. @@ -349,7 +352,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -370,7 +376,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -642,9 +648,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } bool @@ -756,7 +762,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -765,15 +772,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) //set target_bearing in first occurrence + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn = true; + land_noreturn_horizontal = true; } else { @@ -785,6 +792,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { @@ -800,13 +808,19 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float landingslope_length = _parameters.land_slope_length; - float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") - float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out + if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -814,18 +828,34 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, throttle_max, throttle_land, - math::radians(flare_angle_rad), math::radians(15.0f)); + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - warnx("Landing: land with minimal speed"); + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; } else if (wp_distance < L_wp_distance) { @@ -835,26 +865,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally - * if current position is below altitude at L, climb to altitude of L */ + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { /* continue horizontally */ - altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); - } else { - /* climb to L_altitude */ - altitude_desired = L_altitude; - warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -951,7 +977,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { 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 a3140fe48f..4d7400ce66 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 @@ -113,4 +113,4 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); From 56aa8c7e8d5c6c4ebd4afabfdda4493db1d9290e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 6 Nov 2013 14:43:29 +0100 Subject: [PATCH 42/63] Minor fixes for motor off case --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 413474a6e5..b4edb0b362 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 @@ -820,7 +820,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -830,9 +830,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - } + // } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -897,7 +897,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), From f2fdfd11b804151874eff52ded7c7fd12767baaa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 15:09:15 +0100 Subject: [PATCH 43/63] land motor lim independent of flare --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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 94b3b75baf..9704eacb96 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 @@ -163,6 +163,7 @@ private: bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; + bool land_motor_lim; float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -359,6 +360,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), + land_motor_lim(false), flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -815,7 +817,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float flare_relative_alt = 15.0f; - float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; float H1 = 10.0f; float H0 = flare_relative_alt + H1; @@ -826,7 +828,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -836,9 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - // } + land_motor_lim = true; + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -986,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; + land_motor_lim = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 020964c78a7ec246cd437eeef57e0f57132874ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 15:09:15 +0100 Subject: [PATCH 44/63] land motor lim independent of flare --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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 b4edb0b362..c54a4d2d29 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 @@ -163,6 +163,7 @@ private: bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; + bool land_motor_lim; float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -355,6 +356,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), + land_motor_lim(false), flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -809,7 +811,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float flare_relative_alt = 15.0f; - float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; float H1 = 10.0f; float H0 = flare_relative_alt + H1; @@ -820,7 +822,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -830,9 +832,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - // } + land_motor_lim = true; + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -980,6 +983,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; + land_motor_lim = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 50405f9c7c2aac5518c366231d9d419ba38d5333 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 16:17:49 +0100 Subject: [PATCH 45/63] land: fix logic before L to climb to L if last wp is below L --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 9704eacb96..0127160fb2 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 @@ -879,17 +879,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { + } else { /* continue horizontally */ - altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + altitude_desired = math::max(_global_pos.alt, L_altitude); + warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From f92e7cd8c7eb271b3672e3bb02fcc3a33b8e7808 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 16:17:49 +0100 Subject: [PATCH 46/63] land: fix logic before L to climb to L if last wp is below L --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 c54a4d2d29..919990273e 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 @@ -873,17 +873,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { + } else { /* continue horizontally */ - altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + altitude_desired = math::max(_global_pos.alt, L_altitude); + warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From dd5de28ed92144d767c613ae1db76c0255cea554 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 17:01:29 +0100 Subject: [PATCH 47/63] remove commander from hil startup scripts --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 5 ----- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 5 ----- ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 5 ----- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 5 ----- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 5 ----- 5 files changed, 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 5e4028bbb8..40a13b5d17 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index bbe3b7e288..9b664d63ed 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index d5782a89b8..7b9f41bf6d 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -52,11 +52,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 1c85e04f2a..0cc07ad343 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 8c5e4b6a8f..344d784224 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # From 249500188040b12d5930d839dfc94de270378072 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 18:07:19 +0100 Subject: [PATCH 48/63] parametrize some landing parameters --- .../fw_pos_control_l1_main.cpp | 22 +++++++++++++++---- .../fw_pos_control_l1_params.c | 3 +++ 2 files changed, 21 insertions(+), 4 deletions(-) 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 0127160fb2..0e5f805b46 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 @@ -215,6 +215,10 @@ private: float land_slope_angle; float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -254,6 +258,10 @@ private: param_t land_slope_angle; param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -383,6 +391,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -466,6 +477,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -479,7 +493,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); @@ -816,10 +830,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = 15.0f; - float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = 10.0f; + float H1 = _parameters.land_H1_virt; float H0 = flare_relative_alt + H1; float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); 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 61d665b172..f206d808e4 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 @@ -116,3 +116,6 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); From 6fc430bd391463f89cfcfec2c38d19a0e0b13ba5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 18:07:19 +0100 Subject: [PATCH 49/63] parametrize some landing parameters --- .../fw_pos_control_l1_main.cpp | 22 +++++++++++++++---- .../fw_pos_control_l1_params.c | 3 +++ 2 files changed, 21 insertions(+), 4 deletions(-) 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 919990273e..2e88d02493 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 @@ -213,6 +213,10 @@ private: float land_slope_angle; float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -250,6 +254,10 @@ private: param_t land_slope_angle; param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -379,6 +387,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -460,6 +471,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -474,7 +488,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); @@ -810,10 +824,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = 15.0f; - float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = 10.0f; + float H1 = _parameters.land_H1_virt; float H0 = flare_relative_alt + H1; float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); 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 4d7400ce66..43344513dc 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 @@ -114,3 +114,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); From c75c5a5f30cde02002d4db16d803dc979ce8d4d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Nov 2013 23:11:14 +0100 Subject: [PATCH 50/63] Fixedwing: Enable loiter mode, tested in HIL --- src/modules/commander/commander.cpp | 3 ++- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb784..905d5dee55 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + // XXX: only respect the condition_landed when the local position is actually valid + if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } 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 0e5f805b46..345a08f7b5 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 @@ -711,7 +711,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* execute navigation once we have a setpoint */ - if (_setpoint_valid) { + if (_setpoint_valid && _control_mode.flag_control_auto_enabled) { /* current waypoint (the one currently heading for) */ math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); From 7f793d9753e3ae454096a190f4f4989e14b28597 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 23:20:04 +0100 Subject: [PATCH 51/63] mavlink output instead of printf --- .../fw_pos_control_l1_main.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) 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 0e5f805b46..e01b5813ca 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 @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -94,6 +95,8 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +static int mavlink_fd; + class FixedwingPositionControl { public: @@ -164,6 +167,8 @@ private: bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; + bool land_onslope; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -369,8 +374,12 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), + land_onslope(false), flare_curve_alt_last(0.0f) { + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -854,7 +863,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - land_motor_lim = true; + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + } + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -874,9 +887,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - - land_noreturn_vertical = true; - warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; @@ -888,7 +903,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } + } else { /* intersect glide slope: @@ -899,11 +921,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -1004,6 +1026,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_vertical = false; land_stayonground = false; land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 6064a2af7e8f92fa01c6a10a61c2c353ba93f4bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 23:20:04 +0100 Subject: [PATCH 52/63] mavlink output instead of printf --- .../fw_pos_control_l1_main.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) 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 2e88d02493..cd57feac74 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 @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -94,6 +95,8 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +static int mavlink_fd; + class FixedwingPositionControl { public: @@ -164,6 +167,8 @@ private: bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; + bool land_onslope; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -365,8 +370,12 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), + land_onslope(false), flare_curve_alt_last(0.0f) { + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -848,7 +857,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - land_motor_lim = true; + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + } + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -868,9 +881,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - - land_noreturn_vertical = true; - warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; @@ -882,7 +897,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } + } else { /* intersect glide slope: @@ -893,11 +915,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -998,6 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_vertical = false; land_stayonground = false; land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 668f9dc552040b77d298330ff2dc1dcccdb5b3da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 16:45:22 +0100 Subject: [PATCH 53/63] enable seatbelt without position lock for non rotary wing vehicles --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6b..a50af7dafe 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -239,8 +239,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid) { + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } From a1b80ec3f356aa19544eaa318bc188d57877f16f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 21:27:00 +0100 Subject: [PATCH 54/63] fw: att fix initialization and add parameter to disable coordinated turns at low speed --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 7 +++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 8 +++--- .../fw_att_control/fw_att_control_main.cpp | 28 +++++++++++++------ .../fw_att_control/fw_att_control_params.c | 2 +- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 04293efd6b..a4ecc48a28 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 154471cafb..b5ae1e6afc 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 c88b29056c..a5f3f1d91d 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 41e37e61f7..796ab3f906 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); From b172bcd9122e186d152b7bacf6495f7f27efc80c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 21:27:16 +0100 Subject: [PATCH 55/63] fw pos ctrl: struct initialization --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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 602681ce02..2606956208 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 @@ -380,6 +380,20 @@ FixedwingPositionControl::FixedwingPositionControl() : mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + navigation_capabilities_s _nav_capabilities = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _control_mode = {0}; + vehicle_global_position_s _global_pos = {0}; + vehicle_global_position_set_triplet_s _global_triplet = {0}; + accel_report _accel = {0}; + + + + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); From 22ef0c77352e9a60cecc51fc219dbad4b9bd9d0d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 10 Nov 2013 19:24:37 +0100 Subject: [PATCH 56/63] fix MEAS airspeed and airspeed calibration --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++- .../commander/airspeed_calibration.cpp | 44 ++++++++++++++++--- src/modules/sensors/sensors.cpp | 1 + src/modules/systemlib/airspeed.c | 2 +- .../uORB/topics/differential_pressure.h | 4 +- 7 files changed, 52 insertions(+), 13 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0487848133..62c0d1f17c 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - uint16_t _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9d8ad084e5..de371bf324 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -180,7 +180,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = diff_pres_pa; + report.differential_pressure_pa = (float)diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b3003fc1b0..3cd6d67203 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -36,6 +36,7 @@ * @author Lorenz Meier * @author Sarthak Kaingade * @author Simon Wilks + * @author Thomas Gubler * * Driver for the MEAS Spec series connected via I2C. * @@ -76,6 +77,7 @@ #include #include #include +#include #include #include @@ -184,7 +186,7 @@ MEASAirspeed::collect() //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; + dp_raw = 0x3FFF & dp_raw; //mask the used bits dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; @@ -193,7 +195,11 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value - uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); +// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + const float P_min = -1.0f; + const float P_max = 1.0f; + float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66e..1809f96888 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -37,12 +37,15 @@ */ #include "airspeed_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include +#include #include #include #include +#include #include #include #include @@ -55,10 +58,13 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "dpress"; + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); const int calibration_count = 2500; @@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd) int calibration_counter = 0; float diff_pres_offset = 0.0f; + /* Reset sensor parameters */ + struct airspeed_scale airscale = { + 0.0f, + 1.0f, + }; + + bool paramreset_successful = false; + int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { + if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + paramreset_successful = true; + } + close(fd); + } + + if (!paramreset_successful) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } @@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } @@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } - mavlink_log_info(mavlink_fd, "airspeed calibration done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(); close(diff_pres_sub); return OK; } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..6d38b98ec1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) warn("WARNING: failed to set scale / offsets for airspeed sensor"); + close(fd); } #if 0 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60ff..1a479c2058 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -59,7 +59,7 @@ float calc_indicated_airspeed(float differential_pressure) { - if (differential_pressure > 0) { + if (differential_pressure > 0.0f) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ceb..5d94d42883 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,8 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - uint16_t differential_pressure_pa; /**< Differential pressure reading */ - uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ From 38172497c116548320c74696d795f9198e0bf4e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 13 Nov 2013 11:05:22 +0100 Subject: [PATCH 57/63] 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 ++++++++++++- .../fw_att_control/fw_att_control_main.cpp | 16 ++++++++++++++++ .../fw_att_control/fw_att_control_params.c | 6 +++++- 8 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index ccaed14cea..d7dbbebd42 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 ba8ed3e6f2..2ca0490fdc 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 4b0bfc6c42..bd6c9da712 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 dd7d1bf534..efc7b89441 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 a4ecc48a28..7c366aaf2d 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 b5ae1e6afc..f15645fcf6 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 a5f3f1d91d..53f89c7c4a 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 796ab3f906..240749fed2 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); From f337d62f2da3c4765acc8b88a4cef3381a89c6e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Nov 2013 14:36:52 +0100 Subject: [PATCH 58/63] fw attitude: fix handling of invalid airspeed --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 53f89c7c4a..ff3f133065 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,9 +635,9 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { - airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + airspeed = _parameters.airspeed_trim; } else { airspeed = _airspeed.indicated_airspeed_m_s; From fefaab91cf3a67a9d2f66e94fdb2708709635095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Nov 2013 13:41:14 +0100 Subject: [PATCH 59/63] 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(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959c..31d7cecb7b 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) From a318d0cf40057ee22e792518c1011b6128e3535a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 11:44:41 +0100 Subject: [PATCH 60/63] fix off by one in missionlib --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index fa23f996f3..bb857dc690 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, next_setpoint_index = index + 1; } - while (next_setpoint_index < wpm->size - 1) { + while (next_setpoint_index < wpm->size) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || From 1fa609d165485a834cfc7b648955292f8fe3b86c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 11:44:41 +0100 Subject: [PATCH 61/63] fix off by one in missionlib --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index fa23f996f3..bb857dc690 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, next_setpoint_index = index + 1; } - while (next_setpoint_index < wpm->size - 1) { + while (next_setpoint_index < wpm->size) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || From cc96edfe014b0990fdd489ef1e38df2fad456189 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 16:37:48 +0100 Subject: [PATCH 62/63] tecs: fix wrong != 0 check --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a94a06dda2..510b8ed9c1 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, From 37ef10ceead77876108847e31f56ae68015f5784 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Nov 2013 12:17:42 +0100 Subject: [PATCH 63/63] groundspeed undershoot: take into account altitude difference --- .../fw_pos_control_l1_main.cpp | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) 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 2606956208..84983785bf 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 @@ -318,7 +318,7 @@ private: const struct vehicle_global_position_set_triplet_s &global_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(); + void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet); /** * Shim for calling task_main from task_create. @@ -665,17 +665,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot() +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) { if (_global_pos_valid) { - /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); - /* rotate with current attitude */ + /* rotate ground speed vector with current attitude */ math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed_vector; + float ground_speed_body = yaw_vector * ground_speed; + + /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ + float distance = 0.0f; + float delta_altitude = 0.0f; + if (global_triplet.previous_valid) { + distance = get_distance_to_next_waypoint(global_triplet.previous.lat * 1e-7f, global_triplet.previous.lon * 1e-7f, global_triplet.current.lat * 1e-7f, global_triplet.current.lon * 1e-7f); + delta_altitude = global_triplet.current.altitude - global_triplet.previous.altitude; + } else { + distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), global_triplet.current.lat * 1e-7f, global_triplet.current.lon * 1e-7f); + delta_altitude = global_triplet.current.altitude - _global_pos.alt; + } + + float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); + /* * Ground speed undershoot is the amount of ground velocity not reached @@ -686,7 +698,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; @@ -704,7 +716,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio { bool setpoint = true; - calculate_gndspeed_undershoot(); + calculate_gndspeed_undershoot(current_position, ground_speed, global_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements