Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_WindVane: add apparent wind SITL types

Browse Source
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 // @Param: TYPE
// @DisplayName: Wind Vane Type // @DisplayName: Wind Vane Type
// @Description: 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 // @User: Standard
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE), 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 // @Param: SPEED_TYPE
// @DisplayName: Wind speed sensor Type // @DisplayName: Wind speed sensor Type
// @Description: 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 // @User: Standard
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type, 0), 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: case WindVaneType::WINDVANE_ANALOG_PIN:
_direction_driver = new AP_WindVane_Analog(*this); _direction_driver = new AP_WindVane_Analog(*this);
break; break;
case WindVaneType::WINDVANE_SITL: case WindVaneType::WINDVANE_SITL_TRUE:
case WindVaneType::WINDVANE_SITL_APPARENT:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
_direction_driver = new AP_WindVane_SITL(*this); _direction_driver = new AP_WindVane_SITL(*this);
#endif #endif
@ -221,10 +222,11 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
case Speed_type::WINDVANE_WIND_SENSOR_REV_P: case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
_speed_driver = new AP_WindVane_ModernDevice(*this); _speed_driver = new AP_WindVane_ModernDevice(*this);
break; 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 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// single driver does both speed and direction // 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); _speed_driver = new AP_WindVane_SITL(*this);
} else { } else {
_speed_driver = _direction_driver; _speed_driver = _direction_driver;

6
libraries/AP_WindVane/AP_WindVane.h
View File

@ -145,7 +145,8 @@ private:
WINDVANE_PWM_PIN = 2, WINDVANE_PWM_PIN = 2,
WINDVANE_ANALOG_PIN = 3, WINDVANE_ANALOG_PIN = 3,
WINDVANE_NMEA = 4, WINDVANE_NMEA = 4,
WINDVANE_SITL = 10 WINDVANE_SITL_TRUE = 10,
WINDVANE_SITL_APPARENT = 11,
}; };
enum Speed_type { enum Speed_type {
@ -154,7 +155,8 @@ private:
WINDVANE_WIND_SENSOR_REV_P = 2, WINDVANE_WIND_SENSOR_REV_P = 2,
WINDSPEED_RPM = 3, WINDSPEED_RPM = 3,
WINDSPEED_NMEA = 4, WINDSPEED_NMEA = 4,
WINDSPEED_SITL = 10 WINDSPEED_SITL_TRUE = 10,
WINDSPEED_SITL_APPARENT = 11,
}; };
static AP_WindVane *_singleton; 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() void AP_WindVane_SITL::update_direction()
{ {
// temporarily store true speed and direction for easy access if (_frontend._direction_type == _frontend.WindVaneType::WINDVANE_SITL_TRUE) {
const float wind_speed = AP::sitl()->wind_speed_active; // read in the true wind direction and calculate the apparent
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 // temporarily store true speed and direction for easy access
// This is accounted for in these calculations 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 // Note than the SITL wind direction is defined as the direction the wind is traveling to
Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); // This is accounted for in these calculations
// add vehicle speed to get apparent wind vector // convert true wind speed and direction into a 2D vector
wind_vector_ef.x += AP::sitl()->state.speedN; Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
wind_vector_ef.y += AP::sitl()->state.speedE;
// 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() void AP_WindVane_SITL::update_speed()
{ {
// temporarily store true speed and direction for easy access if (_frontend._speed_sensor_type == _frontend.Speed_type::WINDSPEED_SITL_TRUE) {
const float wind_speed = AP::sitl()->wind_speed_active; // read in the true wind direction and calculate the apparent
const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
// convert true wind speed and direction into a 2D vector // temporarily store true speed and direction for easy access
Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed); 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 // convert true wind speed and direction into a 2D vector
wind_vector_ef.x += AP::sitl()->state.speedN; Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
wind_vector_ef.y += AP::sitl()->state.speedE;
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 #endif