From 75956755bb04339cdc15805f5ed80f04b25eed52 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Sat, 7 Sep 2019 23:17:09 +0100
Subject: [PATCH] Rover: add dedicated sailboat crosstrack and loiter params

---
 APMrover2/mode_loiter.cpp | 14 +++++++++++---
 APMrover2/sailboat.cpp    | 26 +++++++++++++++++++++-----
 APMrover2/sailboat.h      |  5 +++++
 3 files changed, 37 insertions(+), 8 deletions(-)

diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp
index 03c9d9eeb9..f6918ab641 100644
--- a/APMrover2/mode_loiter.cpp
+++ b/APMrover2/mode_loiter.cpp
@@ -24,15 +24,23 @@ void ModeLoiter::update()
     // get distance (in meters) to destination
     _distance_to_destination = rover.current_loc.get_distance(_destination);
 
+    const float loiter_radius = rover.g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
+
     // if within loiter radius slew desired speed towards zero and use existing desired heading
-    if (_distance_to_destination <= g2.loit_radius) {
+    if (_distance_to_destination <= loiter_radius) {
         // sailboats should not stop unless motoring
         const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
+
+        // if we have a sail but not trying to use it then point into the wind
+        if (!rover.g2.sailboat.tack_enabled() && rover.g2.sailboat.sail_enabled()) {
+            _desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f;
+        }
     } else {
         // P controller with hard-coded gain to convert distance to desired speed       
-        _desired_speed = MIN((_distance_to_destination - g2.loit_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
-        // calculate bearing to destination
+        _desired_speed = MIN((_distance_to_destination - loiter_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
+
+       // calculate bearing to destination
         _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
         float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
         // if destination is behind vehicle, reverse towards it
diff --git a/APMrover2/sailboat.cpp b/APMrover2/sailboat.cpp
index d89c130d27..94cecfbed8 100644
--- a/APMrover2/sailboat.cpp
+++ b/APMrover2/sailboat.cpp
@@ -86,6 +86,24 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_windspeed_min, 0),
 
+    // @Param: XTRACK_MAX
+    // @DisplayName: Sailing vehicle max cross track error
+    // @Description: The sail boat will tack when it reaches this cross track error, defines a corridor of 2 times this value wide, 0 disables
+    // @Units: m
+    // @Range: 5 25
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("XTRACK_MAX", 8, Sailboat, xtrack_max, 10),
+
+    // @Param: LOIT_RADIUS
+    // @DisplayName: Loiter radius
+    // @Description: When in sailing modes the vehicle will keep moving within this loiter radius
+    // @Units: m
+    // @Range: 0 20
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("LOIT_RADIUS", 9, Sailboat, loit_radius, 5),
+
     AP_GROUPEND
 };
 
@@ -129,8 +147,6 @@ void Sailboat::init()
 
     if (tack_enabled()) {
         rover.g2.loit_type.set_default(1);
-        rover.g2.loit_radius.set_default(5);
-        rover.g2.wp_nav.set_default_overshoot(10);
     }
 
     // initialise motor state to USE_MOTOR_ASSIST
@@ -361,10 +377,10 @@ float Sailboat::calc_heading(float desired_heading_cd)
         tack_request_ms = 0;
     }
 
-    // trigger tack if cross track error larger than waypoint_overshoot parameter
-    // this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
+    // trigger tack if cross track error larger than xtrack_max parameter
+    // this effectively defines a 'corridor' of width 2*xtrack_max that the boat will stay within
     const float cross_track_error = rover.g2.wp_nav.crosstrack_error();
-    if ((fabsf(cross_track_error) >= rover.g2.wp_nav.get_overshoot()) && !is_zero(rover.g2.wp_nav.get_overshoot()) && !should_tack && !currently_tacking) {
+    if ((fabsf(cross_track_error) >= xtrack_max) && !is_zero(xtrack_max) && !should_tack && !currently_tacking) {
         // make sure the new tack will reduce the cross track error
         // if were on starboard tack we are traveling towards the left hand boundary
         if (is_positive(cross_track_error) && (current_tack == AP_WindVane::Sailboat_Tack::TACK_STARBOARD)) {
diff --git a/APMrover2/sailboat.h b/APMrover2/sailboat.h
index cf95cb41cb..5fea04c6e1 100644
--- a/APMrover2/sailboat.h
+++ b/APMrover2/sailboat.h
@@ -80,6 +80,9 @@ public:
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
+    // return sailboat loiter radius
+    float get_loiter_radius() const {return loit_radius;}
+
 private:
 
     // true if motor is on to assist with slow tack
@@ -96,6 +99,8 @@ private:
     AP_Float sail_heel_angle_max;
     AP_Float sail_no_go;
     AP_Float sail_windspeed_min;
+    AP_Float xtrack_max;
+    AP_Float loit_radius;
 
     RC_Channel *channel_mainsail;   // rc input channel for controlling mainsail
     bool currently_tacking;         // true when sailboat is in the process of tacking to a new heading