dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: Tighten preflight GPS quality checks 

Enable drift checks and reduce thresholds to reduce likelihood of commencing  use of  a unstable GPS position.
Publish drift data to assist with tuning.
This commit is contained in:
Paul Riseborough 2018-07-23 13:26:57 +10:00 committed by Paul Riseborough
parent d0f733d375
commit c2fcef8fc1
2 changed files with 52 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -57,6 +57,7 @@
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
@ -252,6 +253,7 @@ private:
orb_advert_t _att_pub{nullptr};
orb_advert_t _wind_pub{nullptr};
orb_advert_t _estimator_status_pub{nullptr};
orb_advert_t _ekf_gps_drift_pub{nullptr};
orb_advert_t _estimator_innovations_pub{nullptr};
orb_advert_t _ekf2_timestamps_pub{nullptr};
orb_advert_t _sensor_bias_pub{nullptr};
@ -469,7 +471,12 @@ private:
(ParamInt<px4::params::EKF2_GPS_MASK>)
_gps_blend_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_gps_blend_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
_gps_blend_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
// Test used to determine if the vehicle is static or moving
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_is_moving_scaler ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
)
};
@ -570,7 +577,8 @@ Ekf2::Ekf2():
_acc_bias_learn_tc(_params->acc_bias_learn_tc),
_drag_noise(_params->drag_noise),
_bcoef_x(_params->bcoef_x),
_bcoef_y(_params->bcoef_y)
_bcoef_y(_params->bcoef_y),
_is_moving_scaler(_params->is_moving_scaler)
{
_airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@ -1440,6 +1448,26 @@ void Ekf2::run()
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
}
// publish GPS drift data only when updated to minimise overhead
float gps_drift[3];
bool blocked;
if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
ekf_gps_drift_s drift_data;
drift_data.timestamp = now;
drift_data.hpos_drift_rate = gps_drift[0];
drift_data.vpos_drift_rate = gps_drift[1];
drift_data.hspd = gps_drift[2];
drift_data.blocked = blocked;
if (_ekf_gps_drift_pub == nullptr) {
_ekf_gps_drift_pub = orb_advertise(ORB_ID(ekf_gps_drift), &drift_data);
} else {
orb_publish(ORB_ID(ekf_gps_drift), _ekf_gps_drift_pub, &drift_data);
}
}
{
/* Check and save learned magnetometer bias estimates */

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

@ -157,9 +157,9 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5);
* 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
* 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment.
* 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehicle is stationary during alignment.
* 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment.
* 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
* 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
* 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
* 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT
*
* @group EKF2
@ -175,7 +175,7 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5);
* @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT)
* @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT)
*/
PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21);
PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245);
/**
* Required EPH to use GPS.
@ -186,7 +186,7 @@ PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21);
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 5.0f);
PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 3.0f);
/**
* Required EPV to use GPS.
@ -197,7 +197,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 5.0f);
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 8.0f);
PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 5.0f);
/**
* Required speed accuracy to use GPS.
@ -208,7 +208,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 8.0f);
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 1.0f);
PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 0.5f);
/**
* Required satellite count to use GPS.
@ -238,7 +238,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GDOP, 2.5f);
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f);
PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.1f);
/**
* Maximum vertical drift speed to use GPS.
@ -249,7 +249,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f);
* @decimal 2
* @unit m/s
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f);
PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.2f);
/**
* Rate gyro noise for covariance prediction.
@ -489,7 +489,7 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
* If set to 'Magnetic heading' magnetic heading fusion is used at all times
* If set to '3-axis' 3-axis field fusion is used at all times.
* If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
* If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference.
* If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests controlled by the EKF2_MOVE_TEST parameter indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
*
* @group EKF2
* @value 0 Automatic
@ -1301,3 +1301,15 @@ PARAM_DEFINE_INT32(EKF2_GPS_MASK, 0);
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_TAU, 10.0f);
/**
* Vehicle movement test threshold
*
* Scales the threshold tests applied to IMU data used to determine if the vehicle is static or moving. See parameter descriptions for EKF2_GPS_CHECK and EKF2_MAG_TYPE for further information on the functionality affected by this parameter.
*
* @group EKF2
* @min 0.1
* @max 10.0
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_MOVE_TEST, 1.0f);