diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-09 21:06:08 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-10 11:53:43 +0200 |
commit | 5182860a68fe5733062bd342fbe85310497e20d3 (patch) | |
tree | 59316aaf93d65d56f55f3014cab14486f57bd070 /src | |
parent | b6a0437c7c474b62400ed5430d4cc1b308eee513 (diff) | |
download | px4-firmware-5182860a68fe5733062bd342fbe85310497e20d3.tar.gz px4-firmware-5182860a68fe5733062bd342fbe85310497e20d3.tar.bz2 px4-firmware-5182860a68fe5733062bd342fbe85310497e20d3.zip |
Added support for inverted flight
Diffstat (limited to 'src')
-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 */ |