diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp
index 14b51ac438..f85975581b 100644
--- a/libraries/AP_WindVane/AP_WindVane.cpp
+++ b/libraries/AP_WindVane/AP_WindVane.cpp
@@ -13,21 +13,16 @@
along with this program. If not, see .
*/
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-#include
-#endif
+#include "AP_WindVane.h"
-extern const AP_HAL::HAL& hal;
+#include "AP_WindVane_Home.h"
+#include "AP_WindVane_Analog.h"
+#include "AP_WindVane_ModernDevice.h"
+#include "AP_WindVane_Airspeed.h"
+#include "AP_WindVane_RPM.h"
+#include "AP_WindVane_SITL.h"
#define WINDVANE_DEFAULT_PIN 15 // default wind vane sensor analog pin
-#define WINDVANE_CALIBRATION_VOLT_DIFF_MIN 1.0f // calibration routine's min voltage difference required for success
#define WINDSPEED_DEFAULT_SPEED_PIN 14 // default pin for reading speed from ModernDevice rev p wind sensor
#define WINDSPEED_DEFAULT_TEMP_PIN 13 // default pin for reading temperature from ModernDevice rev p wind sensor
#define WINDSPEED_DEFAULT_VOLT_OFFSET 1.346 // default voltage offset between speed and temp pins from ModernDevice rev p wind sensor
@@ -39,7 +34,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
// @Description: Wind Vane type
// @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog
// @User: Standard
- AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _type, 0, AP_PARAM_FLAG_ENABLE),
+ // @RebootRequired: True
+ AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
// @Param: RC_IN_NO
// @DisplayName: Wind vane sensor RC Input Channel
@@ -92,8 +88,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
// @Param: CAL
// @DisplayName: Wind vane calibration start
- // @Description: Start wind vane calibration by setting this to 1
- // @Values: 0:None, 1:Calibrate
+ // @Description: Start wind vane calibration by setting this to 1 or 2
+ // @Values: 0:None, 1:Calibrate direction, 2:Calibrate speed
// @User: Standard
AP_GROUPINFO("CAL", 8, AP_WindVane, _calibration, 0),
@@ -120,6 +116,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
// @Description: Wind speed sensor type
// @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,10:SITL
// @User: Standard
+ // @RebootRequired: True
AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type, 0),
// @Param: SPEED_PIN
@@ -178,60 +175,113 @@ AP_WindVane *AP_WindVane::get_singleton()
// return true if wind vane is enabled
bool AP_WindVane::enabled() const
{
- return (_type != WINDVANE_NONE);
+ return _direction_type != WINDVANE_NONE;
}
// Initialize the Wind Vane object and prepare it for use
void AP_WindVane::init()
{
- // a pin for reading the Wind Vane voltage
- dir_analog_source = hal.analogin->channel(_dir_analog_pin);
+ // don't init twice
+ if (_direction_driver != nullptr || _speed_driver != nullptr ) {
+ return;
+ }
- // pins for ModernDevice rev p wind sensor
- speed_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
- speed_temp_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+ // wind direction
+ switch (_direction_type) {
+ case WindVaneType::WINDVANE_NONE:
+ // WindVane disabled
+ return;
+ case WindVaneType::WINDVANE_HOME_HEADING:
+ case WindVaneType::WINDVANE_PWM_PIN:
+ _direction_driver = new AP_WindVane_Home(*this);
+ break;
+ case WindVaneType::WINDVANE_ANALOG_PIN:
+ _direction_driver = new AP_WindVane_Analog(*this);
+ break;
+ case WindVaneType::WINDVANE_SITL:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ _direction_driver = new AP_WindVane_SITL(*this);
+#endif
+ break;
+ }
- // check that airspeed is enabled if it is selected as sensor type, if not revert to no wind speed sensor
- AP_Airspeed* airspeed = AP_Airspeed::get_singleton();
- if (_speed_sensor_type == Speed_type::WINDSPEED_AIRSPEED && (airspeed == nullptr || !airspeed->enabled())) {
- _speed_sensor_type.set(Speed_type::WINDSPEED_NONE);
+ // wind speed
+ switch (_speed_sensor_type) {
+ case Speed_type::WINDSPEED_NONE:
+ break;
+ case Speed_type::WINDSPEED_AIRSPEED:
+ _speed_driver = new AP_WindVane_Airspeed(*this);
+ break;
+ case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
+ _speed_driver = new AP_WindVane_ModernDevice(*this);
+ break;
+ case Speed_type::WINDSPEED_SITL:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ // single driver does both speed and direction
+ if (_direction_type != WINDVANE_SITL) {
+ _speed_driver = new AP_WindVane_SITL(*this);
+ } else {
+ _speed_driver = _direction_driver;
+ }
+#endif
+ break;
+ case Speed_type::WINDSPEED_RPM:
+ _speed_driver = new AP_WindVane_RPM(*this);
+ break;
}
}
// update wind vane, expected to be called at 20hz
void AP_WindVane::update()
{
+ bool have_speed = _speed_driver != nullptr;
+ bool have_direciton = _direction_driver != nullptr;
+
// exit immediately if not enabled
- if (!enabled()) {
+ if (!enabled() || (!have_speed && !have_direciton)) {
return;
}
- // check for calibration
- calibrate();
+ // calibrate if booted and disarmed
+ if (!hal.util->get_soft_armed()) {
+ if (_calibration == 1 && have_direciton) {
+ _direction_driver->calibrate();
+ } else if (_calibration == 2 && have_speed) {
+ _speed_driver->calibrate();
+ } else if (_calibration != 0) {
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: driver not found");
+ _calibration.set_and_save(0);
+ }
+ } else if (_calibration != 0) {
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: disarm for cal");
+ _calibration.set_and_save(0);
+ }
// read apparent wind speed
- update_apparent_wind_speed();
+ if (have_speed) {
+ _speed_driver->update_speed();
+ }
- // read apparent wind direction (relies on wind speed above)
- update_apparent_wind_direction();
+ // read apparent wind direction
+ if (_speed_apparent >= _dir_speed_cutoff && have_direciton) {
+ // only update if enough wind to move vane
+ _direction_driver->update_direction();
+ }
// calculate true wind speed and direction from apparent wind
- update_true_wind_speed_and_direction();
+ if (have_speed && have_direciton) {
+ update_true_wind_speed_and_direction();
+ } else {
+ // no wind speed sensor, so can't do true wind calcs
+ _direction_absolute = _direction_apparent_ef;
+ _speed_true = 0.0f;
+ return;
+ }
}
-// get the apparent wind direction in radians, 0 = head to wind
-float AP_WindVane::get_apparent_wind_direction_rad() const
-{
- return wrap_PI(_direction_apparent_ef - AP::ahrs().yaw);
-}
-// record home heading for use as wind direction if no sensor is fitted
-void AP_WindVane::record_home_heading()
-{
- _home_heading = AP::ahrs().yaw;
-}
-
-bool AP_WindVane::start_calibration()
+// to start direction calibration from mavlink or other
+bool AP_WindVane::start_direction_calibration()
{
if (enabled() && (_calibration == 0)) {
_calibration = 1;
@@ -240,6 +290,16 @@ bool AP_WindVane::start_calibration()
return false;
}
+// to start speed calibration from mavlink or other
+bool AP_WindVane::start_speed_calibration()
+{
+ if (enabled() && (_calibration == 0)) {
+ _calibration = 2;
+ return true;
+ }
+ return false;
+}
+
// send mavlink wind message
void AP_WindVane::send_wind(mavlink_channel_t chan)
{
@@ -256,204 +316,10 @@ void AP_WindVane::send_wind(mavlink_channel_t chan)
0);
}
-// read an analog port and calculate the wind direction in earth-frame in radians
-// assumes voltage increases as wind vane moves clockwise
-float AP_WindVane::read_analog_direction_ef()
-{
- dir_analog_source->set_pin(_dir_analog_pin);
- _current_analog_voltage = dir_analog_source->voltage_average_ratiometric();
-
- const float voltage_ratio = linear_interpolate(0.0f, 1.0f, _current_analog_voltage, _dir_analog_volt_min, _dir_analog_volt_max);
- const float direction = (voltage_ratio * radians(360 - _dir_analog_deadzone)) + radians(_dir_analog_bearing_offset);
-
- return wrap_PI(direction + AP::ahrs().yaw);
-}
-
-// read rc input of apparent wind direction in earth-frame in radians
-float AP_WindVane::read_PWM_direction_ef()
-{
- RC_Channel *chan = rc().channel(_rc_in_no-1);
- if (chan == nullptr) {
- return 0.0f;
- }
- float direction = chan->norm_input() * radians(45);
-
- return wrap_PI(direction + _home_heading);
-}
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-// read SITL's apparent wind direction in earth-frame in radians
-float AP_WindVane::read_SITL_direction_ef()
-{
- // 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);
-
- // Note than the SITL wind direction is defined as the direction the wind is traveling to
- // This is accounted for in these calculations
-
- // 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;
-
- return atan2f(wind_vector_ef.y, wind_vector_ef.x);
-}
-
-// read the apparent wind speed in m/s from SITL
-float AP_WindVane::read_wind_speed_SITL()
-{
- // 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);
-
- // add vehicle speed to get apparent wind vector
- wind_vector_ef.x += AP::sitl()->state.speedN;
- wind_vector_ef.y += AP::sitl()->state.speedE;
-
- return wind_vector_ef.length();
-}
-#endif
-
-// read wind speed from Modern Device rev p wind sensor
-// https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/
-float AP_WindVane::read_wind_speed_ModernDevice()
-{
- float analog_voltage = 0.0f;
-
- // only read temp pin if defined, sensor will do OK assuming constant temp
- float temp_ambient = 28.0f; // equations were generated at this temp in above data sheet
- if (is_positive(_speed_sensor_temp_pin)) {
- speed_temp_analog_source->set_pin(_speed_sensor_temp_pin);
- analog_voltage = speed_temp_analog_source->voltage_average();
- temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C
- // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero
- temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f);
- }
-
- speed_analog_source->set_pin(_speed_sensor_speed_pin);
- analog_voltage = speed_analog_source->voltage_average();
-
- // apply voltage offset and make sure not negative
- // by default the voltage offset is the number provide by the manufacturer
- analog_voltage = analog_voltage - _speed_sensor_voltage_offset;
- if (is_negative(analog_voltage)) {
- analog_voltage = 0.0f;
- }
-
- // simplified equation from data sheet, converted from mph to m/s
- return 24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f);
-}
-
-// update the apparent wind speed
-void AP_WindVane::update_apparent_wind_speed()
-{
- float apparent_speed_in = 0.0f;
-
- switch (_speed_sensor_type) {
- case WINDSPEED_NONE:
- _speed_apparent = 0.0f;
- break;
- case WINDSPEED_AIRSPEED: {
- const AP_Airspeed* airspeed = AP_Airspeed::get_singleton();
- if (airspeed != nullptr) {
- apparent_speed_in = airspeed->get_airspeed();
- }
- break;
- }
- case WINDVANE_WIND_SENSOR_REV_P:
- apparent_speed_in = read_wind_speed_ModernDevice();
- break;
- case WINDSPEED_SITL:
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
- apparent_speed_in = read_wind_speed_SITL();
-#endif
- break;
- case WINDSPEED_RPM: {
- const AP_RPM* rpm = AP_RPM::get_singleton();
- if (rpm != nullptr) {
- const float temp_speed = rpm->get_rpm(0);
- if (!is_negative(temp_speed)) {
- apparent_speed_in = temp_speed;
- }
- }
- break;
- }
- }
-
- // apply low pass filter if enabled
- if (is_positive(_speed_filt_hz)) {
- _speed_filt.set_cutoff_frequency(_speed_filt_hz);
- _speed_apparent = _speed_filt.apply(apparent_speed_in, 0.02f);
- } else {
- _speed_apparent = apparent_speed_in;
- }
-}
-
-// calculate the apparent wind direction in radians, the wind comes from this direction, 0 = head to wind
-// expected to be called at 20hz after apparent wind speed has been updated
-void AP_WindVane::update_apparent_wind_direction()
-{
- float apparent_angle_ef = 0.0f;
-
- switch (_type) {
- case WindVaneType::WINDVANE_HOME_HEADING:
- // this is a approximation as we are not considering boat speed and wind speed
- // do not filter home heading
- _direction_apparent_ef = _home_heading;
- return;
- case WindVaneType::WINDVANE_PWM_PIN:
- // this is a approximation as we are not considering boat speed and wind speed
- // do not filter pwm input from pilot
- _direction_apparent_ef = read_PWM_direction_ef();
- return;
- case WindVaneType::WINDVANE_ANALOG_PIN:
- apparent_angle_ef = read_analog_direction_ef();
- break;
- case WindVaneType::WINDVANE_SITL:
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
- apparent_angle_ef = read_SITL_direction_ef();
-#endif
- break;
- }
-
- // if not enough wind to move vane do not update the value
- if (_speed_apparent < _dir_speed_cutoff){
- return;
- }
-
- // apply low pass filter if enabled
- if (is_positive(_dir_filt_hz)) {
- _dir_sin_filt.set_cutoff_frequency(_dir_filt_hz);
- _dir_cos_filt.set_cutoff_frequency(_dir_filt_hz);
- // https://en.wikipedia.org/wiki/Mean_of_circular_quantities
- const float filtered_sin = _dir_sin_filt.apply(sinf(apparent_angle_ef), 0.05f);
- const float filtered_cos = _dir_cos_filt.apply(cosf(apparent_angle_ef), 0.05f);
- _direction_apparent_ef = atan2f(filtered_sin, filtered_cos);
- } else {
- _direction_apparent_ef = apparent_angle_ef;
- }
-
- // make sure between -pi and pi
- _direction_apparent_ef = wrap_PI(_direction_apparent_ef);
-}
-
// calculate true wind speed and direction from apparent wind
// https://en.wikipedia.org/wiki/Apparent_wind
void AP_WindVane::update_true_wind_speed_and_direction()
{
- if (_speed_sensor_type == Speed_type::WINDSPEED_NONE) {
- // no wind speed sensor, so can't do true wind calcs
- _direction_absolute = _direction_apparent_ef;
- _speed_true = 0.0f;
- return;
- }
-
// if no vehicle speed, can't do calcs
Vector3f veh_velocity;
if (!AP::ahrs().get_velocity_NED(veh_velocity)) {
@@ -475,61 +341,6 @@ void AP_WindVane::update_true_wind_speed_and_direction()
_speed_true = wind_true_vec.length();
}
-// calibrate windvane
-void AP_WindVane::calibrate()
-{
- // exit immediately if armed or too soon after start
- if (hal.util->get_soft_armed()) {
- return;
- }
-
- // return if not calibrating
- if (_calibration == 0) {
- return;
- }
-
- switch (_type) {
- case WindVaneType::WINDVANE_HOME_HEADING:
- case WindVaneType::WINDVANE_PWM_PIN:
- gcs().send_text(MAV_SEVERITY_INFO, "WindVane: No cal required");
- _calibration.set_and_save(0);
- break;
- case WindVaneType::WINDVANE_ANALOG_PIN:
- // start calibration
- if (_cal_start_ms == 0) {
- _cal_start_ms = AP_HAL::millis();
- _cal_volt_max = _current_analog_voltage;
- _cal_volt_min = _current_analog_voltage;
- gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane");
- }
-
- // record min and max voltage
- _cal_volt_max = MAX(_cal_volt_max, _current_analog_voltage);
- _cal_volt_min = MIN(_cal_volt_min, _current_analog_voltage);
-
- // calibrate for 30 seconds
- if ((AP_HAL::millis() - _cal_start_ms) > 30000) {
- // check for required voltage difference
- const float volt_diff = _cal_volt_max - _cal_volt_min;
- if (volt_diff >= WINDVANE_CALIBRATION_VOLT_DIFF_MIN) {
- // save min and max voltage
- _dir_analog_volt_max.set_and_save(_cal_volt_max);
- _dir_analog_volt_min.set_and_save(_cal_volt_min);
- _calibration.set_and_save(0);
- gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)",
- (double)_cal_volt_min,
- (double)_cal_volt_max);
- } else {
- gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)",
- (double)volt_diff,
- (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN);
- }
- _cal_start_ms = 0;
- }
- break;
- }
-}
-
AP_WindVane *AP_WindVane::_singleton = nullptr;
namespace AP {
diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h
index cbeb55595b..ec3e82fa8b 100644
--- a/libraries/AP_WindVane/AP_WindVane.h
+++ b/libraries/AP_WindVane/AP_WindVane.h
@@ -14,24 +14,22 @@
*/
#pragma once
-#include
#include
-#include
-#include
-#include
+#include
+
+class AP_WindVane_Backend;
class AP_WindVane
{
public:
-
- enum WindVaneType {
- WINDVANE_NONE = 0,
- WINDVANE_HOME_HEADING = 1,
- WINDVANE_PWM_PIN = 2,
- WINDVANE_ANALOG_PIN = 3,
- WINDVANE_SITL = 10
- };
+ friend class AP_WindVane_Backend;
+ friend class AP_WindVane_Home;
+ friend class AP_WindVane_Analog;
+ friend class AP_WindVane_SITL;
+ friend class AP_WindVane_ModernDevice;
+ friend class AP_WindVane_Airspeed;
+ friend class AP_WindVane_RPM;
AP_WindVane();
@@ -51,7 +49,9 @@ public:
void update();
// get the apparent wind direction in body-frame in radians, 0 = head to wind
- float get_apparent_wind_direction_rad() const;
+ float get_apparent_wind_direction_rad() const {
+ return wrap_PI(_direction_apparent_ef - AP::ahrs().yaw);
+ }
// get the absolute wind direction in radians, 0 = wind coming from north
float get_absolute_wind_direction_rad() const { return _direction_absolute; }
@@ -62,11 +62,12 @@ public:
// Return true wind speed
float get_true_wind_speed() const { return _speed_true; }
- // record home heading
- void record_home_heading();
+ // record home heading for use as wind direction if no sensor is fitted
+ void record_home_heading() { _home_heading = AP::ahrs().yaw; }
// start calibration routine
- bool start_calibration();
+ bool start_direction_calibration();
+ bool start_speed_calibration();
// send mavlink wind message
void send_wind(mavlink_channel_t chan);
@@ -76,37 +77,8 @@ public:
private:
- // read an analog port and calculate the wind direction in earth-frame in radians
- float read_analog_direction_ef();
-
- // read rc input of apparent wind direction in earth-frame in radians
- float read_PWM_direction_ef();
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
- // read SITL's apparent wind direction in earth-frame in radians
- float read_SITL_direction_ef();
-
- // read the apparent wind speed in m/s from SITL
- float read_wind_speed_SITL();
-#endif
-
- // read wind speed from ModernDevice wind speed sensor rev p
- float read_wind_speed_ModernDevice();
-
- // update wind speed sensor
- void update_apparent_wind_speed();
-
- // update apparent wind direction
- void update_apparent_wind_direction();
-
- // calculate true wind speed and direction from apparent wind
- void update_true_wind_speed_and_direction();
-
- // calibrate
- void calibrate();
-
// parameters
- AP_Int8 _type; // type of windvane being used
+ AP_Int8 _direction_type; // type of windvane being used
AP_Int8 _rc_in_no; // RC input channel to use
AP_Int8 _dir_analog_pin; // analog pin connected to wind vane direction sensor
AP_Float _dir_analog_volt_min; // minimum voltage read by windvane
@@ -122,22 +94,36 @@ private:
AP_Float _speed_sensor_voltage_offset; // analog speed zero wind voltage offset
AP_Float _speed_filt_hz; // speed sensor low pass filter frequency
- static AP_WindVane *_singleton;
+ AP_WindVane_Backend *_direction_driver;
+ AP_WindVane_Backend *_speed_driver;
+
+ // update wind speed sensor
+ void update_apparent_wind_speed();
+
+ // update apparent wind direction
+ void update_apparent_wind_direction();
+
+ // calculate true wind speed and direction from apparent wind
+ void update_true_wind_speed_and_direction();
// wind direction variables
- float _home_heading; // heading in radians recorded when vehicle was armed
float _direction_apparent_ef; // wind's apparent direction in radians (0 = ahead of vehicle)
float _direction_absolute; // wind's absolute direction in radians (0 = North)
- float _current_analog_voltage; // wind direction's latest analog voltage reading
// wind speed variables
float _speed_apparent; // wind's apparent speed in m/s
float _speed_true; // wind's true estimated speed in m/s
- // calibration variables
- uint32_t _cal_start_ms = 0; // calibration start time in milliseconds after boot
- float _cal_volt_max; // maximum observed voltage during calibration
- float _cal_volt_min; // minimum observed voltage during calibration
+ // heading in radians recorded when vehicle was armed
+ float _home_heading;
+
+ enum WindVaneType {
+ WINDVANE_NONE = 0,
+ WINDVANE_HOME_HEADING = 1,
+ WINDVANE_PWM_PIN = 2,
+ WINDVANE_ANALOG_PIN = 3,
+ WINDVANE_SITL = 10
+ };
enum Speed_type {
WINDSPEED_NONE = 0,
@@ -147,15 +133,7 @@ private:
WINDSPEED_SITL = 10
};
- // pin for reading analog voltage
- AP_HAL::AnalogSource *dir_analog_source;
- AP_HAL::AnalogSource *speed_analog_source;
- AP_HAL::AnalogSource *speed_temp_analog_source;
-
- // low pass filters of direction and speed
- LowPassFilterFloat _dir_sin_filt = LowPassFilterFloat(2.0f);
- LowPassFilterFloat _dir_cos_filt = LowPassFilterFloat(2.0f);
- LowPassFilterFloat _speed_filt = LowPassFilterFloat(2.0f);
+ static AP_WindVane *_singleton;
};
namespace AP {
diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp
new file mode 100644
index 0000000000..ee175900c0
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp
@@ -0,0 +1,30 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_Airspeed.h"
+
+// constructor
+AP_WindVane_Airspeed::AP_WindVane_Airspeed(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+}
+
+void AP_WindVane_Airspeed::update_speed()
+{
+ const AP_Airspeed* airspeed = AP_Airspeed::get_singleton();
+ if (airspeed != nullptr) {
+ speed_update_frontend(airspeed->get_airspeed());
+ }
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.h b/libraries/AP_WindVane/AP_WindVane_Airspeed.h
new file mode 100644
index 0000000000..0f0bc748fe
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.h
@@ -0,0 +1,29 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+#include
+
+class AP_WindVane_Airspeed : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_Airspeed(AP_WindVane &frontend);
+
+ // update state
+ void update_speed() override;
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.cpp b/libraries/AP_WindVane/AP_WindVane_Analog.cpp
new file mode 100644
index 0000000000..36fc956108
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Analog.cpp
@@ -0,0 +1,72 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_Analog.h"
+
+#define WINDVANE_CALIBRATION_VOLT_DIFF_MIN 1.0f // calibration routine's min voltage difference required for success
+
+// constructor
+AP_WindVane_Analog::AP_WindVane_Analog(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+ _dir_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+}
+
+void AP_WindVane_Analog::update_direction()
+{
+ _dir_analog_source->set_pin(_frontend._dir_analog_pin);
+ _current_analog_voltage = _dir_analog_source->voltage_average_ratiometric();
+
+ const float voltage_ratio = linear_interpolate(0.0f, 1.0f, _current_analog_voltage, _frontend._dir_analog_volt_min, _frontend._dir_analog_volt_max);
+ const float direction = (voltage_ratio * radians(360 - _frontend._dir_analog_deadzone)) + radians(_frontend._dir_analog_bearing_offset);
+
+ direction_update_frontend(wrap_PI(direction + AP::ahrs().yaw));
+}
+
+void AP_WindVane_Analog::calibrate()
+{
+
+ // start calibration
+ if (_cal_start_ms == 0) {
+ _cal_start_ms = AP_HAL::millis();
+ _cal_volt_max = _current_analog_voltage;
+ _cal_volt_min = _current_analog_voltage;
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane");
+ }
+
+ // record min and max voltage
+ _cal_volt_max = MAX(_cal_volt_max, _current_analog_voltage);
+ _cal_volt_min = MIN(_cal_volt_min, _current_analog_voltage);
+
+ // calibrate for 30 seconds
+ if ((AP_HAL::millis() - _cal_start_ms) > 30000) {
+ // check for required voltage difference
+ const float volt_diff = _cal_volt_max - _cal_volt_min;
+ if (volt_diff >= WINDVANE_CALIBRATION_VOLT_DIFF_MIN) {
+ // save min and max voltage
+ _frontend._dir_analog_volt_max.set_and_save(_cal_volt_max);
+ _frontend._dir_analog_volt_min.set_and_save(_cal_volt_min);
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)",
+ (double)_cal_volt_min,
+ (double)_cal_volt_max);
+ } else {
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)",
+ (double)volt_diff,
+ (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN);
+ }
+ _frontend._calibration.set_and_save(0);
+ _cal_start_ms = 0;
+ }
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.h b/libraries/AP_WindVane/AP_WindVane_Analog.h
new file mode 100644
index 0000000000..eed943835c
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Analog.h
@@ -0,0 +1,42 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+class AP_WindVane_Analog : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_Analog(AP_WindVane &frontend);
+
+ // update state
+ void update_direction() override;
+ void calibrate() override;
+
+private:
+ // pin for reading analog voltage
+ AP_HAL::AnalogSource *_dir_analog_source;
+
+ float _current_analog_voltage;
+ uint32_t _cal_start_ms = 0;
+ float _cal_volt_min;
+ float _cal_volt_max;
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp
new file mode 100644
index 0000000000..8452d4427a
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Backend.cpp
@@ -0,0 +1,62 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane.h"
+#include "AP_WindVane_Backend.h"
+
+// base class constructor.
+AP_WindVane_Backend::AP_WindVane_Backend(AP_WindVane &frontend) :
+ _frontend(frontend)
+{
+}
+
+// update speed to frontend
+void AP_WindVane_Backend::speed_update_frontend(float apparent_speed_in)
+{
+ // apply low pass filter if enabled
+ if (is_positive(_frontend._speed_filt_hz)) {
+ _speed_filt.set_cutoff_frequency(_frontend._speed_filt_hz);
+ _frontend._speed_apparent = _speed_filt.apply(apparent_speed_in, 0.02f);
+ } else {
+ _frontend._speed_apparent = apparent_speed_in;
+ }
+}
+
+// update direction to frontend
+void AP_WindVane_Backend::direction_update_frontend(float apparent_angle_ef)
+{
+ // apply low pass filter if enabled
+ if (is_positive(_frontend._dir_filt_hz)) {
+ _dir_sin_filt.set_cutoff_frequency(_frontend._dir_filt_hz);
+ _dir_cos_filt.set_cutoff_frequency(_frontend._dir_filt_hz);
+ // https://en.wikipedia.org/wiki/Mean_of_circular_quantities
+ const float filtered_sin = _dir_sin_filt.apply(sinf(apparent_angle_ef), 0.05f);
+ const float filtered_cos = _dir_cos_filt.apply(cosf(apparent_angle_ef), 0.05f);
+ _frontend._direction_apparent_ef = atan2f(filtered_sin, filtered_cos);
+ } else {
+ _frontend._direction_apparent_ef = apparent_angle_ef;
+ }
+
+ // make sure between -pi and pi
+ _frontend._direction_apparent_ef = wrap_PI(_frontend._direction_apparent_ef);
+}
+
+// calibrate WindVane
+void AP_WindVane_Backend::calibrate()
+{
+ gcs().send_text(MAV_SEVERITY_INFO, "WindVane: No cal required");
+ _frontend._calibration.set_and_save(0);
+ return;
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h
new file mode 100644
index 0000000000..56e1d73138
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Backend.h
@@ -0,0 +1,49 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane.h"
+
+#include
+#include
+
+class AP_WindVane_Backend
+{
+public:
+ // constructor. This incorporates initialization as well.
+ AP_WindVane_Backend(AP_WindVane &frontend);
+
+ // we declare a virtual destructor so that WindVane drivers can
+ // override with a custom destructor if need be
+ virtual ~AP_WindVane_Backend() {}
+
+ // update the state structure
+ virtual void update_speed() {};
+ virtual void update_direction() {};
+ virtual void calibrate();
+
+protected:
+ // update frontend
+ void speed_update_frontend(float apparent_speed_in);
+ void direction_update_frontend(float apparent_angle_ef);
+
+ AP_WindVane &_frontend;
+
+private:
+ // low pass filters of direction and speed
+ LowPassFilterFloat _dir_sin_filt = LowPassFilterFloat(2.0f);
+ LowPassFilterFloat _dir_cos_filt = LowPassFilterFloat(2.0f);
+ LowPassFilterFloat _speed_filt = LowPassFilterFloat(2.0f);
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_Home.cpp b/libraries/AP_WindVane/AP_WindVane_Home.cpp
new file mode 100644
index 0000000000..d73b16bbd7
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Home.cpp
@@ -0,0 +1,36 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_Home.h"
+
+// constructor
+AP_WindVane_Home::AP_WindVane_Home(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+}
+
+void AP_WindVane_Home::update_direction()
+{
+ float direction_apparent_ef = _frontend._home_heading;
+
+ if (_frontend._direction_type == _frontend.WINDVANE_PWM_PIN && _frontend._rc_in_no != 0) {
+ RC_Channel *chan = rc().channel(_frontend._rc_in_no-1);
+ if (chan != nullptr) {
+ direction_apparent_ef = wrap_PI(direction_apparent_ef + chan->norm_input() * radians(45));
+ }
+ }
+
+ direction_update_frontend(direction_apparent_ef);
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_Home.h b/libraries/AP_WindVane/AP_WindVane_Home.h
new file mode 100644
index 0000000000..7dc1271194
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_Home.h
@@ -0,0 +1,28 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+#include
+
+class AP_WindVane_Home : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_Home(AP_WindVane &frontend);
+
+ // update state
+ void update_direction() override;
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp
new file mode 100644
index 0000000000..b5bcc70ca8
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp
@@ -0,0 +1,54 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_ModernDevice.h"
+// read wind speed from Modern Device rev p wind sensor
+// https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/
+
+// constructor
+AP_WindVane_ModernDevice::AP_WindVane_ModernDevice(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+ _speed_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+ _temp_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+}
+
+void AP_WindVane_ModernDevice::update_speed()
+{
+ float analog_voltage = 0.0f;
+
+ // only read temp pin if defined, sensor will do OK assuming constant temp
+ float temp_ambient = 28.0f; // equations were generated at this temp in above data sheet
+ if (is_positive(_frontend._speed_sensor_temp_pin.get())) {
+ _temp_analog_source->set_pin(_frontend._speed_sensor_temp_pin.get());
+ analog_voltage = _temp_analog_source->voltage_average();
+ temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C
+ // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero
+ temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f);
+ }
+
+ _speed_analog_source->set_pin(_frontend._speed_sensor_speed_pin.get());
+ _current_analog_voltage = _speed_analog_source->voltage_average();
+
+ // apply voltage offset and make sure not negative
+ // by default the voltage offset is the number provide by the manufacturer
+ analog_voltage = _current_analog_voltage - _frontend._speed_sensor_voltage_offset;
+ if (is_negative(analog_voltage)) {
+ analog_voltage = 0.0f;
+ }
+
+ // simplified equation from data sheet, converted from mph to m/s
+ speed_update_frontend(24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f));
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h
new file mode 100644
index 0000000000..66b2e8c5c2
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h
@@ -0,0 +1,40 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+class AP_WindVane_ModernDevice : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_ModernDevice(AP_WindVane &frontend);
+
+ // update state
+ void update_speed() override;
+ void calibrate() override;
+
+private:
+ float _current_analog_voltage;
+
+ // pin for reading analog voltage
+ AP_HAL::AnalogSource *_speed_analog_source;
+ AP_HAL::AnalogSource *_temp_analog_source;
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.cpp b/libraries/AP_WindVane/AP_WindVane_RPM.cpp
new file mode 100644
index 0000000000..311d645dee
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_RPM.cpp
@@ -0,0 +1,33 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_RPM.h"
+
+// constructor
+AP_WindVane_RPM::AP_WindVane_RPM(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+}
+
+void AP_WindVane_RPM::update_speed()
+{
+ const AP_RPM* rpm = AP_RPM::get_singleton();
+ if (rpm != nullptr) {
+ const float temp_speed = rpm->get_rpm(0);
+ if (!is_negative(temp_speed)) {
+ speed_update_frontend(temp_speed);
+ }
+ }
+}
diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.h b/libraries/AP_WindVane/AP_WindVane_RPM.h
new file mode 100644
index 0000000000..aa79c23680
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_RPM.h
@@ -0,0 +1,29 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+#include
+
+class AP_WindVane_RPM : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_RPM(AP_WindVane &frontend);
+
+ // update state
+ void update_speed() override;
+};
diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp
new file mode 100644
index 0000000000..1031a8b32b
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_SITL.cpp
@@ -0,0 +1,60 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WindVane_SITL.h"
+
+// constructor
+AP_WindVane_SITL::AP_WindVane_SITL(AP_WindVane &frontend) :
+ AP_WindVane_Backend(frontend)
+{
+}
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+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);
+
+ // Note than the SITL wind direction is defined as the direction the wind is traveling to
+ // This is accounted for in these calculations
+
+ // 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));
+}
+
+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);
+
+ // 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;
+
+ speed_update_frontend(wind_vector_ef.length());
+}
+#endif
diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.h b/libraries/AP_WindVane/AP_WindVane_SITL.h
new file mode 100644
index 0000000000..86d07b857d
--- /dev/null
+++ b/libraries/AP_WindVane/AP_WindVane_SITL.h
@@ -0,0 +1,30 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+class AP_WindVane_SITL : public AP_WindVane_Backend
+{
+public:
+ // constructor
+ AP_WindVane_SITL(AP_WindVane &frontend);
+
+ // update state
+ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ void update_direction() override;
+ void update_speed() override;
+ #endif
+};