SITL: sailboat add tide

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;

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

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
 

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);