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

SITL: include ahrs_orientation in airspeed calculation

Browse Source
This commit is contained in:
Mark Whitehorn 2019-12-10 12:03:51 -07:00 committed by Andrew Tridgell
parent fc16c5c2ae
commit ab33679124
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -408,7 +408,7 @@ void FlightAxis::update(const struct sitl_input &input)
} }
/* /*
the queternion convention in realflight seems to have Z negative the quaternion convention in realflight seems to have Z negative
*/ */
Quaternion quat(state.m_orientationQuaternion_W, Quaternion quat(state.m_orientationQuaternion_W,
state.m_orientationQuaternion_Y, state.m_orientationQuaternion_Y,
@ -453,13 +453,22 @@ void FlightAxis::update(const struct sitl_input &input)
airspeed = state.m_airspeed_MPS; airspeed = state.m_airspeed_MPS;
/* for pitot airspeed we need the airspeed along the X axis. We /* for pitot airspeed we need the airspeed along the X axis. We
can't get that from m_airspeed_MPS, so instead we canculate 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); 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; Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef;
Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef); Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
if (imu_rotation != ROTATION_NONE) {
Matrix3f rot;
rot.from_rotation(imu_rotation);
airspeed3d = airspeed3d * rot.transposed();
}
}
airspeed_pitot = MAX(airspeed3d.x,0); airspeed_pitot = MAX(airspeed3d.x,0);
#if 0 #if 0