From 872c3df45c97553f7f5f1ccd5cbf43a20c853511 Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Thu, 31 Jan 2019 20:40:19 +0000
Subject: [PATCH] Plane: tailsitter pull in copter ouputs from AP_motors

---
 ArduPlane/tailsitter.cpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index 4bb534b872..7543bfed6b 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -108,6 +108,12 @@ void QuadPlane::tailsitter_output(void)
     plane.pitchController.reset_I();
     plane.rollController.reset_I();
 
+    // pull in copter control outputs
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, (motors->get_throttle()) * 100);
+
     if (hal.util->get_soft_armed()) {
         // scale surfaces for throttle
         tailsitter_speed_scaling();
@@ -123,12 +129,12 @@ void QuadPlane::tailsitter_output(void)
           takeoff without integrator windup
          */
         int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
-        float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0;
+        float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
         float extra_sign = extra_pitch > 0?1:-1;
-        float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500;
+        float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
         tilt_left  = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
         tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain;
-        if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) {
+        if (fabsf(tilt_left) >= SERVO_MAX || fabsf(tilt_right) >= SERVO_MAX) {
             // prevent integrator windup
             motors->limit.roll_pitch = 1;
             motors->limit.yaw = 1;