mirror of https://github.com/ArduPilot/ardupilot
AP_WindVane: mini onion
This commit is contained in:
Peter Hall
Randy Mackay
parent
dbbf09d018
commit
9dba9867c6
|
|
@ -13,21 +13,16 @@
|
|||
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_SITL
|
||||
#include <SITL/SITL.h>
|
||||
#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
|
||||
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 {
|
||||
|
|
|
|||
|
|
@ -14,24 +14,22 @@
|
|||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_RPM/AP_RPM.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
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 {
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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());
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane_Backend.h"
|
||||
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
|
||||
class AP_WindVane_Airspeed : public AP_WindVane_Backend
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
AP_WindVane_Airspeed(AP_WindVane &frontend);
|
||||
|
||||
// update state
|
||||
void update_speed() override;
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane_Backend.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <Filter/Filter.h>
|
||||
|
||||
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);
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane_Backend.h"
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
|
||||
class AP_WindVane_Home : public AP_WindVane_Backend
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
AP_WindVane_Home(AP_WindVane &frontend);
|
||||
|
||||
// update state
|
||||
void update_direction() override;
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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));
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane_Backend.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_WindVane_Backend.h"
|
||||
|
||||
#include <AP_RPM/AP_RPM.h>
|
||||
|
||||
class AP_WindVane_RPM : public AP_WindVane_Backend
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
AP_WindVane_RPM(AP_WindVane &frontend);
|
||||
|
||||
// update state
|
||||
void update_speed() override;
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
};
|
||||
Loading…
Reference in New Issue