Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_WindVane: add apparent wind SITL types

This commit is contained in:
Iampete1 2020-08-15 16:36:29 +01:00 committed by Andrew Tridgell
parent 2764126a1c
commit 917358e4a5
3 changed files with 48 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_WindVane/AP_WindVane.cpp
View File

@ -28,7 +28,7 @@ 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,4:NMEA,10:SITL
// @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL true,11:SITL apparent
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
@ -110,7 +110,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
// @Param: SPEED_TYPE
// @DisplayName: Wind speed sensor Type
// @Description: Wind speed sensor type
// @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL
// @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL true,11:SITL apparent
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type, 0),
@ -200,7 +200,8 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
case WindVaneType::WINDVANE_ANALOG_PIN:
_direction_driver = new AP_WindVane_Analog(*this);
break;
case WindVaneType::WINDVANE_SITL:
case WindVaneType::WINDVANE_SITL_TRUE:
case WindVaneType::WINDVANE_SITL_APPARENT:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
_direction_driver = new AP_WindVane_SITL(*this);
#endif
@ -221,10 +222,11 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
_speed_driver = new AP_WindVane_ModernDevice(*this);
break;
case Speed_type::WINDSPEED_SITL:
case Speed_type::WINDSPEED_SITL_TRUE:
case Speed_type::WINDSPEED_SITL_APPARENT:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// single driver does both speed and direction
if (_direction_type != WindVaneType::WINDVANE_SITL) {
if (_direction_type != _speed_sensor_type) {
_speed_driver = new AP_WindVane_SITL(*this);
} else {
_speed_driver = _direction_driver;

6
libraries/AP_WindVane/AP_WindVane.h
View File

@ -145,7 +145,8 @@ private:
WINDVANE_PWM_PIN = 2,
WINDVANE_ANALOG_PIN = 3,
WINDVANE_NMEA = 4,
WINDVANE_SITL = 10
WINDVANE_SITL_TRUE = 10,
WINDVANE_SITL_APPARENT = 11,
};
enum Speed_type {
@ -154,7 +155,8 @@ private:
WINDVANE_WIND_SENSOR_REV_P = 2,
WINDSPEED_RPM = 3,
WINDSPEED_NMEA = 4,
WINDSPEED_SITL = 10
WINDSPEED_SITL_TRUE = 10,
WINDSPEED_SITL_APPARENT = 11,
};
static AP_WindVane *_singleton;

57
libraries/AP_WindVane/AP_WindVane_SITL.cpp
View File

@ -25,36 +25,53 @@ AP_WindVane_SITL::AP_WindVane_SITL(AP_WindVane &frontend) :
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);
if (_frontend._direction_type == _frontend.WindVaneType::WINDVANE_SITL_TRUE) {
// read in the true wind direction and calculate the apparent
// Note than the SITL wind direction is defined as the direction the wind is traveling to
// This is accounted for in these calculations
// 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);
// Note than the SITL wind direction is defined as the direction the wind is traveling to
// This is accounted for in these calculations
// add vehicle speed to get apparent wind vector
wind_vector_ef.x += AP::sitl()->state.speedN;
wind_vector_ef.y += AP::sitl()->state.speedE;
// 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));
} else { // WINDVANE_SITL_APARRENT
// directly read the apparent wind from as set by physics backend
direction_update_frontend(AP::sitl()->get_apparent_wind_dir());
}
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);
if (_frontend._speed_sensor_type == _frontend.Speed_type::WINDSPEED_SITL_TRUE) {
// read in the true wind direction and calculate the apparent
// 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);
// 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);
// add vehicle speed to get apparent wind vector
wind_vector_ef.x += AP::sitl()->state.speedN;
wind_vector_ef.y += AP::sitl()->state.speedE;
// 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);
speed_update_frontend(wind_vector_ef.length());
// 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());
} else { // WINDSPEED_SITL_APARRENT
// directly read the apparent wind from as set by physics backend
speed_update_frontend(AP::sitl()->get_apparent_wind_spd());
}
}
#endif