From 01d6f1d932d5d25ef0976e733b380cdae4a1eb54 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 2 Mar 2019 14:06:38 -0700 Subject: [PATCH] ArduPlane: add body-frame yaw mode for tailsitters --- ArduPlane/quadplane.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index bb451d0ac7..c5b43b47d1 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -739,7 +739,11 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) if (in_vtol_mode() || is_tailsitter()) { // use euler angle attitude control - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, + // this version interprets the first argument as a rate and the third as an angle + // because it is intended to be used with Q_TAILSIT_INPUT=1 where the roll and yaw sticks + // act in the tailsitter's body frame (i.e. roll is MC/earth frame yaw and + // yaw is MC/earth frame roll) + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(plane.nav_roll_cd, plane.nav_pitch_cd, yaw_rate_cds); } else {