From 391b6dfd00a6c998f396e9ee408da4a116ccd68d Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 26 Jan 2017 09:30:52 +0100 Subject: [PATCH] yaw controller: for now do not do turn compensation when inverted and use the roll setpoint as limit for turn compensation Signed-off-by: Roman --- attitude_fw/ecl_yaw_controller.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp index 8d12dc85b1..344fe2de50 100644 --- a/attitude_fw/ecl_yaw_controller.cpp +++ b/attitude_fw/ecl_yaw_controller.cpp @@ -90,12 +90,14 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control } float constrained_roll; + bool inverted = false; /* roll is used as feedforward term and inverted flight needs to be considered */ if (fabsf(ctl_data.roll) < math::radians(90.0f)) { /* not inverted, but numerically still potentially close to infinity */ constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); } else { + inverted = true; // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity //note: the ranges are extended by 10 deg here to avoid numeric resolution effects if (ctl_data.roll > 0.0f) { @@ -108,9 +110,13 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control } } + constrained_roll = math::constrain(constrained_roll, -ctl_data.roll_setpoint, ctl_data.roll_setpoint); - /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + + if (!inverted) { + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + } /* limit the rate */ //XXX: move to body angluar rates if (_max_rate > 0.01f) { @@ -166,7 +172,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data } /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {