Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-27 09:33:51 +08:00
Code Issues Actions 11 Packages Projects Releases Wiki Activity

ekf: use PDOP instead of GDOP as TDOP (part of GDOP) is not given by the GNSS receiver

Browse Source
This commit is contained in:
bresch
2019-11-25 16:38:51 +02:00
committed by Mathieu Bresciani
parent 20705e3c53
commit 09c8c8f706
7 changed files with 22 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/ecl_ekf/analysis/post_processing.py
View File

@@ -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

4
Tools/ecl_ekf/plotting/pdf_report.py
View File

@@ -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()

2
msg/estimator_status.msg
View File

@@ -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

2
src/lib/ecl

Submodule src/lib/ecl updated: 811ec81246...6b5f011bc2

4
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
View File

@@ -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";

21
src/modules/ekf2/ekf2_main.cpp
View File

@@ -375,8 +375,8 @@ private:
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_GDOP>)
_param_ekf2_req_gdop, ///< maximum acceptable geometric dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
_param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
_param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _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;

8
src/modules/ekf2/ekf2_params.c
View File

@@ -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.