mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
SITL: fixed tailsitter airspeed in RF9
This commit is contained in:
parent
84074d68f6
commit
d06600aca1
@ -494,7 +494,7 @@ void FlightAxis::update(const struct sitl_input &input)
|
|||||||
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) {
|
||||||
airspeed3d = sitl->ahrs_rotation_inv * airspeed3d;
|
airspeed3d = sitl->ahrs_rotation * airspeed3d;
|
||||||
}
|
}
|
||||||
airspeed_pitot = MAX(airspeed3d.x,0);
|
airspeed_pitot = MAX(airspeed3d.x,0);
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user