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

SITL: fill in wind from flightaxis

This commit is contained in:
Andrew Tridgell 2022-07-30 11:45:27 +10:00
parent 77aefa4342
commit 5aff248950
3 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SITL/SIM_Aircraft.cpp
View File

@ -399,6 +399,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.wind_vane_apparent.direction = wind_vane_apparent.direction;
fdm.wind_vane_apparent.speed = wind_vane_apparent.speed;
fdm.wind_ef = wind_ef;
if (is_smoothed) {
fdm.xAccel = smoothing.accel_body.x;
fdm.yAccel = smoothing.accel_body.y;

4
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -489,8 +489,8 @@ void FlightAxis::update(const struct sitl_input &input)
can't get that from m_airspeed_MPS, so instead we calculate it
from wind vector and ground speed
*/
Vector3f m_wind_ef(-state.m_windY_MPS,-state.m_windX_MPS,-state.m_windZ_MPS);
Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef;
wind_ef = Vector3f(state.m_windY_MPS,state.m_windX_MPS,state.m_windZ_MPS);
Vector3f airspeed_3d_ef = velocity_ef - wind_ef;
Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
if (last_imu_rotation != ROTATION_NONE) {

3
libraries/SITL/SITL.h
View File

@ -86,6 +86,9 @@ struct sitl_fdm {
} wind_vane_apparent;
bool is_lock_step_scheduled;
// earthframe wind, from backends that know it
Vector3f wind_ef;
};
// number of rc output channels