adding turbulance to SITL

libraries/SITL/SIM_Aircraft.cpp

@@ -510,6 +510,27 @@ void Aircraft::update_wind(const struct sitl_input &input)
 {
     // wind vector in earth frame
     wind_ef = Vector3f(cosf(radians(input.wind.direction)), sinf(radians(input.wind.direction)), 0) * input.wind.speed;
+    
+    //float wind_turb=input.wind.turbulence*input.wind.speed; //creating a linear dependency between wind and turbulence
+    
+    float wind_turb=input.wind.turbulence;
+    wind_turb=wind_turb*10.0; //scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
+    float iir_coef=0.98; //filtering high frequencies from turbulence
+    
+    if (wind_turb>0 && !on_ground(position)) {
+
+        turbulence_azimuth=turbulence_azimuth+2*rand();
+
+        turbulence_horizontal_speed=
+            turbulence_horizontal_speed*iir_coef+wind_turb*rand_normal(0,1)*(1-iir_coef);
+
+        turbulence_vertical_speed=turbulence_vertical_speed*iir_coef+wind_turb*rand_normal(0,1)*(1-iir_coef);
+
+        wind_ef += Vector3f(
+            cosf(radians(turbulence_azimuth))*turbulence_horizontal_speed,
+            sinf(radians(turbulence_azimuth))*turbulence_horizontal_speed,
+            turbulence_vertical_speed);
+    }     
 }
 
 /*

libraries/SITL/SIM_Aircraft.h

@@ -134,6 +134,11 @@ protected:
     uint8_t rcin_chan_count = 0;
     float rcin[8];
 
+    //Wind Turbulence simulated Data
+    float turbulence_azimuth = 0;
+    float turbulence_horizontal_speed = 0; // m/s
+    float turbulence_vertical_speed =0; // m/s
+
     Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame
 
     uint64_t time_now_us;