AP_WindVane: library to read wind direction from sensor

libraries/AP_WindVane/AP_WindVane.cpp

@@ -0,0 +1,292 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_WindVane/AP_WindVane.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <RC_Channel/RC_Channel.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <GCS_MAVLink/GCS.h>
+#include <Filter/Filter.h>
+#include <utility>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include <board_config.h>
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+// by default use the airspeed pin for Vane
+#define WINDVANE_DEFAULT_PIN 15
+
+const AP_Param::GroupInfo AP_WindVane::var_info[] = {
+
+    // @Param: TYPE
+    // @DisplayName: Wind Vane Type
+    // @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),
+
+    // @Param: RC_IN_NO
+    // @DisplayName: RC Input Channel to use as wind angle value
+    // @Description: RC Input Channel to use as wind angle value
+    // @Range: 0 16
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RC_IN_NO", 2, AP_WindVane, _rc_in_no, 0),
+
+    // @Param: ANA_PIN
+    // @DisplayName: Analog input
+    // @Description: Analog input pin to read as Wind vane sensor pot
+    // @Values: 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6,15:Pixhawk2 ADC,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6,103:Pixhawk SBUS
+    // @User: Standard
+    AP_GROUPINFO("ANA_PIN", 3, AP_WindVane, _analog_pin_no, WINDVANE_DEFAULT_PIN),
+
+    // @Param: ANA_V_MIN
+    // @DisplayName: Analog minumum voltage
+    // @Description: Minimum analalog voltage read by windvane
+    // @Units: V
+    // @Increment: 0.01
+    // @Range: 0 5.0
+    // @User: Standard
+    AP_GROUPINFO("ANA_V_MIN", 4, AP_WindVane, _analog_volt_min, 0.0f),
+
+    // @Param: ANA_V_MAX
+    // @DisplayName: Analog maximum voltage
+    // @Description: Minimum analalog voltage read by windvane
+    // @Units: V
+    // @Increment: 0.01
+    // @Range: 0 5.0
+    // @User: Standard
+    AP_GROUPINFO("ANA_V_MAX", 5, AP_WindVane, _analog_volt_max, 3.3f),
+
+    // @Param: ANA_OF_HD
+    // @DisplayName: Analog headwind offset
+    // @Description: Angle offset when windvane is indicating a headwind, ie 0 degress relative to vehicle
+    // @Units: deg
+    // @Increment: 1
+    // @Range: 0 360
+    // @User: Standard
+    AP_GROUPINFO("ANA_OF_HD", 6, AP_WindVane, _analog_head_bearing_offset, 0.0f),
+
+    // @Param: VANE_FILT
+    // @DisplayName: Wind vane low pass filter frequency
+    // @Description: Wind vane low pass filter frequency, a value of -1 disables filter
+    // @Units: Hz
+    // @User: Standard
+    AP_GROUPINFO("VANE_FILT", 7, AP_WindVane, _vane_filt_hz, 0.5f),
+
+    // @Param: CAL
+    // @DisplayName: set to one to enter calibration on reboot
+    // @Description: set to one to enter calibration on reboot
+    // @Values: 0:None, 1:Calibrate
+    // @User: Standard
+    AP_GROUPINFO("CAL", 8, AP_WindVane, _calibration, 0),
+
+    // @Param: ANA_DZ
+    // @DisplayName: Analog potentiometer dead zone
+    // @Description: Analog potentiometer mechanical dead zone
+    // @Units: deg
+    // @Increment: 1
+    // @Range: 0 360
+    // @User: Standard
+    AP_GROUPINFO("ANA_DZ", 9, AP_WindVane, _analog_deadzone, 0),
+
+    AP_GROUPEND
+};
+
+// create a global instances of low pass filter
+LowPassFilterFloat low_pass_filter_wind_sin = LowPassFilterFloat(2.0f);
+LowPassFilterFloat low_pass_filter_wind_cos = LowPassFilterFloat(2.0f);
+LowPassFilterFloat low_pass_filter_wind_speed = LowPassFilterFloat(2.0f);
+
+// Public
+// ------
+
+// constructor
+AP_WindVane::AP_WindVane()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    if (_s_instance) {
+        AP_HAL::panic("Too many Wind Vane sensors");
+    }
+    _s_instance = this;
+}
+
+// destructor
+AP_WindVane::~AP_WindVane(void)
+{
+}
+
+/*
+ * Get the AP_WindVane singleton
+ */
+AP_WindVane *AP_WindVane::get_instance()
+{
+    return _s_instance;
+}
+
+// return true if wind vane is enabled
+bool AP_WindVane::enabled() const
+{
+    return (_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
+    windvane_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+}
+
+// Update wind vane, called at 10hz
+void AP_WindVane::update()
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // check for calibration
+    calibrate();
+
+    update_apparent_wind_direction();
+}
+
+// record home heading for use as wind direction if no sensor is fitted
+void AP_WindVane::record_home_headng()
+{
+    _home_heading = AP::ahrs().yaw;
+}
+
+// Private
+// -------
+
+// read the Wind Vane value from an analog pin, calculate bearing from analog voltage
+// assumes voltage increases as wind vane moves clockwise
+float AP_WindVane::read_analog()
+{
+    windvane_analog_source->set_pin(_analog_pin_no);
+    _current_analog_voltage = windvane_analog_source->voltage_average();
+
+    float current_analog_voltage_constrain = constrain_float(_current_analog_voltage,_analog_volt_min,_analog_volt_max);
+    float voltage_ratio = linear_interpolate(0.0f, 1.0f, current_analog_voltage_constrain, _analog_volt_min, _analog_volt_max);
+
+    float bearing = (voltage_ratio * radians(360 - _analog_deadzone)) + radians(_analog_head_bearing_offset);
+
+    return wrap_PI(bearing);
+}
+
+// read the bearing value from a PWM value on a RC channel (+- 45deg)
+float AP_WindVane::read_PWM_bearing()
+{
+    RC_Channel *ch = rc().channel(_rc_in_no-1);
+    if (ch == nullptr) {
+        return 0.0f;
+    }
+    float bearing = ch->norm_input() * radians(45);
+
+    return wrap_PI(bearing);
+}
+
+// Calculate the apparent wind direction in radians, the wind comes from this direction, 0 = head to wind
+void AP_WindVane::update_apparent_wind_direction()
+{
+    float apparent_angle_in = 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 = wrap_PI(_home_heading - AP::ahrs().yaw);
+            return;
+        case WindVaneType::WINDVANE_PWM_PIN:
+            // this is a approximation as we are not considering boat speed and wind speed
+            // do not filter home heading and pwm type vanes
+            _direction_apparent = wrap_PI(read_PWM_bearing() + _home_heading - AP::ahrs().yaw);
+            return;
+        case WindVaneType::WINDVANE_ANALOG_PIN:
+            apparent_angle_in = read_analog();
+            break;
+    }
+
+    // apply low pass filter if enabled
+    if (is_positive(_vane_filt_hz)) {
+        low_pass_filter_wind_sin.set_cutoff_frequency(_vane_filt_hz);
+        low_pass_filter_wind_cos.set_cutoff_frequency(_vane_filt_hz);
+        // https://en.wikipedia.org/wiki/Mean_of_circular_quantities
+        float filtered_sin = low_pass_filter_wind_sin.apply(sinf(apparent_angle_in), 0.02f);
+        float filtered_cos = low_pass_filter_wind_cos.apply(cosf(apparent_angle_in), 0.02f);
+        _direction_apparent = atan2f(filtered_sin, filtered_cos);
+    } else {
+        _direction_apparent = apparent_angle_in;
+    }
+
+    // make sure between -pi and pi
+    _direction_apparent = wrap_PI(_direction_apparent);
+}
+
+// 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: Analog input calibrating");
+            }
+
+            // 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) {
+                // save min and max voltage
+                _analog_volt_max.set_and_save(_cal_volt_max);
+                _analog_volt_min.set_and_save(_cal_volt_min);
+                _calibration.set_and_save(0);
+                _cal_start_ms = 0;
+                gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Analog input calibration complete");
+            }
+            break;
+    }
+}
+
+AP_WindVane *AP_WindVane::_s_instance = nullptr;
+
+namespace AP {
+    AP_WindVane *windvane()
+    {
+        return AP_WindVane::get_instance();
+    }
+};

libraries/AP_WindVane/AP_WindVane.h

@@ -0,0 +1,105 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+
+class AP_WindVane
+{
+
+public:
+
+    enum WindVaneType {
+        WINDVANE_NONE       = 0,
+        WINDVANE_HOME_HEADING = 1,
+        WINDVANE_PWM_PIN    = 2,
+        WINDVANE_ANALOG_PIN = 3,
+    };
+
+    AP_WindVane();
+
+    /* Do not allow copies */
+    AP_WindVane(const AP_WindVane &other) = delete;
+    AP_WindVane &operator=(const AP_WindVane&) = delete;
+
+    // destructor
+    ~AP_WindVane(void);
+
+    static AP_WindVane *get_instance();
+
+    // return true if wind vane is enabled
+    bool enabled() const;
+
+    // Initialize the Wind Vane object and prepare it for use
+    void init();
+
+    // update wind vane
+    void update();
+
+    // get the apparent wind direction in radians, 0 = head to wind
+    float get_apparent_wind_direction_rad() const { return _direction_apparent; }
+
+    // record home heading
+    void record_home_headng();
+
+    // parameter block
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    // read the bearing value from an analog pin - returns radians
+    float read_analog();
+
+    // read the bearing value from a PWM value on a RC channel - returns radians
+    float read_PWM_bearing();
+
+    // update apparent wind direction
+    void update_apparent_wind_direction();
+
+    // calibrate
+    void calibrate();
+
+    // parameters
+    AP_Int8 _type;                                  // type of windvane being used
+    AP_Int8 _rc_in_no;                              // RC input channel to use
+    AP_Int8 _analog_pin_no;                         // analog pin connected to sensor
+    AP_Float _analog_volt_min;                      // minimum voltage read by windvane
+    AP_Float _analog_volt_max;                      // maximum voltage read by windvane
+    AP_Float _analog_head_bearing_offset;           // angle offset when windvane is indicating a headwind, ie 0 degress relative to vehicle
+    AP_Float _vane_filt_hz;                         // vane Low pass filter frequency
+    AP_Int8 _calibration;                           // enter calibration
+    AP_Float _analog_deadzone;                      // analog pot deadzone in degrees
+
+    static AP_WindVane *_s_instance;
+
+    // wind direction variables
+    float _home_heading;                            // heading in radians recorded when vehicle was armed
+    float _direction_apparent;                      // wind's apparent direction in radians (0 = ahead of vehicle)
+    float _current_analog_voltage;                  // wind direction's latest analog voltage reading
+
+    // 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
+
+    // pin for reading analog voltage
+    AP_HAL::AnalogSource *windvane_analog_source;
+};
+
+namespace AP {
+    AP_WindVane *windvane();
+};