AP_AHRS: fixed synthetic airspeed to be along +ve X axis

this prevents us from thinking we have +ve airspeed when flying
backwards with no pitot tube
This commit is contained in:
Andrew Tridgell 2018-09-14 10:49:00 +10:00
parent 2215913b02
commit de02cb9c8b
1 changed files with 8 additions and 2 deletions

View File

@ -674,8 +674,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
// rotate vector to body frame
const Matrix3f &rot = get_rotation_body_to_ned();
airspeed = rot.mul_transpose(airspeed);
// take positive component in X direction. This mimics a pitot
// tube
_last_airspeed = MAX(airspeed.x, 0);
}
if (have_gps()) {