diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index 4f65d154c8..e6d4f0ff2e 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/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; diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index 4dc5b2b0cf..2067c38fbc 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/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; diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp index 1031a8b32b..41af63f8a6 100644 --- a/libraries/AP_WindVane/AP_WindVane_SITL.cpp +++ b/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