ardupilot/libraries/AP_WindVane/AP_WindVane.h

117 lines
4.0 KiB
C++

/*
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>
#include <Filter/LowPassFilter.h>
class AP_WindVane
{
public:
enum WindVaneType {
WINDVANE_NONE = 0,
WINDVANE_HOME_HEADING = 1,
WINDVANE_PWM_PIN = 2,
WINDVANE_ANALOG_PIN = 3,
WINDVANE_SITL = 10
};
AP_WindVane();
/* Do not allow copies */
AP_WindVane(const AP_WindVane &other) = delete;
AP_WindVane &operator=(const AP_WindVane&) = delete;
static AP_WindVane *get_singleton();
// 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 body-frame in radians, 0 = head to wind
float get_apparent_wind_direction_rad() const;
// record home heading
void record_home_headng();
// start calibration routine
bool start_calibration();
// parameter block
static const struct AP_Param::GroupInfo var_info[];
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();
#endif
// 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 *_singleton;
// 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 _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;
// low pass filters of direction
LowPassFilterFloat wind_sin_filt = LowPassFilterFloat(2.0f);
LowPassFilterFloat wind_cos_filt = LowPassFilterFloat(2.0f);
};
namespace AP {
AP_WindVane *windvane();
};