aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_WindVane: library to read wind direction from sensor

Browse Source
This commit is contained in:
IamPete1 2018-08-22 11:45:56 +09:00 committed by Randy Mackay
parent 7a8e7449fa
commit b54e3df57c
2 changed files with 397 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

292
libraries/AP_WindVane/AP_WindVane.cpp Normal file
View File

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

105
libraries/AP_WindVane/AP_WindVane.h Normal file
View File

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