SITL: sailboat allow motor sailing

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