From 4afcc9c289af521c9400c2f30bcd46cd7f975153 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Tue, 13 Aug 2019 22:21:36 +0100
Subject: [PATCH] SITL: sailboat allow motor sailing

---
 libraries/SITL/SIM_Sailboat.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp
index 22692601e8..1a1a4d04e7 100644
--- a/libraries/SITL/SIM_Sailboat.cpp
+++ b/libraries/SITL/SIM_Sailboat.cpp
@@ -32,6 +32,7 @@ namespace SITL {
 
 #define STEERING_SERVO_CH   0   // steering controlled by servo output 1
 #define MAINSAIL_SERVO_CH   3   // main sail controlled by servo output 4
+#define THROTTLE_SERVO_CH   2   // throttle controlled by servo output 3
 
     // very roughly sort of a stability factors for waves
 #define WAVE_ANGLE_GAIN 1
@@ -218,8 +219,16 @@ void Sailboat::update(const struct sitl_input &input)
         hull_drag *= -1.0f;
     }
 
+    // throttle force (for motor sailing)
+    // gives throttle force == hull drag at 10m/s
+    float throttle_force = 0.0f;
+    uint16_t throttle_in = input.servos[THROTTLE_SERVO_CH];
+    if (throttle_in > 900 && throttle_in < 2100) {
+        throttle_force = (throttle_in-1500) * 0.1f;
+    }
+
     // accel in body frame due acceleration from sail and deceleration from hull friction
-    accel_body = Vector3f(force_fwd - hull_drag, 0, 0);
+    accel_body = Vector3f((throttle_force + force_fwd) - hull_drag, 0, 0);
     accel_body /= mass;
 
     // add in accel due to direction change