AP_Baro: added modelling of baro wind effects in SITL

libraries/AP_Baro/AP_Baro_SITL.cpp

@@ -122,6 +122,9 @@ void AP_Baro_SITL::_timer()
     float T = 303.16f * theta - C_TO_KELVIN;  // Assume 30 degrees at sea level - converted to degrees Kelvin
 #endif
 
+    // add in correction for wind effects
+    p += wind_pressure_correction();
+
     _recent_press = p;
     _recent_temp = T;
     _has_sample = true;
@@ -145,4 +148,32 @@ void AP_Baro_SITL::update(void)
     _has_sample = false;
 }
 
+/*
+  return pressure correction for wind based on SIM_BARO_WCF parameters
+ */
+float AP_Baro_SITL::wind_pressure_correction(void)
+{
+    const auto &bp = _sitl->baro[_instance];
+
+    // correct for static pressure position errors
+    const Vector3f &airspeed_vec_bf = _sitl->state.velocity_air_bf;
+
+    float error = 0.0;
+    const float sqx = sq(airspeed_vec_bf.x);
+    const float sqy = sq(airspeed_vec_bf.y);
+
+    if (is_positive(airspeed_vec_bf.x)) {
+        error += bp.wcof_xp * sqx;
+    } else {
+        error += bp.wcof_xn * sqx;
+    }
+    if (is_positive(airspeed_vec_bf.y)) {
+        error += bp.wcof_yp * sqy;
+    } else {
+        error += bp.wcof_yn * sqy;
+    }
+
+    return error * 0.5 * SSL_AIR_DENSITY * AP::baro().get_air_density_ratio();
+}
+
 #endif  // CONFIG_HAL_BOARD

libraries/AP_Baro/AP_Baro_SITL.h

@@ -32,7 +32,10 @@ private:
 
     // adjust for simulated board temperature
     void temperature_adjustment(float &p, float &T);
-    
+
+    // adjust for wind effects
+    float wind_pressure_correction(void);
+
     // is the barometer usable for flight 
     bool healthy(uint8_t instance);