fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add comprehensive user selectable GPS checks

Browse Source
This commit is contained in:
Paul Riseborough 2016-01-25 00:36:31 +11:00
parent 8d0022ab1e
commit 94a6644684
4 changed files with 239 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -54,6 +54,7 @@ px4_add_module(
EKF/covariance.cpp
EKF/vel_pos_fusion.cpp
EKF/mag_fusion.cpp
EKF/gps_checks.cpp
DEPENDS
platforms__common
)

24
EKF/estimator_base.cpp
View File

@ -36,6 +36,7 @@
* Definition of base class for attitude estimators
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
@ -162,7 +163,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
return;
}
if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) {
if (time_usec - _time_last_gps > 70000) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
@ -320,27 +321,6 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
}
}
bool EstimatorBase::gps_is_good(struct gps_message *gps)
{
// go through apm implementation of calcGpsGoodToAlign for fancier checks
// Use a stricter check for initialisation than during flight to avoid complete loss of GPS
if (_gps_initialised) {
if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) {
return true;
} else {
return false;
}
} else {
if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
return true;
} else {
return false;
}
}
}
bool EstimatorBase::position_is_valid()
{
// return true if the position estimate is valid

67
EKF/estimator_base.h
View File

@ -46,16 +46,19 @@
struct gps_message {
uint64_t time_usec;
int32_t lat; // Latitude in 1E-7 degrees
int32_t lon; // Longitude in 1E-7 degrees
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
float eph; // GPS HDOP horizontal dilution of position in m
float epv; // GPS VDOP horizontal dilution of position in m
uint64_t time_usec_vel; // Timestamp for velocity informations
float vel_m_s; // GPS ground speed (m/s)
float vel_ned[3]; // GPS ground speed NED
bool vel_ned_valid; // GPS ground speed is valid
int32_t lat; // Latitude in 1E-7 degrees
int32_t lon; // Longitude in 1E-7 degrees
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
float eph; // GPS horizontal position accuracy in m
float epv; // GPS vertical position accuracy in m
float sacc; // GPS speed accuracy in m/s
uint64_t time_usec_vel; // Timestamp for velocity informations
float vel_m_s; // GPS ground speed (m/s)
float vel_ned[3]; // GPS ground speed NED
bool vel_ned_valid; // GPS ground speed is valid
uint8_t nsats; // number of satellites used
float gdop; // geometric dilution of precision
};
struct parameters {
@ -63,8 +66,6 @@ struct parameters {
float baro_delay_ms = 0.0f;
float gps_delay_ms = 200.0f;
float airspeed_delay_ms = 200.0f;
float requiredEph = 10.0f;
float requiredEpv = 20.0f;
// input noise
float gyro_noise = 0.001f;
@ -84,6 +85,17 @@ struct parameters {
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees
float heading_innov_gate = 0.5f; // innovation gate for heading innovation test
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float requiredEph = 5.0f; // maximum acceptable horizontal position error
float requiredEpv = 8.0f; // maximum acceptable vertical position error
float requiredSacc = 1.0f; // maximum acceptable speed error
int requiredNsats = 6; // minimum acceptable satellite count
float requiredGDoP = 2.0f; // maximum acceptable geometric dilution of precision
float requiredHdrift = 0.3f; // maximum acceptable horizontal drift speed
float requiredVdrift = 0.5f; // maximum acceptable vertical drift speed
};
class EstimatorBase
@ -281,6 +293,15 @@ protected:
bool gps_is_good(struct gps_message *gps);
// variables used for the GPS quality checks
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
float _gpsDriftVelD = 0.0f; // GPS down position derivative (m/s)
float _gpsVertVelFilt = 0.0f; // GPS filtered Down velocity (m/s)
float _gpsVelNorthFilt = 0.0f; // GPS filtered North velocity (m/s)
float _gpsVelEastFilt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _lastGpsFail_us = 0; // last system time in usec that the GPS failed it's checks
public:
void printIMU(struct imuSample *data);
void printStoredIMU();
@ -320,5 +341,25 @@ public:
uint64_t _last_gps_origin_time_us = 0;
struct map_projection_reference_s _posRef = {};
float _gps_alt_ref = 0.0f;
float _gps_alt_ref = 0.0f;
bool _vehicleArmed = false; // vehicle arm status used to turn off funtionality used on the ground
// publish the status of various GPS quality checks
union gpsCheckFailStatus_u {
struct {
uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
} flags;
uint16_t value;
};
gpsCheckFailStatus_u _gpsCheckFailStatus;
};

182
EKF/gps_checks.cpp Normal file
View File

@ -0,0 +1,182 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gps_checks.cpp
* Perform pre-flight and in-flight GPS quality checks
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "ekf.h"
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_GDOP (1<<1)
#define MASK_GPS_HACC (1<<2)
#define MASK_GPS_VACC (1<<3)
#define MASK_GPS_SACC (1<<4)
#define MASK_GPS_HDRIFT (1<<5)
#define MASK_GPS_VDRIFT (1<<6)
#define MASK_GPS_HSPD (1<<7)
#define MASK_GPS_VSPD (1<<8)
/*
* Return true if the GPS solution quality is adequate to set an origin for the EKF
* and start GPS aiding.
* All activated checks must pass for 10 seconds.
* Checks are activated using the EKF2_GPS_CHECKS bitmask parameter
* Checks are adjusted using the EKF2_REQ_* parameters
*/
bool EstimatorBase::gps_is_good(struct gps_message *gps)
{
// Check the number of satellites
_gpsCheckFailStatus.flags.nsats = (gps->nsats < _params.requiredNsats);
// Check the geometric dilution of precision
_gpsCheckFailStatus.flags.gdop = (gps->gdop > _params.requiredGDoP);
// Check the reported horizontal position accuracy
_gpsCheckFailStatus.flags.hacc = (gps->eph > _params.requiredEph);
// Check the reported vertical position accuracy
_gpsCheckFailStatus.flags.vacc = (gps->epv > _params.requiredEpv);
// Check the reported speed accuracy
_gpsCheckFailStatus.flags.sacc = (gps->eph > _params.requiredEph);
// Calculate position movement since last measurement
float deltaPosN = 0.0f;
float deltaPosE = 0.0f;
double lat = gps->lat * 1.0e-7;
double lon = gps->lon * 1.0e-7;
if (_posRef.init_done) {
map_projection_project(&_posRef, lat, lon, &deltaPosN, &deltaPosE);
} else {
map_projection_init(&_posRef, lat, lon);
_gps_alt_ref = gps->alt * 1e-3f;
}
// Calculate time lapsesd since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
const float filtTimeConst = 10.0f;
float dt = fminf(fmaxf(float(hrt_absolute_time() - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
float filterCoef = dt/filtTimeConst;
// Calculate the horizontal drift velocity components and limit to 10x the threshold
float velLimit = 10.0f * _params.requiredHdrift;
float velN = fminf(fmaxf(deltaPosN / dt, -velLimit), velLimit);
float velE = fminf(fmaxf(deltaPosE / dt, -velLimit), velLimit);
// Apply a low pass filter
_gpsDriftVelN = velN * filterCoef + _gpsDriftVelN * (1.0f - filterCoef);
_gpsDriftVelE = velE * filterCoef + _gpsDriftVelE * (1.0f - filterCoef);
// Calculate the horizontal drift speed and fail if too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicleArmed) {
float driftSpeed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
_gpsCheckFailStatus.flags.hdrift = (driftSpeed > _params.requiredHdrift);
} else {
_gpsCheckFailStatus.flags.hdrift = false;
}
// Save current position as the reference for next time
map_projection_init(&_posRef, lat, lon);
_last_gps_origin_time_us = hrt_absolute_time();
// Calculate the vertical drift velocity and limit to 10x the threshold
velLimit = 10.0f * _params.requiredVdrift;
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -velLimit), velLimit);
// Save the current height as the reference for next time
_gps_alt_ref = gps->alt * 1e-3f;
// Apply a low pass filter to the vertical velocity
_gpsDriftVelD = velD * filterCoef + _gpsDriftVelD * (1.0f - filterCoef);
// Fail if the vertical drift speed is too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicleArmed) {
_gpsCheckFailStatus.flags.vdrift = (fabsf(_gpsDriftVelD) > _params.requiredVdrift);
} else {
_gpsCheckFailStatus.flags.vdrift = false;
}
// Check the magnitude of the filtered horizontal GPS velocity
// This check can only be used if the vehicle is stationary during alignment
if (_vehicleArmed) {
velLimit = 10.0f * _params.requiredHdrift;
float velN = fminf(fmaxf(gps->vel_ned[0],-velLimit),velLimit);
float velE = fminf(fmaxf(gps->vel_ned[1],-velLimit),velLimit);
_gpsVelNorthFilt = velN * filterCoef + _gpsVelNorthFilt * (1.0f - filterCoef);
_gpsVelEastFilt = velE * filterCoef + _gpsVelEastFilt * (1.0f - filterCoef);
float horizSpeed = sqrtf(_gpsVelNorthFilt * _gpsVelNorthFilt + _gpsVelEastFilt * _gpsVelEastFilt);
_gpsCheckFailStatus.flags.hspeed = (horizSpeed > _params.requiredHdrift);
} else {
_gpsCheckFailStatus.flags.hspeed = false;
}
// Check the filtered difference between GPS and EKF vertical velocity
velLimit = 10.0f * _params.requiredVdrift;
float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -velLimit), velLimit);
_gpsVertVelFilt = vertVel * filterCoef + _gpsVertVelFilt * (1.0f - filterCoef);
_gpsCheckFailStatus.flags.vspeed = (fabsf(_gpsVertVelFilt) > _params.requiredVdrift);
// assume failed first time through
if (_lastGpsFail_us == 0) {
_lastGpsFail_us = hrt_absolute_time();
}
// if any user selected checks have failed, record the fail time
if (
(_gpsCheckFailStatus.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
(_gpsCheckFailStatus.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
(_gpsCheckFailStatus.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||
(_gpsCheckFailStatus.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) ||
(_gpsCheckFailStatus.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) ||
(_gpsCheckFailStatus.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
(_gpsCheckFailStatus.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
(_gpsCheckFailStatus.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
(_gpsCheckFailStatus.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
) {
_lastGpsFail_us = hrt_absolute_time();
}
// continuous period without fail of 10 seconds required to return a healthy status
if (hrt_absolute_time() - _lastGpsFail_us > 1e7) {
return true;
}
return false;
}