AP_WindVane: add apparent wind SITL types

libraries/AP_WindVane/AP_WindVane.cpp

@@ -28,7 +28,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Wind Vane Type
     // @Description: Wind Vane type
-    // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL
+    // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL true,11:SITL apparent
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
@@ -110,7 +110,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     // @Param: SPEED_TYPE
     // @DisplayName: Wind speed sensor Type
     // @Description: Wind speed sensor type
-    // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL
+    // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL true,11:SITL apparent
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type,  0),
@@ -200,7 +200,8 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
         case WindVaneType::WINDVANE_ANALOG_PIN:
             _direction_driver = new AP_WindVane_Analog(*this);
             break;
-        case WindVaneType::WINDVANE_SITL:
+        case WindVaneType::WINDVANE_SITL_TRUE:
+        case WindVaneType::WINDVANE_SITL_APPARENT:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
             _direction_driver = new AP_WindVane_SITL(*this);
 #endif
@@ -221,10 +222,11 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
         case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
             _speed_driver = new AP_WindVane_ModernDevice(*this);
             break;
-        case Speed_type::WINDSPEED_SITL:
+        case Speed_type::WINDSPEED_SITL_TRUE:
+        case Speed_type::WINDSPEED_SITL_APPARENT:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
             // single driver does both speed and direction
-            if (_direction_type != WindVaneType::WINDVANE_SITL) {
+            if (_direction_type != _speed_sensor_type) {
                 _speed_driver = new AP_WindVane_SITL(*this);
             } else {
                 _speed_driver = _direction_driver;

libraries/AP_WindVane/AP_WindVane.h

@@ -145,7 +145,8 @@ private:
         WINDVANE_PWM_PIN        = 2,
         WINDVANE_ANALOG_PIN     = 3,
         WINDVANE_NMEA           = 4,
-        WINDVANE_SITL           = 10
+        WINDVANE_SITL_TRUE      = 10,
+        WINDVANE_SITL_APPARENT  = 11,
     };
 
     enum Speed_type {
@@ -154,7 +155,8 @@ private:
         WINDVANE_WIND_SENSOR_REV_P   = 2,
         WINDSPEED_RPM                = 3,
         WINDSPEED_NMEA               = 4,
-        WINDSPEED_SITL               = 10
+        WINDSPEED_SITL_TRUE          = 10,
+        WINDSPEED_SITL_APPARENT      = 11,
     };
 
     static AP_WindVane *_singleton;

libraries/AP_WindVane/AP_WindVane_SITL.cpp

@@ -25,36 +25,53 @@ AP_WindVane_SITL::AP_WindVane_SITL(AP_WindVane &frontend) :
 
 void AP_WindVane_SITL::update_direction()
 {
-    // temporarily store true speed and direction for easy access
-    const float wind_speed = AP::sitl()->wind_speed_active;
-    const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
+    if (_frontend._direction_type == _frontend.WindVaneType::WINDVANE_SITL_TRUE) {
+        // read in the true wind direction and calculate the apparent
 
-    // Note than the SITL wind direction is defined as the direction the wind is traveling to
-    // This is accounted for in these calculations
+        // temporarily store true speed and direction for easy access
+        const float wind_speed = AP::sitl()->wind_speed_active;
+        const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
 
-    // convert true wind speed and direction into a 2D vector
-    Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
+        // Note than the SITL wind direction is defined as the direction the wind is traveling to
+        // This is accounted for in these calculations
 
-    // add vehicle speed to get apparent wind vector
-    wind_vector_ef.x += AP::sitl()->state.speedN;
-    wind_vector_ef.y += AP::sitl()->state.speedE;
+        // convert true wind speed and direction into a 2D vector
+        Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
+
+        // add vehicle speed to get apparent wind vector
+        wind_vector_ef.x += AP::sitl()->state.speedN;
+        wind_vector_ef.y += AP::sitl()->state.speedE;
+
+        direction_update_frontend(atan2f(wind_vector_ef.y, wind_vector_ef.x));
+
+    } else { // WINDVANE_SITL_APARRENT
+        // directly read the apparent wind from as set by physics backend
+        direction_update_frontend(AP::sitl()->get_apparent_wind_dir());
+    }
 
-    direction_update_frontend(atan2f(wind_vector_ef.y, wind_vector_ef.x));
 }
 
 void AP_WindVane_SITL::update_speed()
 {
-    // temporarily store true speed and direction for easy access
-    const float wind_speed = AP::sitl()->wind_speed_active;
-    const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
+    if (_frontend._speed_sensor_type == _frontend.Speed_type::WINDSPEED_SITL_TRUE) {
+        // read in the true wind direction and calculate the apparent
+
+        // temporarily store true speed and direction for easy access
+        const float wind_speed = AP::sitl()->wind_speed_active;
+        const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
+
+        // convert true wind speed and direction into a 2D vector
+        Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
 
-    // convert true wind speed and direction into a 2D vector
-    Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
+        // add vehicle speed to get apparent wind vector
+        wind_vector_ef.x += AP::sitl()->state.speedN;
+        wind_vector_ef.y += AP::sitl()->state.speedE;
 
-    // add vehicle speed to get apparent wind vector
-    wind_vector_ef.x += AP::sitl()->state.speedN;
-    wind_vector_ef.y += AP::sitl()->state.speedE;
+        speed_update_frontend(wind_vector_ef.length());
 
-    speed_update_frontend(wind_vector_ef.length());
+    } else { // WINDSPEED_SITL_APARRENT
+        // directly read the apparent wind from as set by physics backend
+        speed_update_frontend(AP::sitl()->get_apparent_wind_spd());
+    }
 }
 #endif