fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 01:41:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix incorrect use of double precision operation

Browse Source
This commit is contained in:
Paul Riseborough
2017-08-07 10:34:19 +10:00
parent cc5db74d1b
commit 35ffd55481
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+6 -6
View File
@@ -537,12 +537,12 @@ void Ekf::fuseHeading()
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m // Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
Dcmf R_to_earth; Dcmf R_to_earth;
float sy = sin(yaw); float sy = sinf(yaw);
float cy = cos(yaw); float cy = cosf(yaw);
float sp = sin(pitch); float sp = sinf(pitch);
float cp = cos(pitch); float cp = cosf(pitch);
float sr = sin(roll); float sr = sinf(roll);
float cr = cos(roll); float cr = cosf(roll);
R_to_earth(0,0) = cy*cp-sy*sp*sr; R_to_earth(0,0) = cy*cp-sy*sp*sr;
R_to_earth(0,1) = -sy*cr; R_to_earth(0,1) = -sy*cr;
R_to_earth(0,2) = cy*sp+sy*cp*sr; R_to_earth(0,2) = cy*sp+sy*cp*sr;