diff --git a/CMakeLists.txt b/CMakeLists.txt index 6633b43867..368c0e651c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ) diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index c720750139..b35e01546b 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -36,6 +36,7 @@ * Definition of base class for attitude estimators * * @author Roman Bast + * @author Paul Riseborough * */ @@ -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 diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index bd716601b3..3aa7ed219d 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -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; + }; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp new file mode 100644 index 0000000000..cefa03ea60 --- /dev/null +++ b/EKF/gps_checks.cpp @@ -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 + * + */ + +#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; +} +