diff options
Diffstat (limited to 'src/lib/ecl')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 22 |
1 files changed, 22 insertions, 0 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 77ec15c53..d0b5fcab7 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -79,9 +79,31 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc airspeed = airspeed_min; } + /* flying inverted (wings upside down) ? */ + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); + } else { + /* 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 (roll > 0.0f) { + /* right hemisphere */ + roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); + } else { + /* left hemisphere */ + roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + /* calculate the offset in the rate resulting from rolling */ float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; + if (inverted) + turn_offset = -turn_offset; float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ |