fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

style and format clean

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 2cc193c173
commit efe312d4a6
5 changed files with 23 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -4
View File
@@ -1178,13 +1178,15 @@ void Ekf2::run()
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
}
// orientation measurement error from parameters
if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18], ev_odom.pose_covariance[20]))));
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18],
ev_odom.pose_covariance[20]))));
} else {
ev_data.angErr = _ev_ang_noise.get();
}
else {
ev_data.angErr = _ev_ang_noise.get();
}
// only set data if all positions are valid
if (sqrtf(ev_odom.pose_covariance[0]) < ep_max_std_dev && sqrtf(ev_odom.pose_covariance[6]) < ep_max_std_dev) {