diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index 14b51ac438..f85975581b 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -13,21 +13,16 @@ along with this program. If not, see . */ -#include -#include -#include -#include -#include -#include -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include -#endif +#include "AP_WindVane.h" -extern const AP_HAL::HAL& hal; +#include "AP_WindVane_Home.h" +#include "AP_WindVane_Analog.h" +#include "AP_WindVane_ModernDevice.h" +#include "AP_WindVane_Airspeed.h" +#include "AP_WindVane_RPM.h" +#include "AP_WindVane_SITL.h" #define WINDVANE_DEFAULT_PIN 15 // default wind vane sensor analog pin -#define WINDVANE_CALIBRATION_VOLT_DIFF_MIN 1.0f // calibration routine's min voltage difference required for success #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 @@ -39,7 +34,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Description: Wind Vane type // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog // @User: Standard - AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _type, 0, AP_PARAM_FLAG_ENABLE), + // @RebootRequired: True + AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE), // @Param: RC_IN_NO // @DisplayName: Wind vane sensor RC Input Channel @@ -92,8 +88,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Param: CAL // @DisplayName: Wind vane calibration start - // @Description: Start wind vane calibration by setting this to 1 - // @Values: 0:None, 1:Calibrate + // @Description: Start wind vane calibration by setting this to 1 or 2 + // @Values: 0:None, 1:Calibrate direction, 2:Calibrate speed // @User: Standard AP_GROUPINFO("CAL", 8, AP_WindVane, _calibration, 0), @@ -120,6 +116,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Description: Wind speed sensor type // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,10:SITL // @User: Standard + // @RebootRequired: True AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type, 0), // @Param: SPEED_PIN @@ -178,60 +175,113 @@ AP_WindVane *AP_WindVane::get_singleton() // return true if wind vane is enabled bool AP_WindVane::enabled() const { - return (_type != WINDVANE_NONE); + return _direction_type != WINDVANE_NONE; } // Initialize the Wind Vane object and prepare it for use void AP_WindVane::init() { - // a pin for reading the Wind Vane voltage - dir_analog_source = hal.analogin->channel(_dir_analog_pin); + // don't init twice + if (_direction_driver != nullptr || _speed_driver != nullptr ) { + return; + } - // pins for ModernDevice rev p wind sensor - speed_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); - speed_temp_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); + // wind direction + switch (_direction_type) { + case WindVaneType::WINDVANE_NONE: + // WindVane disabled + return; + case WindVaneType::WINDVANE_HOME_HEADING: + case WindVaneType::WINDVANE_PWM_PIN: + _direction_driver = new AP_WindVane_Home(*this); + break; + case WindVaneType::WINDVANE_ANALOG_PIN: + _direction_driver = new AP_WindVane_Analog(*this); + break; + case WindVaneType::WINDVANE_SITL: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + _direction_driver = new AP_WindVane_SITL(*this); +#endif + break; + } - // check that airspeed is enabled if it is selected as sensor type, if not revert to no wind speed sensor - AP_Airspeed* airspeed = AP_Airspeed::get_singleton(); - if (_speed_sensor_type == Speed_type::WINDSPEED_AIRSPEED && (airspeed == nullptr || !airspeed->enabled())) { - _speed_sensor_type.set(Speed_type::WINDSPEED_NONE); + // wind speed + switch (_speed_sensor_type) { + case Speed_type::WINDSPEED_NONE: + break; + case Speed_type::WINDSPEED_AIRSPEED: + _speed_driver = new AP_WindVane_Airspeed(*this); + break; + case Speed_type::WINDVANE_WIND_SENSOR_REV_P: + _speed_driver = new AP_WindVane_ModernDevice(*this); + break; + case Speed_type::WINDSPEED_SITL: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // single driver does both speed and direction + if (_direction_type != WINDVANE_SITL) { + _speed_driver = new AP_WindVane_SITL(*this); + } else { + _speed_driver = _direction_driver; + } +#endif + break; + case Speed_type::WINDSPEED_RPM: + _speed_driver = new AP_WindVane_RPM(*this); + break; } } // 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; + // exit immediately if not enabled - if (!enabled()) { + if (!enabled() || (!have_speed && !have_direciton)) { return; } - // check for calibration - calibrate(); + // calibrate if booted and disarmed + if (!hal.util->get_soft_armed()) { + if (_calibration == 1 && have_direciton) { + _direction_driver->calibrate(); + } else if (_calibration == 2 && have_speed) { + _speed_driver->calibrate(); + } else if (_calibration != 0) { + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: driver not found"); + _calibration.set_and_save(0); + } + } else if (_calibration != 0) { + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: disarm for cal"); + _calibration.set_and_save(0); + } // read apparent wind speed - update_apparent_wind_speed(); + if (have_speed) { + _speed_driver->update_speed(); + } - // read apparent wind direction (relies on wind speed above) - update_apparent_wind_direction(); + // read apparent wind direction + if (_speed_apparent >= _dir_speed_cutoff && have_direciton) { + // only update if enough wind to move vane + _direction_driver->update_direction(); + } // calculate true wind speed and direction from apparent wind - update_true_wind_speed_and_direction(); + if (have_speed && have_direciton) { + update_true_wind_speed_and_direction(); + } else { + // no wind speed sensor, so can't do true wind calcs + _direction_absolute = _direction_apparent_ef; + _speed_true = 0.0f; + return; + } } -// get the apparent wind direction in radians, 0 = head to wind -float AP_WindVane::get_apparent_wind_direction_rad() const -{ - return wrap_PI(_direction_apparent_ef - AP::ahrs().yaw); -} -// record home heading for use as wind direction if no sensor is fitted -void AP_WindVane::record_home_heading() -{ - _home_heading = AP::ahrs().yaw; -} - -bool AP_WindVane::start_calibration() +// to start direction calibration from mavlink or other +bool AP_WindVane::start_direction_calibration() { if (enabled() && (_calibration == 0)) { _calibration = 1; @@ -240,6 +290,16 @@ bool AP_WindVane::start_calibration() return false; } +// to start speed calibration from mavlink or other +bool AP_WindVane::start_speed_calibration() +{ + if (enabled() && (_calibration == 0)) { + _calibration = 2; + return true; + } + return false; +} + // send mavlink wind message void AP_WindVane::send_wind(mavlink_channel_t chan) { @@ -256,204 +316,10 @@ void AP_WindVane::send_wind(mavlink_channel_t chan) 0); } -// read an analog port and calculate the wind direction in earth-frame in radians -// assumes voltage increases as wind vane moves clockwise -float AP_WindVane::read_analog_direction_ef() -{ - dir_analog_source->set_pin(_dir_analog_pin); - _current_analog_voltage = dir_analog_source->voltage_average_ratiometric(); - - const float voltage_ratio = linear_interpolate(0.0f, 1.0f, _current_analog_voltage, _dir_analog_volt_min, _dir_analog_volt_max); - const float direction = (voltage_ratio * radians(360 - _dir_analog_deadzone)) + radians(_dir_analog_bearing_offset); - - return wrap_PI(direction + AP::ahrs().yaw); -} - -// read rc input of apparent wind direction in earth-frame in radians -float AP_WindVane::read_PWM_direction_ef() -{ - RC_Channel *chan = rc().channel(_rc_in_no-1); - if (chan == nullptr) { - return 0.0f; - } - float direction = chan->norm_input() * radians(45); - - return wrap_PI(direction + _home_heading); -} - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -// read SITL's apparent wind direction in earth-frame in radians -float AP_WindVane::read_SITL_direction_ef() -{ - // temporarily store true speed and direction for easy access - const float wind_speed = AP::sitl()->wind_speed_active; - const float wind_dir_rad = radians(AP::sitl()->wind_direction_active); - - // Note than the SITL wind direction is defined as the direction the wind is traveling to - // This is accounted for in these calculations - - // convert true wind speed and direction into a 2D vector - Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); - - // add vehicle speed to get apparent wind vector - wind_vector_ef.x += AP::sitl()->state.speedN; - wind_vector_ef.y += AP::sitl()->state.speedE; - - return atan2f(wind_vector_ef.y, wind_vector_ef.x); -} - -// read the apparent wind speed in m/s from SITL -float AP_WindVane::read_wind_speed_SITL() -{ - // temporarily store true speed and direction for easy access - const float wind_speed = AP::sitl()->wind_speed_active; - const float wind_dir_rad = radians(AP::sitl()->wind_direction_active); - - // convert true wind speed and direction into a 2D vector - Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); - - // add vehicle speed to get apparent wind vector - wind_vector_ef.x += AP::sitl()->state.speedN; - wind_vector_ef.y += AP::sitl()->state.speedE; - - return wind_vector_ef.length(); -} -#endif - -// read wind speed from Modern Device rev p wind sensor -// https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/ -float AP_WindVane::read_wind_speed_ModernDevice() -{ - float analog_voltage = 0.0f; - - // only read temp pin if defined, sensor will do OK assuming constant temp - float temp_ambient = 28.0f; // equations were generated at this temp in above data sheet - if (is_positive(_speed_sensor_temp_pin)) { - speed_temp_analog_source->set_pin(_speed_sensor_temp_pin); - analog_voltage = speed_temp_analog_source->voltage_average(); - temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C - // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero - temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f); - } - - speed_analog_source->set_pin(_speed_sensor_speed_pin); - analog_voltage = speed_analog_source->voltage_average(); - - // apply voltage offset and make sure not negative - // by default the voltage offset is the number provide by the manufacturer - analog_voltage = analog_voltage - _speed_sensor_voltage_offset; - if (is_negative(analog_voltage)) { - analog_voltage = 0.0f; - } - - // simplified equation from data sheet, converted from mph to m/s - return 24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f); -} - -// update the apparent wind speed -void AP_WindVane::update_apparent_wind_speed() -{ - float apparent_speed_in = 0.0f; - - switch (_speed_sensor_type) { - case WINDSPEED_NONE: - _speed_apparent = 0.0f; - break; - case WINDSPEED_AIRSPEED: { - const AP_Airspeed* airspeed = AP_Airspeed::get_singleton(); - if (airspeed != nullptr) { - apparent_speed_in = airspeed->get_airspeed(); - } - break; - } - case WINDVANE_WIND_SENSOR_REV_P: - apparent_speed_in = read_wind_speed_ModernDevice(); - break; - case WINDSPEED_SITL: -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - apparent_speed_in = read_wind_speed_SITL(); -#endif - break; - case WINDSPEED_RPM: { - const AP_RPM* rpm = AP_RPM::get_singleton(); - if (rpm != nullptr) { - const float temp_speed = rpm->get_rpm(0); - if (!is_negative(temp_speed)) { - apparent_speed_in = temp_speed; - } - } - break; - } - } - - // apply low pass filter if enabled - if (is_positive(_speed_filt_hz)) { - _speed_filt.set_cutoff_frequency(_speed_filt_hz); - _speed_apparent = _speed_filt.apply(apparent_speed_in, 0.02f); - } else { - _speed_apparent = apparent_speed_in; - } -} - -// calculate the apparent wind direction in radians, the wind comes from this direction, 0 = head to wind -// expected to be called at 20hz after apparent wind speed has been updated -void AP_WindVane::update_apparent_wind_direction() -{ - float apparent_angle_ef = 0.0f; - - switch (_type) { - case WindVaneType::WINDVANE_HOME_HEADING: - // this is a approximation as we are not considering boat speed and wind speed - // do not filter home heading - _direction_apparent_ef = _home_heading; - return; - case WindVaneType::WINDVANE_PWM_PIN: - // this is a approximation as we are not considering boat speed and wind speed - // do not filter pwm input from pilot - _direction_apparent_ef = read_PWM_direction_ef(); - return; - case WindVaneType::WINDVANE_ANALOG_PIN: - apparent_angle_ef = read_analog_direction_ef(); - break; - case WindVaneType::WINDVANE_SITL: -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - apparent_angle_ef = read_SITL_direction_ef(); -#endif - break; - } - - // if not enough wind to move vane do not update the value - if (_speed_apparent < _dir_speed_cutoff){ - return; - } - - // apply low pass filter if enabled - if (is_positive(_dir_filt_hz)) { - _dir_sin_filt.set_cutoff_frequency(_dir_filt_hz); - _dir_cos_filt.set_cutoff_frequency(_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); - _direction_apparent_ef = atan2f(filtered_sin, filtered_cos); - } else { - _direction_apparent_ef = apparent_angle_ef; - } - - // make sure between -pi and pi - _direction_apparent_ef = wrap_PI(_direction_apparent_ef); -} - // calculate true wind speed and direction from apparent wind // https://en.wikipedia.org/wiki/Apparent_wind void AP_WindVane::update_true_wind_speed_and_direction() { - if (_speed_sensor_type == Speed_type::WINDSPEED_NONE) { - // no wind speed sensor, so can't do true wind calcs - _direction_absolute = _direction_apparent_ef; - _speed_true = 0.0f; - return; - } - // if no vehicle speed, can't do calcs Vector3f veh_velocity; if (!AP::ahrs().get_velocity_NED(veh_velocity)) { @@ -475,61 +341,6 @@ void AP_WindVane::update_true_wind_speed_and_direction() _speed_true = wind_true_vec.length(); } -// calibrate windvane -void AP_WindVane::calibrate() -{ - // exit immediately if armed or too soon after start - if (hal.util->get_soft_armed()) { - return; - } - - // return if not calibrating - if (_calibration == 0) { - return; - } - - switch (_type) { - case WindVaneType::WINDVANE_HOME_HEADING: - case WindVaneType::WINDVANE_PWM_PIN: - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: No cal required"); - _calibration.set_and_save(0); - break; - case WindVaneType::WINDVANE_ANALOG_PIN: - // start calibration - if (_cal_start_ms == 0) { - _cal_start_ms = AP_HAL::millis(); - _cal_volt_max = _current_analog_voltage; - _cal_volt_min = _current_analog_voltage; - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane"); - } - - // record min and max voltage - _cal_volt_max = MAX(_cal_volt_max, _current_analog_voltage); - _cal_volt_min = MIN(_cal_volt_min, _current_analog_voltage); - - // calibrate for 30 seconds - if ((AP_HAL::millis() - _cal_start_ms) > 30000) { - // check for required voltage difference - const float volt_diff = _cal_volt_max - _cal_volt_min; - if (volt_diff >= WINDVANE_CALIBRATION_VOLT_DIFF_MIN) { - // save min and max voltage - _dir_analog_volt_max.set_and_save(_cal_volt_max); - _dir_analog_volt_min.set_and_save(_cal_volt_min); - _calibration.set_and_save(0); - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)", - (double)_cal_volt_min, - (double)_cal_volt_max); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)", - (double)volt_diff, - (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN); - } - _cal_start_ms = 0; - } - break; - } -} - AP_WindVane *AP_WindVane::_singleton = nullptr; namespace AP { diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index cbeb55595b..ec3e82fa8b 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -14,24 +14,22 @@ */ #pragma once -#include #include -#include -#include -#include +#include + +class AP_WindVane_Backend; class AP_WindVane { public: - - enum WindVaneType { - WINDVANE_NONE = 0, - WINDVANE_HOME_HEADING = 1, - WINDVANE_PWM_PIN = 2, - WINDVANE_ANALOG_PIN = 3, - WINDVANE_SITL = 10 - }; + friend class AP_WindVane_Backend; + friend class AP_WindVane_Home; + friend class AP_WindVane_Analog; + friend class AP_WindVane_SITL; + friend class AP_WindVane_ModernDevice; + friend class AP_WindVane_Airspeed; + friend class AP_WindVane_RPM; AP_WindVane(); @@ -51,7 +49,9 @@ public: void update(); // get the apparent wind direction in body-frame in radians, 0 = head to wind - float get_apparent_wind_direction_rad() const; + float get_apparent_wind_direction_rad() const { + return wrap_PI(_direction_apparent_ef - AP::ahrs().yaw); + } // get the absolute wind direction in radians, 0 = wind coming from north float get_absolute_wind_direction_rad() const { return _direction_absolute; } @@ -62,11 +62,12 @@ public: // Return true wind speed float get_true_wind_speed() const { return _speed_true; } - // record home heading - void record_home_heading(); + // record home heading for use as wind direction if no sensor is fitted + void record_home_heading() { _home_heading = AP::ahrs().yaw; } // start calibration routine - bool start_calibration(); + bool start_direction_calibration(); + bool start_speed_calibration(); // send mavlink wind message void send_wind(mavlink_channel_t chan); @@ -76,37 +77,8 @@ public: private: - // read an analog port and calculate the wind direction in earth-frame in radians - float read_analog_direction_ef(); - - // read rc input of apparent wind direction in earth-frame in radians - float read_PWM_direction_ef(); - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - // read SITL's apparent wind direction in earth-frame in radians - float read_SITL_direction_ef(); - - // read the apparent wind speed in m/s from SITL - float read_wind_speed_SITL(); -#endif - - // read wind speed from ModernDevice wind speed sensor rev p - float read_wind_speed_ModernDevice(); - - // update wind speed sensor - void update_apparent_wind_speed(); - - // update apparent wind direction - void update_apparent_wind_direction(); - - // calculate true wind speed and direction from apparent wind - void update_true_wind_speed_and_direction(); - - // calibrate - void calibrate(); - // parameters - AP_Int8 _type; // type of windvane being used + AP_Int8 _direction_type; // type of windvane being used AP_Int8 _rc_in_no; // RC input channel to use AP_Int8 _dir_analog_pin; // analog pin connected to wind vane direction sensor AP_Float _dir_analog_volt_min; // minimum voltage read by windvane @@ -122,22 +94,36 @@ private: AP_Float _speed_sensor_voltage_offset; // analog speed zero wind voltage offset AP_Float _speed_filt_hz; // speed sensor low pass filter frequency - static AP_WindVane *_singleton; + AP_WindVane_Backend *_direction_driver; + AP_WindVane_Backend *_speed_driver; + + // update wind speed sensor + void update_apparent_wind_speed(); + + // update apparent wind direction + void update_apparent_wind_direction(); + + // calculate true wind speed and direction from apparent wind + void update_true_wind_speed_and_direction(); // wind direction variables - float _home_heading; // heading in radians recorded when vehicle was armed float _direction_apparent_ef; // wind's apparent direction in radians (0 = ahead of vehicle) float _direction_absolute; // wind's absolute direction in radians (0 = North) - float _current_analog_voltage; // wind direction's latest analog voltage reading // wind speed variables float _speed_apparent; // wind's apparent speed in m/s float _speed_true; // wind's true estimated speed in m/s - // calibration variables - uint32_t _cal_start_ms = 0; // calibration start time in milliseconds after boot - float _cal_volt_max; // maximum observed voltage during calibration - float _cal_volt_min; // minimum observed voltage during calibration + // heading in radians recorded when vehicle was armed + float _home_heading; + + enum WindVaneType { + WINDVANE_NONE = 0, + WINDVANE_HOME_HEADING = 1, + WINDVANE_PWM_PIN = 2, + WINDVANE_ANALOG_PIN = 3, + WINDVANE_SITL = 10 + }; enum Speed_type { WINDSPEED_NONE = 0, @@ -147,15 +133,7 @@ private: WINDSPEED_SITL = 10 }; - // pin for reading analog voltage - AP_HAL::AnalogSource *dir_analog_source; - AP_HAL::AnalogSource *speed_analog_source; - AP_HAL::AnalogSource *speed_temp_analog_source; - - // 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); + static AP_WindVane *_singleton; }; namespace AP { diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp new file mode 100644 index 0000000000..ee175900c0 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp @@ -0,0 +1,30 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_Airspeed.h" + +// constructor +AP_WindVane_Airspeed::AP_WindVane_Airspeed(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ +} + +void AP_WindVane_Airspeed::update_speed() +{ + const AP_Airspeed* airspeed = AP_Airspeed::get_singleton(); + if (airspeed != nullptr) { + speed_update_frontend(airspeed->get_airspeed()); + } +} diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.h b/libraries/AP_WindVane/AP_WindVane_Airspeed.h new file mode 100644 index 0000000000..0f0bc748fe --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.h @@ -0,0 +1,29 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" + +#include + +class AP_WindVane_Airspeed : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_Airspeed(AP_WindVane &frontend); + + // update state + void update_speed() override; +}; diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.cpp b/libraries/AP_WindVane/AP_WindVane_Analog.cpp new file mode 100644 index 0000000000..36fc956108 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Analog.cpp @@ -0,0 +1,72 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_Analog.h" + +#define WINDVANE_CALIBRATION_VOLT_DIFF_MIN 1.0f // calibration routine's min voltage difference required for success + +// constructor +AP_WindVane_Analog::AP_WindVane_Analog(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ + _dir_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); +} + +void AP_WindVane_Analog::update_direction() +{ + _dir_analog_source->set_pin(_frontend._dir_analog_pin); + _current_analog_voltage = _dir_analog_source->voltage_average_ratiometric(); + + 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)); +} + +void AP_WindVane_Analog::calibrate() +{ + + // start calibration + if (_cal_start_ms == 0) { + _cal_start_ms = AP_HAL::millis(); + _cal_volt_max = _current_analog_voltage; + _cal_volt_min = _current_analog_voltage; + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane"); + } + + // record min and max voltage + _cal_volt_max = MAX(_cal_volt_max, _current_analog_voltage); + _cal_volt_min = MIN(_cal_volt_min, _current_analog_voltage); + + // calibrate for 30 seconds + if ((AP_HAL::millis() - _cal_start_ms) > 30000) { + // check for required voltage difference + const float volt_diff = _cal_volt_max - _cal_volt_min; + if (volt_diff >= WINDVANE_CALIBRATION_VOLT_DIFF_MIN) { + // save min and max voltage + _frontend._dir_analog_volt_max.set_and_save(_cal_volt_max); + _frontend._dir_analog_volt_min.set_and_save(_cal_volt_min); + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)", + (double)_cal_volt_min, + (double)_cal_volt_max); + } else { + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)", + (double)volt_diff, + (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN); + } + _frontend._calibration.set_and_save(0); + _cal_start_ms = 0; + } +} diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.h b/libraries/AP_WindVane/AP_WindVane_Analog.h new file mode 100644 index 0000000000..eed943835c --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Analog.h @@ -0,0 +1,42 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +class AP_WindVane_Analog : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_Analog(AP_WindVane &frontend); + + // update state + void update_direction() override; + void calibrate() override; + +private: + // pin for reading analog voltage + AP_HAL::AnalogSource *_dir_analog_source; + + float _current_analog_voltage; + uint32_t _cal_start_ms = 0; + float _cal_volt_min; + float _cal_volt_max; +}; diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp new file mode 100644 index 0000000000..8452d4427a --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Backend.cpp @@ -0,0 +1,62 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane.h" +#include "AP_WindVane_Backend.h" + +// base class constructor. +AP_WindVane_Backend::AP_WindVane_Backend(AP_WindVane &frontend) : + _frontend(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 = atan2f(filtered_sin, filtered_cos); + } else { + _frontend._direction_apparent_ef = apparent_angle_ef; + } + + // make sure between -pi and pi + _frontend._direction_apparent_ef = wrap_PI(_frontend._direction_apparent_ef); +} + +// calibrate WindVane +void AP_WindVane_Backend::calibrate() +{ + gcs().send_text(MAV_SEVERITY_INFO, "WindVane: No cal required"); + _frontend._calibration.set_and_save(0); + return; +} diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h new file mode 100644 index 0000000000..56e1d73138 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Backend.h @@ -0,0 +1,49 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane.h" + +#include +#include + +class AP_WindVane_Backend +{ +public: + // constructor. This incorporates initialization as well. + AP_WindVane_Backend(AP_WindVane &frontend); + + // we declare a virtual destructor so that WindVane drivers can + // override with a custom destructor if need be + virtual ~AP_WindVane_Backend() {} + + // update the state structure + virtual void update_speed() {}; + virtual void update_direction() {}; + 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); +}; diff --git a/libraries/AP_WindVane/AP_WindVane_Home.cpp b/libraries/AP_WindVane/AP_WindVane_Home.cpp new file mode 100644 index 0000000000..d73b16bbd7 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Home.cpp @@ -0,0 +1,36 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_Home.h" + +// constructor +AP_WindVane_Home::AP_WindVane_Home(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ +} + +void AP_WindVane_Home::update_direction() +{ + float direction_apparent_ef = _frontend._home_heading; + + 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_update_frontend(direction_apparent_ef); +} diff --git a/libraries/AP_WindVane/AP_WindVane_Home.h b/libraries/AP_WindVane/AP_WindVane_Home.h new file mode 100644 index 0000000000..7dc1271194 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_Home.h @@ -0,0 +1,28 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" +#include + +class AP_WindVane_Home : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_Home(AP_WindVane &frontend); + + // update state + void update_direction() override; +}; diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp new file mode 100644 index 0000000000..b5bcc70ca8 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp @@ -0,0 +1,54 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_ModernDevice.h" +// read wind speed from Modern Device rev p wind sensor +// https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/ + +// constructor +AP_WindVane_ModernDevice::AP_WindVane_ModernDevice(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ + _speed_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); + _temp_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); +} + +void AP_WindVane_ModernDevice::update_speed() +{ + float analog_voltage = 0.0f; + + // only read temp pin if defined, sensor will do OK assuming constant temp + float temp_ambient = 28.0f; // equations were generated at this temp in above data sheet + if (is_positive(_frontend._speed_sensor_temp_pin.get())) { + _temp_analog_source->set_pin(_frontend._speed_sensor_temp_pin.get()); + analog_voltage = _temp_analog_source->voltage_average(); + temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C + // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero + temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f); + } + + _speed_analog_source->set_pin(_frontend._speed_sensor_speed_pin.get()); + _current_analog_voltage = _speed_analog_source->voltage_average(); + + // apply voltage offset and make sure not negative + // by default the voltage offset is the number provide by the manufacturer + analog_voltage = _current_analog_voltage - _frontend._speed_sensor_voltage_offset; + if (is_negative(analog_voltage)) { + analog_voltage = 0.0f; + } + + // 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)); +} diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h new file mode 100644 index 0000000000..66b2e8c5c2 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h @@ -0,0 +1,40 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +class AP_WindVane_ModernDevice : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_ModernDevice(AP_WindVane &frontend); + + // update state + void update_speed() override; + void calibrate() override; + +private: + float _current_analog_voltage; + + // pin for reading analog voltage + AP_HAL::AnalogSource *_speed_analog_source; + AP_HAL::AnalogSource *_temp_analog_source; +}; diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.cpp b/libraries/AP_WindVane/AP_WindVane_RPM.cpp new file mode 100644 index 0000000000..311d645dee --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_RPM.cpp @@ -0,0 +1,33 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_RPM.h" + +// constructor +AP_WindVane_RPM::AP_WindVane_RPM(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ +} + +void AP_WindVane_RPM::update_speed() +{ + const AP_RPM* rpm = AP_RPM::get_singleton(); + if (rpm != nullptr) { + const float temp_speed = rpm->get_rpm(0); + if (!is_negative(temp_speed)) { + speed_update_frontend(temp_speed); + } + } +} diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.h b/libraries/AP_WindVane/AP_WindVane_RPM.h new file mode 100644 index 0000000000..aa79c23680 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_RPM.h @@ -0,0 +1,29 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" + +#include + +class AP_WindVane_RPM : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_RPM(AP_WindVane &frontend); + + // update state + void update_speed() override; +}; diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp new file mode 100644 index 0000000000..1031a8b32b --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_SITL.cpp @@ -0,0 +1,60 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_WindVane_SITL.h" + +// constructor +AP_WindVane_SITL::AP_WindVane_SITL(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ +} + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +void AP_WindVane_SITL::update_direction() +{ + // temporarily store true speed and direction for easy access + const float wind_speed = AP::sitl()->wind_speed_active; + const float wind_dir_rad = radians(AP::sitl()->wind_direction_active); + + // Note than the SITL wind direction is defined as the direction the wind is traveling to + // This is accounted for in these calculations + + // convert true wind speed and direction into a 2D vector + Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); + + // add vehicle speed to get apparent wind vector + 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)); +} + +void AP_WindVane_SITL::update_speed() +{ + // temporarily store true speed and direction for easy access + const float wind_speed = AP::sitl()->wind_speed_active; + const float wind_dir_rad = radians(AP::sitl()->wind_direction_active); + + // convert true wind speed and direction into a 2D vector + Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); + + // add vehicle speed to get apparent wind vector + wind_vector_ef.x += AP::sitl()->state.speedN; + wind_vector_ef.y += AP::sitl()->state.speedE; + + speed_update_frontend(wind_vector_ef.length()); +} +#endif diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.h b/libraries/AP_WindVane/AP_WindVane_SITL.h new file mode 100644 index 0000000000..86d07b857d --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_SITL.h @@ -0,0 +1,30 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_WindVane_Backend.h" + +class AP_WindVane_SITL : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_SITL(AP_WindVane &frontend); + + // update state + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL + void update_direction() override; + void update_speed() override; + #endif +};