aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS_DCM: changed estimate to use prev values for dir & vel

Browse Source
This commit is contained in:
masierra 2015-09-16 10:30:13 -07:00 committed by Tom Pittenger
parent ee29f38cdf
commit 8fc494cfcd
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -907,12 +907,12 @@ void AP_AHRS_DCM::estimate_wind(void)
// estimate airspeed it using equation 6 // estimate airspeed it using equation 6
V = velocityDiff.length() / diff_length; V = velocityDiff.length() / diff_length;
_last_fuse = fuselageDirection;
_last_vel = velocity;
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel; Vector3f velocitySum = velocity + _last_vel;
_last_fuse = fuselageDirection;
_last_vel = velocity;
float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x); float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sinf(theta); float sintheta = sinf(theta);
float costheta = cosf(theta); float costheta = cosf(theta);