From 2a3676003aadf9fe3b2475b72e9687d77379e099 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Tue, 28 May 2019 00:52:11 +0100
Subject: [PATCH] SITL: sailboat add tide

---
 libraries/SITL/SIM_Sailboat.cpp | 13 +++++++++++--
 libraries/SITL/SIM_Sailboat.h   |  2 ++
 libraries/SITL/SITL.cpp         |  4 +++-
 libraries/SITL/SITL.h           |  5 +++++
 4 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp
index 40333b86a5..22692601e8 100644
--- a/libraries/SITL/SIM_Sailboat.cpp
+++ b/libraries/SITL/SIM_Sailboat.cpp
@@ -198,7 +198,7 @@ void Sailboat::update(const struct sitl_input &input)
     float delta_time = frame_time_us * 1.0e-6f;
 
     // speed in m/s in body frame
-    Vector3f velocity_body = dcm.transposed() * velocity_ef;
+    Vector3f velocity_body = dcm.transposed() * velocity_ef_water;
 
     // speed along x axis, +ve is forward
     float speed = velocity_body.x;
@@ -240,8 +240,17 @@ void Sailboat::update(const struct sitl_input &input)
     // acceleration (ie. real movement), plus gravity
     accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
 
+    // tide calcs
+    Vector3f tide_velocity_ef;
+     if (hal.util->get_soft_armed() && !is_zero(sitl->tide.speed) ) {
+        tide_velocity_ef.x = -cosf(radians(sitl->tide.direction)) * sitl->tide.speed;
+        tide_velocity_ef.y = -sinf(radians(sitl->tide.direction)) * sitl->tide.speed;
+        tide_velocity_ef.z = 0.0f;
+     }
+
     // new velocity vector
-    velocity_ef += accel_earth * delta_time;
+    velocity_ef_water += accel_earth * delta_time;
+    velocity_ef = velocity_ef_water + tide_velocity_ef;
 
     // new position vector
     position += velocity_ef * delta_time;
diff --git a/libraries/SITL/SIM_Sailboat.h b/libraries/SITL/SIM_Sailboat.h
index 3a62cf7cd6..df0f5a0e00 100644
--- a/libraries/SITL/SIM_Sailboat.h
+++ b/libraries/SITL/SIM_Sailboat.h
@@ -62,6 +62,8 @@ private:
 
     const float mass = 2.0f;
 
+    Vector3f velocity_ef_water; // m/s
+
     // simulate basic waves / swell
     void update_wave(float delta_time);
     Vector3f wave_gyro; // rad/s
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 0ee6a90d9d..906e2975d7 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -170,12 +170,14 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
 
     AP_GROUPINFO("GPS_HDG",     43, SITL,  gps_hdg_enabled, 0),
 
-    // sailboat wave simulation parameters
+    // sailboat wave and tide simulation parameters
     AP_GROUPINFO("WAVE_ENABLE", 44, SITL,  wave.enable, 0.0f),
     AP_GROUPINFO("WAVE_LENGTH", 45, SITL,  wave.length, 10.0f),
     AP_GROUPINFO("WAVE_AMP",    46, SITL,  wave.amp, 0.5f),
     AP_GROUPINFO("WAVE_DIR",    47, SITL,  wave.direction, 0.0f),
     AP_GROUPINFO("WAVE_SPEED",  48, SITL,  wave.speed, 0.5f),
+    AP_GROUPINFO("TIDE_DIR",    49, SITL,  tide.direction, 0.0f),
+    AP_GROUPINFO("TIDE_SPEED",  50, SITL,  tide.speed, 0.0f),
 
     AP_GROUPEND
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index b61156bc31..aaf8430fe0 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -263,6 +263,11 @@ public:
         AP_Float speed;     // m/s
     } wave;
 
+    struct {
+        AP_Float direction; // deg (direction tide is coming from)
+        AP_Float speed;     // m/s
+    } tide;
+
     uint16_t irlock_port;
 
     void simstate_send(mavlink_channel_t chan);