diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index e7c3d716ef..abeb3e7477 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -23,6 +23,8 @@ #include "AP_WindVane_SITL.h" #include "AP_WindVane_NMEA.h" +#include + 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(); - } else { - // no wind speed sensor, so can't do true wind calcs - _direction_true = _direction_apparent_ef; - _speed_true = 0.0f; - return; + 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 { + _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; diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index be0094aa69..ee32ba4aa7 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/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; diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp index ee175900c0..c06527cb2e 100644 --- a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp +++ b/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(); } } diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.cpp b/libraries/AP_WindVane/AP_WindVane_Analog.cpp index 36fc956108..dcec6ed203 100644 --- a/libraries/AP_WindVane/AP_WindVane_Analog.cpp +++ b/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() diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp index b8e3d2f69c..02e041a252 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.cpp +++ b/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() { diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h index f8681a2733..c2319acfba 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.h +++ b/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); }; diff --git a/libraries/AP_WindVane/AP_WindVane_Home.cpp b/libraries/AP_WindVane/AP_WindVane_Home.cpp index d73b16bbd7..657d8b5ea6 100644 --- a/libraries/AP_WindVane/AP_WindVane_Home.cpp +++ b/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); } diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp index 8a892f739f..30714106df 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp +++ b/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() diff --git a/libraries/AP_WindVane/AP_WindVane_NMEA.cpp b/libraries/AP_WindVane/AP_WindVane_NMEA.cpp index fed7c40a36..f587d0846a 100644 --- a/libraries/AP_WindVane/AP_WindVane_NMEA.cpp +++ b/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; } } } diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.cpp b/libraries/AP_WindVane/AP_WindVane_RPM.cpp index 75c44ad036..44aca4d955 100644 --- a/libraries/AP_WindVane/AP_WindVane_RPM.cpp +++ b/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; } } } diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp index 41af63f8a6..08d100c624 100644 --- a/libraries/AP_WindVane/AP_WindVane_SITL.cpp +++ b/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