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

AP_HAL_SITL: add wind type parameters

This commit is contained in:
Eugene Shamaev 2018-04-30 08:26:15 -07:00 committed by Tom Pittenger
parent d086edc762
commit 5d4579a086
2 changed files with 27 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -359,24 +359,44 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
uint32_t now = AP_HAL::micros();
last_update_usec = now;
// pass wind into simulators, using a wind gradient below 60m
float altitude = _barometer?_barometer->get_altitude():0;
float wind_speed = 0;
float wind_direction = 0;
float wind_dir_z = 0;
if (_sitl) {
// give 5 seconds to calibrate airspeed sensor at 0 wind speed
if (wind_start_delay_micros == 0) {
wind_start_delay_micros = now;
} else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) {
// The EKF does not like step inputs so this LPF keeps it happy.
wind_speed = _sitl->wind_speed_active = (0.95f*_sitl->wind_speed_active) + (0.05f*_sitl->wind_speed);
wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction);
wind_dir_z = _sitl->wind_dir_z_active = (0.95f*_sitl->wind_dir_z_active) + (0.05f*_sitl->wind_dir_z);
}
// pass wind into simulators using different wind types via param SIM_WIND_T*.
switch (_sitl->wind_type) {
case SITL::SITL::WIND_TYPE_SQRT:
if (altitude < _sitl->wind_type_alt) {
wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
}
break;
case SITL::SITL::WIND_TYPE_COEF:
wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
break;
case SITL::SITL::WIND_TYPE_NO_LIMIT:
default:
break;
}
// never allow negative wind velocity
wind_speed = MAX(wind_speed, 0);
}
if (altitude < 0) {
altitude = 0;
}
if (altitude < 60) {
wind_speed *= sqrtf(MAX(altitude / 60, 0));
}
input.wind.speed = wind_speed;
input.wind.direction = wind_direction;

1
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -202,6 +202,7 @@ private:
VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
uint32_t time_delta_wind;
uint32_t delayed_time_wind;
uint32_t wind_start_delay_micros;
// internal SITL model
SITL::Aircraft *sitl_model;