From 5d4579a08625eba9f78bc39f0d43c30591a1360f Mon Sep 17 00:00:00 2001
From: Eugene Shamaev <Eugene.Shamaev@gmail.com>
Date: Mon, 30 Apr 2018 08:26:15 -0700
Subject: [PATCH] AP_HAL_SITL: add wind type parameters

---
 libraries/AP_HAL_SITL/SITL_State.cpp | 32 ++++++++++++++++++++++------
 libraries/AP_HAL_SITL/SITL_State.h   |  1 +
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index e6936256af..c23e532434 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -359,24 +359,44 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
     uint32_t now = AP_HAL::micros();
     last_update_usec = now;
 
-    // pass wind into simulators, using a wind gradient below 60m
     float altitude = _barometer?_barometer->get_altitude():0;
     float wind_speed = 0;
     float wind_direction = 0;
     float wind_dir_z = 0;
-    if (_sitl) {
+
+    // give 5 seconds to calibrate airspeed sensor at 0 wind speed
+    if (wind_start_delay_micros == 0) {
+        wind_start_delay_micros = now;
+    } else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) {
         // The EKF does not like step inputs so this LPF keeps it happy.
         wind_speed =     _sitl->wind_speed_active     = (0.95f*_sitl->wind_speed_active)     + (0.05f*_sitl->wind_speed);
         wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction);
         wind_dir_z =     _sitl->wind_dir_z_active     = (0.95f*_sitl->wind_dir_z_active)     + (0.05f*_sitl->wind_dir_z);
-    }
+        
+        // pass wind into simulators using different wind types via param SIM_WIND_T*.
+        switch (_sitl->wind_type) {
+        case SITL::SITL::WIND_TYPE_SQRT:
+            if (altitude < _sitl->wind_type_alt) {
+                wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
+            }
+            break;
 
+        case SITL::SITL::WIND_TYPE_COEF:
+            wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
+            break;
+
+        case SITL::SITL::WIND_TYPE_NO_LIMIT:
+        default:
+            break;
+        }
+
+        // never allow negative wind velocity
+        wind_speed = MAX(wind_speed, 0);
+    }
+    
     if (altitude < 0) {
         altitude = 0;
     }
-    if (altitude < 60) {
-        wind_speed *= sqrtf(MAX(altitude / 60, 0));
-    }
 
     input.wind.speed = wind_speed;
     input.wind.direction = wind_direction;
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index dde8e6b0a0..ec9f3b536a 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -202,6 +202,7 @@ private:
     VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
     uint32_t time_delta_wind;
     uint32_t delayed_time_wind;
+    uint32_t wind_start_delay_micros;
 
     // internal SITL model
     SITL::Aircraft *sitl_model;