From 68f252e0dff1b25948b477b573197d0b7682a526 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 6 Nov 2021 18:54:02 +0000
Subject: [PATCH] Plane: tiltrotor: allow tilt wing as flap

---
 ArduPlane/tiltrotor.cpp | 38 ++++++++++++++++++++++++++++++--------
 ArduPlane/tiltrotor.h   |  5 ++++-
 2 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
index f2033be319..a4417d7ee6 100644
--- a/ArduPlane/tiltrotor.cpp
+++ b/ArduPlane/tiltrotor.cpp
@@ -72,6 +72,15 @@ const AP_Param::GroupInfo Tiltrotor::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("FIX_GAIN", 9, Tiltrotor, fixed_gain, 0),
 
+    // @Param: WING_FLAP
+    // @DisplayName: Tiltrotor tilt angle that will be used as flap
+    // @Description: For use on tilt wings, the wing will tilt up to this angle for flap, transistion will be complete when the wing reaches this angle from the forward fight position, 0 disables
+    // @Units: deg
+    // @Increment: 1
+    // @Range: 0 15
+    // @User: Standard
+    AP_GROUPINFO("WING_FLAP", 10, Tiltrotor, flap_angle_deg, 0),
+
     AP_GROUPEND
 };
 
@@ -128,7 +137,7 @@ void Tiltrotor::setup()
 /*
   calculate maximum tilt change as a proportion from 0 to 1 of tilt
  */
-float Tiltrotor::tilt_max_change(bool up) const
+float Tiltrotor::tilt_max_change(bool up, bool in_flap_range) const
 {
     float rate;
     if (up || max_rate_down_dps <= 0) {
@@ -136,7 +145,7 @@ float Tiltrotor::tilt_max_change(bool up) const
     } else {
         rate = max_rate_down_dps;
     }
-    if (type != TILT_TYPE_BINARY && !up) {
+    if (type != TILT_TYPE_BINARY && !up && !in_flap_range) {
         bool fast_tilt = false;
         if (plane.control_mode == &plane.mode_manual) {
             fast_tilt = true;
@@ -158,13 +167,26 @@ float Tiltrotor::tilt_max_change(bool up) const
  */
 void Tiltrotor::slew(float newtilt)
 {
-    float max_change = tilt_max_change(newtilt<current_tilt);
+    float max_change = tilt_max_change(newtilt<current_tilt, newtilt > get_fully_forward_tilt());
     current_tilt = constrain_float(newtilt, current_tilt-max_change, current_tilt+max_change);
 
     // translate to 0..1000 range and output
     SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt);
 }
 
+// return the current tilt value that represens forward flight
+// tilt wings can sustain forward flight with some amount of wing tilt
+float Tiltrotor::get_fully_forward_tilt() const
+{
+    return 1.0 - (flap_angle_deg / 90.0);
+}
+
+// return the target tilt value for forward flight
+float Tiltrotor::get_forward_flight_tilt() const
+{
+    return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) * 0.01);
+}
+
 /*
   update motor tilt for continuous tilt servos
  */
@@ -179,12 +201,12 @@ void Tiltrotor::continuous_update(void)
     if (!quadplane.in_vtol_mode() && (!hal.util->get_soft_armed() || !quadplane.assisted_flight)) {
         // we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as
         // a forward motor
-        slew(1);
+        slew(get_forward_flight_tilt());
 
         max_change = tilt_max_change(false);
 
         float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
-        if (current_tilt < 1) {
+        if (current_tilt < get_fully_forward_tilt()) {
             current_throttle = constrain_float(new_throttle,
                                                     current_throttle-max_change,
                                                     current_throttle+max_change);
@@ -259,14 +281,14 @@ void Tiltrotor::continuous_update(void)
         transition->transition_state >= Tiltrotor_Transition::TRANSITION_TIMER) {
         // we are transitioning to fixed wing - tilt the motors all
         // the way forward
-        slew(1);
+        slew(get_forward_flight_tilt());
     } else {
         // until we have completed the transition we limit the tilt to
         // Q_TILT_MAX. Anything above 50% throttle gets
         // Q_TILT_MAX. Below 50% throttle we decrease linearly. This
         // relies heavily on Q_VFWD_GAIN being set appropriately.
        float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) / 50.0f, 0, 1);
-       slew(settilt * max_angle_deg / 90.0f);
+       slew(MIN(settilt * max_angle_deg / 90.0f, get_forward_flight_tilt()));
     }
 }
 
@@ -436,7 +458,7 @@ bool Tiltrotor::fully_fwd(void) const
     if (!enabled() || (tilt_mask == 0)) {
         return false;
     }
-    return (current_tilt >= 1);
+    return (current_tilt >= get_fully_forward_tilt());
 }
 
 /*
diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h
index 74f0d9b82f..150c6b5d16 100644
--- a/ArduPlane/tiltrotor.h
+++ b/ArduPlane/tiltrotor.h
@@ -48,7 +48,9 @@ public:
     }
 
     bool fully_fwd() const;
-    float tilt_max_change(bool up) const;
+    float tilt_max_change(bool up, bool in_flap_range = false) const;
+    float get_fully_forward_tilt() const;
+    float get_forward_flight_tilt() const;
 
     // update yaw target for tiltrotor transition
     void update_yaw_target();
@@ -66,6 +68,7 @@ public:
     AP_Float tilt_yaw_angle;
     AP_Float fixed_angle;
     AP_Float fixed_gain;
+    AP_Float flap_angle_deg;
 
     float current_tilt;
     float current_throttle;