AP_WindVane: rework filtering and add logging

libraries/AP_WindVane/AP_WindVane.cpp

@@ -23,6 +23,8 @@
 #include "AP_WindVane_SITL.h"
 #include "AP_WindVane_NMEA.h"
 
+#include <AP_Logger/AP_Logger.h>
+
 const AP_Param::GroupInfo AP_WindVane::var_info[] = {
 
     // @Param: TYPE
@@ -76,8 +78,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     AP_GROUPINFO("DIR_OFS", 6, AP_WindVane, _dir_analog_bearing_offset, 0.0f),
 
     // @Param: DIR_FILT
-    // @DisplayName: Wind vane direction low pass filter frequency
-    // @Description: Wind vane direction low pass filter frequency, a value of -1 disables filter
+    // @DisplayName: apparent Wind vane direction low pass filter frequency
+    // @Description: apparent Wind vane direction low pass filter frequency, a value of -1 disables filter
     // @Units: Hz
     // @User: Standard
     AP_GROUPINFO("DIR_FILT", 7, AP_WindVane, _dir_filt_hz, 0.5f),
@@ -139,12 +141,19 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     AP_GROUPINFO("SPEED_OFS", 14, AP_WindVane, _speed_sensor_voltage_offset, WINDSPEED_DEFAULT_VOLT_OFFSET),
 
     // @Param: SPEED_FILT
-    // @DisplayName: Wind speed low pass filter frequency
-    // @Description: Wind speed low pass filter frequency, a value of -1 disables filter
+    // @DisplayName: apparent wind speed low pass filter frequency
+    // @Description: apparent Wind speed low pass filter frequency, a value of -1 disables filter
     // @Units: Hz
     // @User: Standard
     AP_GROUPINFO("SPEED_FILT", 15, AP_WindVane, _speed_filt_hz, 0.5f),
 
+    // @Param: TRUE_FILT
+    // @DisplayName: True speed and direction low pass filter frequency
+    // @Description: True speed and direction low pass filter frequency, a value of -1 disables filter
+    // @Units: Hz
+    // @User: Standard
+    AP_GROUPINFO("TRUE_FILT", 16, AP_WindVane, _true_filt_hz, 0.05f),
+
     AP_GROUPEND
 };
 
@@ -251,17 +260,17 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
 // 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;
+    const bool have_speed = _speed_driver != nullptr;
+    const bool have_direction = _direction_driver != nullptr;
 
     // exit immediately if not enabled
-    if (!enabled() || (!have_speed && !have_direciton)) {
+    if (!enabled() || (!have_speed && !have_direction)) {
         return;
     }
 
     // calibrate if booted and disarmed
     if (!hal.util->get_soft_armed()) {
-        if (_calibration == 1 && have_direciton) {
+        if (_calibration == 1 && have_direction) {
             _direction_driver->calibrate();
         } else if (_calibration == 2 && have_speed) {
             _speed_driver->calibrate();
@@ -280,20 +289,86 @@ void AP_WindVane::update()
     }
 
     // read apparent wind direction
-    if (_speed_apparent >= _dir_speed_cutoff && have_direciton) {
-        // only update if enough wind to move vane
+    if (have_direction) {
         _direction_driver->update_direction();
     }
 
-    // calculate true wind speed and direction from apparent wind
-    if (have_speed && have_direciton) {
-        update_true_wind_speed_and_direction();
+    if (have_speed && have_direction) {
+        if (_speed_apparent >= _dir_speed_cutoff) {
+            // calculate true wind speed and direction from apparent wind
+            update_true_wind_speed_and_direction();
+        } else {
+            // assume true wind has not changed, calculate the apparent wind direction and speed
+            update_apparent_wind_dir_from_true();
+        }
+    }
+
+    /*
+        Apply filters
+        https://en.wikipedia.org/wiki/Mean_of_circular_quantities
+    */
+    float filtered_sin;
+    float filtered_cos;
+
+    // apparent speed
+    if (is_positive(_speed_filt_hz)) {
+        _speed_apparent_filt.set_cutoff_frequency(_speed_filt_hz);
+        _speed_apparent = _speed_apparent_filt.apply(_speed_apparent_raw,0.05f);
     } else {
-        // no wind speed sensor, so can't do true wind calcs
-        _direction_true = _direction_apparent_ef;
-        _speed_true = 0.0f;
-        return;
+        _speed_apparent = _speed_apparent_raw;
+    }
+
+    // apparent direction
+    if (is_positive(_dir_filt_hz)) {
+        _direction_apparent_sin_filt.set_cutoff_frequency(_dir_filt_hz);
+        _direction_apparent_cos_filt.set_cutoff_frequency(_dir_filt_hz);
+        filtered_sin = _direction_apparent_sin_filt.apply(sinf(_direction_apparent_raw),0.05f);
+        filtered_cos = _direction_apparent_cos_filt.apply(cosf(_direction_apparent_raw),0.05f);
+        _direction_apparent = atan2f(filtered_sin, filtered_cos);
+    } else {
+        _direction_apparent = _direction_apparent_raw;
+    }
+
+    // apparent direction for tack threshold, hard coded freq
+    filtered_sin = _tack_sin_filt.apply(sinf(_direction_apparent_raw),0.05f);
+    filtered_cos = _tack_cos_filt.apply(cosf(_direction_apparent_raw),0.05f);
+    _direction_tack = atan2f(filtered_sin, filtered_cos);
+    _current_tack = is_negative(_direction_tack) ? Sailboat_Tack::TACK_PORT : Sailboat_Tack::TACK_STARBOARD;
+
+    // true wind direction and speed, both at the same freq
+    if (is_positive(_true_filt_hz)) {
+        _speed_true_filt.set_cutoff_frequency(_true_filt_hz);
+        _direction_true_sin_filt.set_cutoff_frequency(_true_filt_hz);
+        _direction_true_cos_filt.set_cutoff_frequency(_true_filt_hz);
+
+        _speed_true = _speed_true_filt.apply(_speed_true_raw,0.05f);
+        filtered_sin = _direction_true_sin_filt.apply(sinf(_direction_true_raw),0.05f);
+        filtered_cos = _direction_true_cos_filt.apply(cosf(_direction_true_raw),0.05f);
+        _direction_true = atan2f(filtered_sin, filtered_cos);
+    } else {
+        _speed_true = _speed_true_raw;
+        _direction_true = _direction_true_raw;
     }
+
+// @LoggerMessage: WIND
+// @Description: Windvane readings
+// @Field: TimeUS: Time since system startup
+// @Field: DrRaw: raw apparent wind direction direct from sensor, in body-frame
+// @Field: DrApp: Apparent wind direction, in body-frame
+// @Field: DrTru: True wind direction
+// @Field: SpdRaw: raw wind speed direct from sensor
+// @Field: SpdApp: Apparent wind Speed
+// @Field: SpdTru: True wind speed
+    AP::logger().Write("WIND", "TimeUS,DrRaw,DrApp,DrTru,SpdRaw,SpdApp,SpdTru",
+                        "sddhnnn", "F000000", "Qffffff",
+                        AP_HAL::micros64(),
+                        degrees(_direction_apparent_raw),
+                        degrees(_direction_apparent),
+                        degrees(_direction_true),
+                        _speed_apparent_raw,
+                        _speed_apparent,
+                        _speed_true);
+
 }
 
 
@@ -347,21 +422,42 @@ void AP_WindVane::update_true_wind_speed_and_direction()
     Vector3f veh_velocity;
     if (!AP::ahrs().get_velocity_NED(veh_velocity)) {
         // if no vehicle speed use apparent speed and direction directly
-        _direction_true = _direction_apparent_ef;
-        _speed_true = _speed_apparent;
+        _direction_true_raw = _direction_apparent_raw;
+        _speed_true_raw = _speed_apparent;
         return;
     }
 
     // convert apparent wind speed and direction to 2D vector in same frame as vehicle velocity
-    const float wind_dir_180 = wrap_PI(_direction_apparent_ef + radians(180));
+    const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().yaw + radians(180);
     const Vector2f wind_apparent_vec(cosf(wind_dir_180) * _speed_apparent, sinf(wind_dir_180) * _speed_apparent);
 
     // add vehicle velocity
     Vector2f wind_true_vec = Vector2f(wind_apparent_vec.x + veh_velocity.x, wind_apparent_vec.y + veh_velocity.y);
 
     // calculate true speed and direction
-    _direction_true = wrap_PI(atan2f(wind_true_vec.y, wind_true_vec.x) - radians(180));
-    _speed_true = wind_true_vec.length();
+    _direction_true_raw = wrap_PI(atan2f(wind_true_vec.y, wind_true_vec.x) - radians(180));
+    _speed_true_raw = wind_true_vec.length();
+}
+
+// apparent wind is low, can't trust sensors, assume true wind has not changed and calculate apparent wind
+void AP_WindVane::update_apparent_wind_dir_from_true()
+{
+    // if no vehicle speed, can't do calcs
+    Vector3f veh_velocity;
+    if (!AP::ahrs().get_velocity_NED(veh_velocity)) {
+        return;
+    }
+
+    // convert true wind speed and direction to 2D vector in same frame as vehicle velocity
+    const float wind_dir_180 = _direction_true + radians(180);
+    const Vector2f wind_true_vec(cosf(wind_dir_180) * _speed_true, sinf(wind_dir_180) * _speed_true);
+
+    // add vehicle velocity
+    Vector2f wind_apparent_vec = Vector2f(wind_true_vec.x - veh_velocity.x, wind_true_vec.y - veh_velocity.y);
+
+    // calculate apartment speed and direction
+    _direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().yaw);
+    _speed_apparent_raw = wind_apparent_vec.length();
 }
 
 AP_WindVane *AP_WindVane::_singleton = nullptr;

libraries/AP_WindVane/AP_WindVane.h

@@ -22,7 +22,6 @@
 #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;
 
@@ -59,9 +58,7 @@ public:
     void update();
 
     // get the apparent wind direction in body-frame in radians, 0 = head to wind
-    float get_apparent_wind_direction_rad() const {
-        return wrap_PI(_direction_apparent_ef - AP::ahrs().yaw);
-    }
+    float get_apparent_wind_direction_rad() const { return _direction_apparent; }
 
     // get the true wind direction in radians, 0 = wind coming from north
     float get_true_wind_direction_rad() const { return _direction_true; }
@@ -72,6 +69,9 @@ public:
     // Return true wind speed
     float get_true_wind_speed() const { return _speed_true; }
 
+    // Return the apparent wind angle used to determin the current tack
+    float get_tack_threshold_wind_dir_rad() const { return _direction_tack; }
+
     // enum defining current tack
     enum Sailboat_Tack {
         TACK_PORT,
@@ -112,6 +112,7 @@ private:
     AP_Float _speed_sensor_temp_pin;                // speed sensor analog pin for reading temp, -1 if disable
     AP_Float _speed_sensor_voltage_offset;          // analog speed zero wind voltage offset
     AP_Float _speed_filt_hz;                        // speed sensor low pass filter frequency
+    AP_Float _true_filt_hz;                         // true wind speed and direction low pass filter frequency
 
     AP_WindVane_Backend *_direction_driver;
     AP_WindVane_Backend *_speed_driver;
@@ -125,13 +126,29 @@ private:
     // calculate true wind speed and direction from apparent wind
     void update_true_wind_speed_and_direction();
 
+    // assume true wind has not changed and calculate apparent wind
+    void update_apparent_wind_dir_from_true();
+
     // wind direction variables
-    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)
+    float _direction_apparent_raw;                  // wind's apparent direction in radians (0 = ahead of vehicle) in body frame
+    float _direction_apparent;                      // wind's apparent direction in radians (0 = ahead of vehicle) in body frame - filtered
+    float _direction_true_raw;                      // wind's true direction in radians (0 = North)
+    float _direction_true;                          // wind's true direction in radians (0 = North) - filtered
+    float _direction_tack;                          // filtered apparent wind used to determin the current tack
+    LowPassFilterFloat _direction_apparent_sin_filt = LowPassFilterFloat(2.0f);
+    LowPassFilterFloat _direction_apparent_cos_filt = LowPassFilterFloat(2.0f);
+    LowPassFilterFloat _direction_true_sin_filt = LowPassFilterFloat(2.0f);
+    LowPassFilterFloat _direction_true_cos_filt = LowPassFilterFloat(2.0f);
+    LowPassFilterFloat _tack_sin_filt = LowPassFilterFloat(0.1f);
+    LowPassFilterFloat _tack_cos_filt = LowPassFilterFloat(0.1f);
 
     // wind speed variables
-    float _speed_apparent;                          // wind's apparent speed in m/s
-    float _speed_true;                              // wind's true estimated speed in m/s
+    float _speed_apparent_raw;                      // wind's apparent speed in m/s
+    float _speed_apparent;                          // wind's apparent speed in m/s - filtered
+    float _speed_true_raw;                          // wind's true estimated speed in m/s
+    float _speed_true;                              // wind's true estimated speed in m/s - filtered
+    LowPassFilterFloat _speed_apparent_filt = LowPassFilterFloat(2.0f);
+    LowPassFilterFloat _speed_true_filt = LowPassFilterFloat(2.0f);
 
     // current tack
     Sailboat_Tack _current_tack;

libraries/AP_WindVane/AP_WindVane_Airspeed.cpp

@@ -25,6 +25,6 @@ void AP_WindVane_Airspeed::update_speed()
 {
     const AP_Airspeed* airspeed = AP_Airspeed::get_singleton();
     if (airspeed != nullptr) {
-        speed_update_frontend(airspeed->get_airspeed());
+        _frontend._speed_apparent_raw = airspeed->get_raw_airspeed();
     }
 }

libraries/AP_WindVane/AP_WindVane_Analog.cpp

@@ -32,7 +32,7 @@ void AP_WindVane_Analog::update_direction()
     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));
+    _frontend._direction_apparent_raw  = wrap_PI(direction);
 }
 
 void AP_WindVane_Analog::calibrate()

libraries/AP_WindVane/AP_WindVane_Backend.cpp

@@ -22,38 +22,6 @@ AP_WindVane_Backend::AP_WindVane_Backend(AP_WindVane &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 = wrap_PI(atan2f(filtered_sin, filtered_cos));
-    } else {
-        _frontend._direction_apparent_ef = wrap_PI(apparent_angle_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
 void AP_WindVane_Backend::calibrate()
 {

libraries/AP_WindVane/AP_WindVane_Backend.h

@@ -38,16 +38,7 @@ public:
     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);
-    LowPassFilterFloat _tack_filt = LowPassFilterFloat(TACK_FILT_CUTOFF);
 };

libraries/AP_WindVane/AP_WindVane_Home.cpp

@@ -28,9 +28,9 @@ void AP_WindVane_Home::update_direction()
     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_apparent_ef += chan->norm_input() * radians(45);
         }
     }
 
-    direction_update_frontend(direction_apparent_ef);
+    _frontend._direction_apparent_raw = wrap_PI(direction_apparent_ef - AP::ahrs().yaw);
 }

libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp

@@ -50,7 +50,7 @@ void AP_WindVane_ModernDevice::update_speed()
     }
 
     // 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));
+    _frontend._speed_apparent_raw  = 24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f);
 }
 
 void AP_WindVane_ModernDevice::calibrate()

libraries/AP_WindVane/AP_WindVane_NMEA.cpp

@@ -64,10 +64,10 @@ void AP_WindVane_NMEA::update()
         if (decode(c)) {
             // user may not have NMEA selected for both speed and direction
             if (_frontend._direction_type.get() == _frontend.WindVaneType::WINDVANE_NMEA) {
-                direction_update_frontend(wrap_PI(radians(_wind_dir_deg + _frontend._dir_analog_bearing_offset.get()) + AP::ahrs().yaw));
+                _frontend._direction_apparent_raw = wrap_PI(radians(_wind_dir_deg + _frontend._dir_analog_bearing_offset.get()));
             }
             if (_frontend._speed_sensor_type.get() == _frontend.Speed_type::WINDSPEED_NMEA) {
-                speed_update_frontend(_speed_ms);
+                _frontend._speed_apparent_raw = _speed_ms;
             }
         }
     }

libraries/AP_WindVane/AP_WindVane_RPM.cpp

@@ -28,7 +28,7 @@ void AP_WindVane_RPM::update_speed()
         float temp_speed;
         if (rpm->get_rpm(0, temp_speed) &&
             !is_negative(temp_speed)) {
-            speed_update_frontend(temp_speed);
+            _frontend._speed_apparent_raw = temp_speed;
         }
     }
 }

libraries/AP_WindVane/AP_WindVane_SITL.cpp

@@ -42,11 +42,11 @@ void AP_WindVane_SITL::update_direction()
         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));
+        _frontend._direction_apparent_raw =  wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().yaw);
 
     } else { // WINDVANE_SITL_APARRENT
         // directly read the apparent wind from as set by physics backend
-        direction_update_frontend(AP::sitl()->get_apparent_wind_dir());
+        _frontend._direction_apparent_raw =  wrap_PI(AP::sitl()->get_apparent_wind_dir() - AP::ahrs().yaw);
     }
 
 }
@@ -67,11 +67,11 @@ void AP_WindVane_SITL::update_speed()
         wind_vector_ef.x += AP::sitl()->state.speedN;
         wind_vector_ef.y += AP::sitl()->state.speedE;
 
-        speed_update_frontend(wind_vector_ef.length());
+        _frontend._speed_apparent_raw = 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());
+        _frontend._speed_apparent_raw = AP::sitl()->get_apparent_wind_spd();
     }
 }
 #endif