diff --git a/attitude_fw/ecl_pitch_controller.cpp b/attitude_fw/ecl_pitch_controller.cpp index 5a726f7be7..8fa2930c9c 100644 --- a/attitude_fw/ecl_pitch_controller.cpp +++ b/attitude_fw/ecl_pitch_controller.cpp @@ -112,51 +112,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da lock_integrator = true; } - /* apply turning offset to desired bodyrate setpoint*/ - /* flying inverted (wings upside down)*/ - bool inverted = false; - float constrained_roll; - - /* 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, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - inverted = true; - - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (ctl_data.roll > 0.0f) { - /* right hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - - if (ctl_data.do_turn_compensation) { - /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. - For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 - Availible on google books 8/11/2015: - https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ - float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(constrained_roll) * sinf(constrained_roll))); - - if (inverted) { - body_fixed_turn_offset = -body_fixed_turn_offset; - } - - /* Finally add the turn offset to your bodyrate setpoint*/ - _bodyrate_setpoint += body_fixed_turn_offset; - } - - _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; if (!lock_integrator && _k_i > 0.0f) {