From 09c8c8f706c5f6df3d42ee968f9c9d52cbf2e25a Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Nov 2019 16:38:51 +0200 Subject: [PATCH] ekf: use PDOP instead of GDOP as TDOP (part of GDOP) is not given by the GNSS receiver --- Tools/ecl_ekf/analysis/post_processing.py | 4 ++-- Tools/ecl_ekf/plotting/pdf_report.py | 4 ++-- msg/estimator_status.msg | 2 +- src/lib/ecl | 2 +- .../PreFlightCheck/checks/ekf2Check.cpp | 4 ++-- src/modules/ekf2/ekf2_main.cpp | 21 +++++++++---------- src/modules/ekf2/ekf2_params.c | 8 +++---- 7 files changed, 22 insertions(+), 23 deletions(-) diff --git a/Tools/ecl_ekf/analysis/post_processing.py b/Tools/ecl_ekf/analysis/post_processing.py index 55499c34cb..3a367c4bd6 100644 --- a/Tools/ecl_ekf/analysis/post_processing.py +++ b/Tools/ecl_ekf/analysis/post_processing.py @@ -104,7 +104,7 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict: # 0 : insufficient fix type (no 3D solution) # 1 : minimum required sat count fail - # 2 : minimum required GDoP fail + # 2 : minimum required PDOP fail # 3 : maximum allowed horizontal position error fail # 4 : maximum allowed vertical position error fail # 5 : maximum allowed speed error fail @@ -114,7 +114,7 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict: # 9 : maximum allowed vertical velocity discrepancy fail gps_fail_flags['gfix_fail'] = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1 gps_fail_flags['nsat_fail'] = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1 - gps_fail_flags['gdop_fail'] = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['pdop_fail'] = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1 gps_fail_flags['herr_fail'] = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1 gps_fail_flags['verr_fail'] = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1 gps_fail_flags['serr_fail'] = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1 diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index d57d944f1a..41da608b05 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -219,11 +219,11 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # gps_check_fail_flags summary data_plot = CheckFlagsPlot( status_time, gps_fail_flags, - [['nsat_fail', 'gdop_fail', 'herr_fail', 'verr_fail', 'gfix_fail', 'serr_fail'], + [['nsat_fail', 'pdop_fail', 'herr_fail', 'verr_fail', 'gfix_fail', 'serr_fail'], ['hdrift_fail', 'vdrift_fail', 'hspd_fail', 'veld_diff_fail']], x_label='time (sec)', y_lim=(-0.1, 1.1), y_labels=['failed', 'failed'], sub_titles=['GPS Direct Output Check Failures', 'GPS Derived Output Check Failures'], - legend=[['N sats', 'GDOP', 'horiz pos error', 'vert pos error', 'fix type', + legend=[['N sats', 'PDOP', 'horiz pos error', 'vert pos error', 'fix type', 'speed error'], ['horiz drift', 'vert drift', 'horiz speed', 'vert vel inconsistent']], annotate=False, pdf_handle=pdf_pages) data_plot.save() diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 7c0a1869ea..3f4c6d3741 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -13,7 +13,7 @@ uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see # bits are true when corresponding test has failed uint8 GPS_CHECK_FAIL_GPS_FIX = 0 # 0 : insufficient fix type (no 3D solution) uint8 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1 # 1 : minimum required sat count fail -uint8 GPS_CHECK_FAIL_MIN_GDOP = 2 # 2 : minimum required GDoP fail +uint8 GPS_CHECK_FAIL_MIN_PDOP = 2 # 2 : minimum required PDOP fail uint8 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3 # 3 : maximum allowed horizontal position error fail uint8 GPS_CHECK_FAIL_MAX_VERT_ERR = 4 # 4 : maximum allowed vertical position error fail uint8 GPS_CHECK_FAIL_MAX_SPD_ERR = 5 # 5 : maximum allowed speed error fail diff --git a/src/lib/ecl b/src/lib/ecl index 811ec81246..6b5f011bc2 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 811ec812469e0b60365867738eea9b20c06e227b +Subproject commit 6b5f011bc224a45fd57d33e90cc7a136aa075b6c diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index e6cd17fc1f..60b7683ac4 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -186,8 +186,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { message = "Preflight%s: not enough GPS Satellites"; - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) { - message = "Preflight%s: GPS GDoP too low"; + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_PDOP)) { + message = "Preflight%s: GPS PDOP too low"; } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { message = "Preflight%s: GPS Horizontal Pos Error too high"; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 6ace555721..c9f4542ce2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -375,8 +375,8 @@ private: (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count - (ParamExtFloat) - _param_ekf2_req_gdop, ///< maximum acceptable geometric dilution of precision + (ParamExtFloat) + _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision (ParamExtFloat) _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) @@ -581,7 +581,7 @@ Ekf2::Ekf2(bool replay_mode): _param_ekf2_req_epv(_params->req_vacc), _param_ekf2_req_sacc(_params->req_sacc), _param_ekf2_req_nsats(_params->req_nsats), - _param_ekf2_req_gdop(_params->req_gdop), + _param_ekf2_req_pdop(_params->req_pdop), _param_ekf2_req_hdrift(_params->req_hdrift), _param_ekf2_req_vdrift(_params->req_vdrift), _param_ekf2_aid_mask(_params->fusion_mode), @@ -1661,8 +1661,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p msg.vel_ned[2] = data.vel_d_m_s; msg.vel_ned_valid = data.vel_ned_valid; msg.nsats = data.satellites_used; - const float tdop = 0.9f; // TDOP is usually never worse than 0.9 - msg.gdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop + tdop * tdop); + msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop); } void Ekf2::runPreFlightChecks(const float dt, @@ -2032,7 +2031,7 @@ void Ekf2::update_gps_blend_states() _gps_blended_state.vel_ned[2] = 0.0f; _gps_blended_state.vel_ned_valid = true; _gps_blended_state.nsats = 0; - _gps_blended_state.gdop = FLT_MAX; + _gps_blended_state.pdop = FLT_MAX; _blended_antenna_offset.zero(); @@ -2071,9 +2070,9 @@ void Ekf2::update_gps_blend_states() _gps_blended_state.sacc = _gps_state[i].sacc; } - if (_gps_state[i].gdop > 0 - && _gps_state[i].gdop < _gps_blended_state.gdop) { - _gps_blended_state.gdop = _gps_state[i].gdop; + if (_gps_state[i].pdop > 0 + && _gps_state[i].pdop < _gps_blended_state.pdop) { + _gps_blended_state.pdop = _gps_state[i].pdop; } if (_gps_state[i].nsats > 0 @@ -2257,7 +2256,7 @@ void Ekf2::apply_gps_offsets() _gps_output[i].eph = _gps_state[i].eph; _gps_output[i].epv = _gps_state[i].epv; _gps_output[i].sacc = _gps_state[i].sacc; - _gps_output[i].gdop = _gps_state[i].gdop; + _gps_output[i].pdop = _gps_state[i].pdop; _gps_output[i].nsats = _gps_state[i].nsats; _gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid; _gps_output[i].yaw = _gps_state[i].yaw; @@ -2319,7 +2318,7 @@ void Ekf2::calc_gps_blend_output() _gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph; _gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv; _gps_output[GPS_BLENDED_INSTANCE].sacc = _gps_blended_state.sacc; - _gps_output[GPS_BLENDED_INSTANCE].gdop = _gps_blended_state.gdop; + _gps_output[GPS_BLENDED_INSTANCE].pdop = _gps_blended_state.pdop; _gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats; _gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid; _gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index c48b4df1c4..cc773de842 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -153,7 +153,7 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5); * * Set bits to 1 to enable checks. Checks enabled by the following bit positions * 0 : Minimum required sat count set by EKF2_REQ_NSATS - * 1 : Minimum required GDoP set by EKF2_REQ_GDOP + * 1 : Minimum required PDOP set by EKF2_REQ_PDOP * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV * 4 : Maximum allowed speed error set by EKF2_REQ_SACC @@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5); * @min 0 * @max 511 * @bit 0 Min sat count (EKF2_REQ_NSATS) - * @bit 1 Min GDoP (EKF2_REQ_GDOP) + * @bit 1 Min PDOP (EKF2_REQ_PDOP) * @bit 2 Max horizontal position error (EKF2_REQ_EPH) * @bit 3 Max vertical position error (EKF2_REQ_EPV) * @bit 4 Max speed error (EKF2_REQ_SACC) @@ -220,14 +220,14 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 0.5f); PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6); /** - * Required GDoP to use GPS. + * Required PDOP to use GPS. * * @group EKF2 * @min 1.5 * @max 5.0 * @decimal 1 */ -PARAM_DEFINE_FLOAT(EKF2_REQ_GDOP, 2.5f); +PARAM_DEFINE_FLOAT(EKF2_REQ_PDOP, 2.5f); /** * Maximum horizontal drift speed to use GPS.