Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_WindVane: rework filtering and add logging

This commit is contained in:
Iampete1 2021-04-10 21:22:53 +01:00 committed by Peter Barker
parent 2af6854437
commit bf651ea1d9
11 changed files with 156 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
libraries/AP_WindVane/AP_WindVane.cpp
View File

@ -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();
}
if (have_speed && have_direction) {
if (_speed_apparent >= _dir_speed_cutoff) {
// 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;
// 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;

33
libraries/AP_WindVane/AP_WindVane.h
View File

@ -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;

2
libraries/AP_WindVane/AP_WindVane_Airspeed.cpp
View File

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

2
libraries/AP_WindVane/AP_WindVane_Analog.cpp
View File

@ -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()

32
libraries/AP_WindVane/AP_WindVane_Backend.cpp
View File

@ -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()
{

9
libraries/AP_WindVane/AP_WindVane_Backend.h
View File

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

4
libraries/AP_WindVane/AP_WindVane_Home.cpp
View File

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

2
libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp
View File

@ -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()

4
libraries/AP_WindVane/AP_WindVane_NMEA.cpp
View File

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

2
libraries/AP_WindVane/AP_WindVane_RPM.cpp
View File

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

8
libraries/AP_WindVane/AP_WindVane_SITL.cpp
View File

@ -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