fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:30:34 +08:00
Code Issues 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
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+2 -2
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";
src/modules/ekf2/ekf2_main.cpp
+10 -11
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;
src/modules/ekf2/ekf2_params.c
+4 -4
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.