From 65755454e07e836f480d57523f290ca51109e392 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 9 May 2017 22:45:18 -0600 Subject: [PATCH] ArduPlane: set tailsitter VTOL pitch limits using only Q_ANGLE_MAX --- ArduPlane/ArduPlane.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 6dc0fd3a1e..8e4b140f50 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -774,12 +774,16 @@ void Plane::update_flight_mode(void) nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); float pitch_input = channel_pitch->norm_input(); - if (pitch_input > 0) { - nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max); + if (quadplane.is_tailsitter()) { + nav_pitch_cd = pitch_input * quadplane.aparm.angle_max; } else { - nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max); + if (pitch_input > 0) { + nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max); + } else { + nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max); + } + nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); break; }