AP_Windvane: caculate vehicles current tack

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

libraries/AP_WindVane/AP_WindVane.h

@@ -18,6 +18,12 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_SerialManager/AP_SerialManager.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.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;
 

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

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);
 };