From 32ef6e85eb9f455003a7212dd43bbef4a6e1b9d2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 9 Apr 2018 21:09:49 +0900
Subject: [PATCH] AP_MotorsUGV: support vectored thrust

steering scaled down as throttle increases
---
 APMrover2/AP_MotorsUGV.cpp | 24 ++++++++++++++++++++----
 APMrover2/AP_MotorsUGV.h   |  4 ++++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp
index 5a38bd9075..b7b3c48a17 100644
--- a/APMrover2/AP_MotorsUGV.cpp
+++ b/APMrover2/AP_MotorsUGV.cpp
@@ -79,6 +79,14 @@ const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("THST_EXPO", 9, AP_MotorsUGV, _thrust_curve_expo, 0.0f),
 
+    // @Param: VEC_THR_BASE
+    // @DisplayName: Vector thrust throttle base
+    // @Description: Throttle level above which steering is scaled down when using vector thrust.  zero to disable vectored thrust
+    // @Units: %
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("VEC_THR_BASE", 10, AP_MotorsUGV, _vector_throttle_base, 0.0f),
+
     AP_GROUPEND
 };
 
@@ -98,6 +106,9 @@ void AP_MotorsUGV::init()
 
     // set safety output
     setup_safety_output();
+
+    // sanity check parameters
+    _vector_throttle_base = constrain_float(_vector_throttle_base, 0.0f, 100.0f);
 }
 
 // setup output in case of main CPU failure
@@ -353,12 +364,14 @@ void AP_MotorsUGV::setup_pwm_type()
 // output to regular steering and throttle channels
 void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
 {
-    // always allow steering to move
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
-
     // output to throttle channels
     if (armed) {
-        // handle armed case
+        // vectored thrust handling
+        if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) {
+            // scale steering down linearly as throttle increases above _vector_throttle_base
+            const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
+            steering *= steering_scalar;
+        }
         output_throttle(SRV_Channel::k_throttle, throttle);
     } else {
         // handle disarmed case
@@ -368,6 +381,9 @@ void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
             SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
         }
     }
+
+    // always allow steering to move
+    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
 }
 
 // output to skid steering channels
diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h
index daf090679b..0a4fa3e47d 100644
--- a/APMrover2/AP_MotorsUGV.h
+++ b/APMrover2/AP_MotorsUGV.h
@@ -45,6 +45,9 @@ public:
     // true if vehicle is capable of skid steering
     bool have_skid_steering() const;
 
+    // true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
+    bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
+
     // output to motors and steering servos
     void output(bool armed, float dt);
 
@@ -103,6 +106,7 @@ protected:
     AP_Int8 _throttle_min; // throttle minimum percentage
     AP_Int8 _throttle_max; // throttle maximum percentage
     AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear
+    AP_Float _vector_throttle_base;  // throttle level above which steering is scaled down when using vector thrust.  zero to disable vectored thrust
 
     // internal variables
     float   _steering;  // requested steering as a value from -4500 to +4500