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

AP_NavEKF: use radians values from AHRS directly

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-30 15:09:42 +11:00
parent 6b798d2821
commit 4771a4fc77
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_NavEKF/AP_NavEKF.cpp
View File

@ -60,9 +60,9 @@ void NavEKF::InitialiseFilter(void)
{
// Calculate initial filter quaternion states from ahrs solution
Quaternion initQuat;
ahrsEul[0] = _ahrs.roll_sensor*0.01f*DEG_TO_RAD;
ahrsEul[1] = _ahrs.pitch_sensor*0.01f*DEG_TO_RAD;
ahrsEul[2] = _ahrs.yaw_sensor*0.01f*DEG_TO_RAD;
ahrsEul[0] = _ahrs.roll;
ahrsEul[1] = _ahrs.pitch;
ahrsEul[2] = _ahrs.yaw;
eul2quat(initQuat, ahrsEul);
// Calculate initial Tbn matrix and rotate Mag measurements into NED