Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SITL: added SIM_WIND_TC #27083

Merged
merged 2 commits into from
May 20, 2024
Merged

SITL: added SIM_WIND_TC #27083

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d0b9c3a
SITL: added SIM_WIND_TC
tridge May 17, 2024
f58a358
HAL_SITL: implement wind rate of change
tridge Jul 16, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 15 additions & 3 deletions libraries/AP_HAL_SITL/SITL_State.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -327,9 +327,21 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
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);
uint32_t dt_us = now - last_wind_update_us;
if (dt_us > 1000) {
last_wind_update_us = now;
// slew wind based on the configured time constant
const float dt = dt_us * 1.0e-6;
const float tc = MAX(_sitl->wind_change_tc, 0.1);
const float alpha = calc_lowpass_alpha_dt(dt, 1.0/tc);
_sitl->wind_speed_active += (_sitl->wind_speed - _sitl->wind_speed_active) * alpha;
_sitl->wind_direction_active += (wrap_180(_sitl->wind_direction - _sitl->wind_direction_active)) * alpha;
_sitl->wind_dir_z_active += (_sitl->wind_dir_z - _sitl->wind_dir_z_active) * alpha;
_sitl->wind_direction_active = wrap_180(_sitl->wind_direction_active);
}
wind_speed = _sitl->wind_speed_active;
wind_direction = _sitl->wind_direction_active;
wind_dir_z = _sitl->wind_dir_z_active;

// pass wind into simulators using different wind types via param SIM_WIND_T*.
switch (_sitl->wind_type) {
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_HAL_SITL/SITL_State.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ class HALSITL::SITL_State : public SITL_State_Common {
uint32_t time_delta_wind;
uint32_t delayed_time_wind;
uint32_t wind_start_delay_micros;
uint32_t last_wind_update_us;

// simulated GPS devices
SITL::GPS *gps[2]; // constrained by # of parameter sets
Expand Down
8 changes: 8 additions & 0 deletions libraries/SITL/SITL.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,21 @@ const AP_Param::GroupInfo SIM::var_info[] = {
// @Units: deg
// @User: Advanced
AP_GROUPINFO("WIND_DIR", 10, SIM, wind_direction, 180),

// @Param: WIND_TURB
// @DisplayName: Simulated Wind variation
// @Description: Allows you to emulate random wind variations in sim
// @Units: m/s
// @User: Advanced
AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0),

// @Param: WIND_TC
// @DisplayName: Wind variation time constant
// @Description: this controls the time over which wind changes take effect
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

normally we don't need the "this controls" at the front because all parameters control something.

// @Units: s
// @User: Advanced
AP_GROUPINFO("WIND_TC", 12, SIM, wind_change_tc, 5),

// @Group: SERVO_
// @Path: ./ServoModel.cpp
AP_SUBGROUPINFO(servo, "SERVO_", 16, SIM, ServoParams),
Expand Down
1 change: 1 addition & 0 deletions libraries/SITL/SITL.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -349,6 +349,7 @@ class SIM {
AP_Float wind_direction;
AP_Float wind_turbulance;
AP_Float wind_dir_z;
AP_Float wind_change_tc;
AP_Int8 wind_type; // enum WindLimitType
AP_Float wind_type_alt;
AP_Float wind_type_coef;
Expand Down
Loading