From 3f7e7d456f0d2484caa5b620eddcf46338e7ef25 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 13 Mar 2017 18:45:46 +1100
Subject: [PATCH] Plane: in fixed wing mode slave quadplane attitude rate
 controller

For quadplanes this uses the attitude controller from fixed wing
when using the multicopter controller to assist fixed wing
flight. This prevents a rate mismatch between the two controller
leading to oscillation
---
 ArduPlane/quadplane.cpp | 31 +++++++++++++++++++++++--------
 ArduPlane/quadplane.h   |  3 +++
 2 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 295878d8cf..a0bbf0fc99 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -616,14 +616,32 @@ void QuadPlane::init_stabilize(void)
     throttle_wait = false;
 }
 
+
+/*
+  ask the multicopter attitude control to match the roll and pitch rates being demanded by the
+  fixed wing controller if not in a pure VTOL mode
+ */
+void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds, float smooth_gain)
+{
+    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,
+                                                                      plane.nav_pitch_cd,
+                                                                      yaw_rate_cds,
+                                                                      smooth_gain);
+    } else {
+        // use the fixed wing desired rates
+        float roll_rate = plane.rollController.get_pid_info().desired;
+        float pitch_rate = plane.pitchController.get_pid_info().desired;
+        attitude_control->input_euler_rate_roll_pitch_yaw(roll_rate*100, pitch_rate*100, yaw_rate_cds);
+    }
+}
+
 // hold in stabilize with given throttle
 void QuadPlane::hold_stabilize(float throttle_in)
 {    
     // call attitude controller
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
-                                                                         plane.nav_pitch_cd,
-                                                                         get_desired_yaw_rate_cds(),
-                                                                         smoothing_gain);
+    multicopter_attitude_rate_update(get_desired_yaw_rate_cds(), smoothing_gain);
 
     if (throttle_in <= 0) {
         motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@@ -709,10 +727,7 @@ void QuadPlane::hold_hover(float target_climb_rate)
     pos_control->set_accel_z(pilot_accel_z);
 
     // call attitude controller
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
-                                                                         plane.nav_pitch_cd,
-                                                                         get_desired_yaw_rate_cds(),
-                                                                         smoothing_gain);
+    multicopter_attitude_rate_update(get_desired_yaw_rate_cds(), smoothing_gain);
 
     // call position controller
     pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false);
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 1655671ca8..10e9f0484b 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -156,6 +156,9 @@ private:
     // initialise throttle_wait when entering mode
     void init_throttle_wait();
 
+    // use multicopter rate controller
+    void multicopter_attitude_rate_update(float yaw_rate_cds, float smoothing_gain);
+    
     // main entry points for VTOL flight modes
     void init_stabilize(void);
     void control_stabilize(void);