Browse Source

SITL: fill in wind from flightaxis

apm_2208
Andrew Tridgell 3 years ago
parent
commit
8cc5172ff3
  1. 2
      libraries/SITL/SIM_Aircraft.cpp
  2. 4
      libraries/SITL/SIM_FlightAxis.cpp
  3. 3
      libraries/SITL/SITL.h

2
libraries/SITL/SIM_Aircraft.cpp

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

4
libraries/SITL/SIM_FlightAxis.cpp

@ -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 can't get that from m_airspeed_MPS, so instead we calculate it
from wind vector and ground speed from wind vector and ground speed
*/ */
Vector3f m_wind_ef(-state.m_windY_MPS,-state.m_windX_MPS,-state.m_windZ_MPS); wind_ef = Vector3f(state.m_windY_MPS,state.m_windX_MPS,state.m_windZ_MPS);
Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef; Vector3f airspeed_3d_ef = velocity_ef - wind_ef;
Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef); Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
if (last_imu_rotation != ROTATION_NONE) { if (last_imu_rotation != ROTATION_NONE) {

3
libraries/SITL/SITL.h

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

Loading…
Cancel
Save