diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index f9a1c20ac5..d2575f4f71 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -23,11 +23,6 @@ #include "AP_WindVane_SITL.h" #include "AP_WindVane_NMEA.h" -#define WINDVANE_DEFAULT_PIN 15 // default wind vane sensor analog pin -#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 - const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Param: TYPE diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index 97cadb2df5..4dc5b2b0cf 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -18,6 +18,12 @@ #include #include +#define WINDVANE_DEFAULT_PIN 15 // default wind vane sensor analog pin +#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.346f // default voltage offset between speed and temp pins from ModernDevice rev p wind sensor +#define TACK_FILT_CUTOFF 0.1f // cutoff frequency in Hz used in low pass filter to determine the current tack + class AP_WindVane_Backend; class AP_WindVane @@ -66,6 +72,15 @@ public: // Return true wind speed float get_true_wind_speed() const { return _speed_true; } + // enum defining current tack + enum Sailboat_Tack { + TACK_PORT, + TACK_STARBOARD + }; + + // return the current tack + Sailboat_Tack get_current_tack() const { return _current_tack; } + // record home heading for use as wind direction if no sensor is fitted void record_home_heading() { _home_heading = AP::ahrs().yaw; } @@ -111,13 +126,16 @@ private: void update_true_wind_speed_and_direction(); // wind direction variables - float _direction_apparent_ef; // wind's apparent direction in radians (0 = ahead of vehicle) + float _direction_apparent_ef; // wind's apparent direction in radians (0 = ahead of vehicle) in earth frame float _direction_true; // wind's true direction in radians (0 = North) // wind speed variables float _speed_apparent; // wind's apparent speed in m/s float _speed_true; // wind's true estimated speed in m/s + // current tack + Sailboat_Tack _current_tack; + // heading in radians recorded when vehicle was armed float _home_heading; diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp index 8452d4427a..b8e3d2f69c 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Backend.cpp @@ -44,13 +44,14 @@ void AP_WindVane_Backend::direction_update_frontend(float apparent_angle_ef) // 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); + _frontend._direction_apparent_ef = wrap_PI(atan2f(filtered_sin, filtered_cos)); } else { - _frontend._direction_apparent_ef = apparent_angle_ef; + _frontend._direction_apparent_ef = wrap_PI(apparent_angle_ef); } - // make sure between -pi and pi - _frontend._direction_apparent_ef = wrap_PI(_frontend._direction_apparent_ef); + // apply low pass filter for current tack, this is at a hard coded cutoff frequency + const float tack_direction_apparent = _tack_filt.apply(wrap_PI(apparent_angle_ef - AP::ahrs().yaw), 0.05f); + _frontend._current_tack = is_negative(tack_direction_apparent) ? _frontend.Sailboat_Tack::TACK_PORT : _frontend.Sailboat_Tack::TACK_STARBOARD; } // calibrate WindVane diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h index 2fdb3a4656..f8681a2733 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.h +++ b/libraries/AP_WindVane/AP_WindVane_Backend.h @@ -49,4 +49,5 @@ private: LowPassFilterFloat _dir_sin_filt = LowPassFilterFloat(2.0f); LowPassFilterFloat _dir_cos_filt = LowPassFilterFloat(2.0f); LowPassFilterFloat _speed_filt = LowPassFilterFloat(2.0f); + LowPassFilterFloat _tack_filt = LowPassFilterFloat(TACK_FILT_CUTOFF); };