Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed sign of wind with AHRS_EKF_TYPE=10 and builtin models

Browse Source 
an alternative to #21929 that doesn't break RealFlight

needs testing with sailboats
This commit is contained in:
Andrew Tridgell 2022-11-29 09:36:25 +11:00 committed by Peter Barker
parent 353eec0a1b
commit eed6edc3b0
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/SITL/SIM_Aircraft.cpp
View File

@ -641,7 +641,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
position += (velocity_ef * delta_time).todouble();
// velocity relative to air mass, in earth frame
velocity_air_ef = velocity_ef + wind_ef;
velocity_air_ef = velocity_ef - wind_ef;
// velocity relative to airmass in body frame
velocity_air_bf = dcm.transposed() * velocity_air_ef;
@ -785,6 +785,9 @@ void Aircraft::update_wind(const struct sitl_input &input)
sinf(radians(turbulence_azimuth)) * turbulence_horizontal_speed,
turbulence_vertical_speed);
}
// the AHRS wants wind with opposite sense
wind_ef = -wind_ef;
}
/*
@ -937,7 +940,7 @@ void Aircraft::extrapolate_sensors(float delta_time)
// new velocity and position vectors
velocity_ef += accel_earth * delta_time;
position += (velocity_ef * delta_time).todouble();
velocity_air_ef = velocity_ef + wind_ef;
velocity_air_ef = velocity_ef - wind_ef;
velocity_air_bf = dcm.transposed() * velocity_air_ef;
}

2
libraries/SITL/SIM_Sailboat.cpp
View File

@ -184,7 +184,7 @@ void Sailboat::update(const struct sitl_input &input)
// calculate apparent wind in earth-frame (this is the direction the wind is coming from)
// Note than the SITL wind direction is defined as the direction the wind is travelling to
// This is accounted for in these calculations
Vector3f wind_apparent_ef = wind_ef + velocity_ef;
Vector3f wind_apparent_ef = velocity_ef - wind_ef;
const float wind_apparent_dir_ef = degrees(atan2f(wind_apparent_ef.y, wind_apparent_ef.x));
const float wind_apparent_speed = safe_sqrt(sq(wind_apparent_ef.x)+sq(wind_apparent_ef.y));