From 5d6e0587db76dbcce68d0c5fcb6c3f1c85badf17 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 13 Nov 2019 18:15:07 +0100 Subject: [PATCH] ekf_main: fill gps gdop topic with pdop (combination of hdop and vdop) --- src/modules/ekf2/ekf2_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 9da92a588d..9a080fb582 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -944,8 +944,8 @@ void Ekf2::Run() _gps_state[0].vel_ned[2] = gps.vel_d_m_s; _gps_state[0].vel_ned_valid = gps.vel_ned_valid; _gps_state[0].nsats = gps.satellites_used; - //TODO: add gdop to gps topic - _gps_state[0].gdop = 0.0f; + const float tdop = 0.9f; + _gps_state[0].gdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop + tdop * tdop); _gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid; ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); @@ -975,8 +975,8 @@ void Ekf2::Run() _gps_state[1].vel_ned[2] = gps.vel_d_m_s; _gps_state[1].vel_ned_valid = gps.vel_ned_valid; _gps_state[1].nsats = gps.satellites_used; - //TODO: add gdop to gps topic - _gps_state[1].gdop = 0.0f; + const float tdop = 0.9f; + _gps_state[0].gdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop + tdop * tdop); _gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid; } }