SITL: fixed tailsitter airspeed in RF9

This commit is contained in:
Andrew Tridgell 2022-09-13 08:40:39 +10:00
parent 84074d68f6
commit d06600aca1

View File

@ -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);