From 39eef3a2d7518155dc49474c0b08e4305bdcf419 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 28 Jan 2016 08:37:45 +1100 Subject: [PATCH] EKF: Remove use of camel case variable names Also fixes bug in GPS speed accuracy check that was using horizontal position accuracy variable by mistake. --- EKF/ekf.cpp | 2 +- EKF/estimator_base.cpp | 4 +- EKF/estimator_base.h | 34 ++++++------- EKF/gps_checks.cpp | 110 ++++++++++++++++++++--------------------- 4 files changed, 75 insertions(+), 75 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 49b8910e7d..4186f751f8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -206,7 +206,7 @@ void Ekf::predictState() { if (!_earth_rate_initialised) { if (_gps_initialised) { - calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad); + calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); _earth_rate_initialised = true; } } diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 5ae5cb469f..3d1c1aee2a 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -181,7 +181,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) float lpos_x = 0.0f; float lpos_y = 0.0f; - map_projection_project(&_posRef, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); + map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); gps_sample_new.pos(0) = lpos_x; gps_sample_new.pos(1) = lpos_y; gps_sample_new.hgt = gps->alt / 1e3f; @@ -312,7 +312,7 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps) // Initialise projection double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; - map_projection_init(&_posRef, lat, lon); + map_projection_init(&_pos_ref, lat, lon); _gps_alt_ref = gps->alt / 1e3f; _gps_initialised = true; _last_gps_origin_time_us = _time_last_imu; diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index b7b0a96453..4edee4c81d 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -88,14 +88,14 @@ struct parameters { // 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 + int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used + float req_hacc = 5.0f; // maximum acceptable horizontal position error + float req_vacc = 8.0f; // maximum acceptable vertical position error + float req_sacc = 1.0f; // maximum acceptable speed error + int req_nsats = 6; // minimum acceptable satellite count + float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision + float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed + float req_vdrift = 0.5f; // maximum acceptable vertical drift speed }; class EstimatorBase @@ -294,11 +294,11 @@ protected: // 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 + float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) + float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) + float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) + float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) + uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks public: void printIMU(struct imuSample *data); @@ -339,13 +339,13 @@ public: // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) - struct map_projection_reference_s _posRef = {}; // Contains WGS-84 position latitude and longitude (radians) + struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) float _gps_alt_ref = 0.0f; // WGS-84 height (m) - bool _vehicleArmed = false; // vehicle arm status used to turn off funtionality used on the ground + bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground // publish the status of various GPS quality checks - union gpsCheckFailStatus_u { + union gps_check_fail_status_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 @@ -359,6 +359,6 @@ public: } flags; uint16_t value; }; - gpsCheckFailStatus_u _gpsCheckFailStatus; + gps_check_fail_status_u _gps_check_fail_status; }; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 533ebc2e1e..019a72809b 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -62,119 +62,119 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps) { // Check the number of satellites - _gpsCheckFailStatus.flags.nsats = (gps->nsats < _params.requiredNsats); + _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); // Check the geometric dilution of precision - _gpsCheckFailStatus.flags.gdop = (gps->gdop > _params.requiredGDoP); + _gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop); // Check the reported horizontal position accuracy - _gpsCheckFailStatus.flags.hacc = (gps->eph > _params.requiredEph); + _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); // Check the reported vertical position accuracy - _gpsCheckFailStatus.flags.vacc = (gps->epv > _params.requiredEpv); + _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); // Check the reported speed accuracy - _gpsCheckFailStatus.flags.sacc = (gps->eph > _params.requiredEph); + _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); // Calculate position movement since last measurement - float deltaPosN = 0.0f; - float deltaPosE = 0.0f; + float delta_posN = 0.0f; + float delta_PosE = 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); + if (_pos_ref.init_done) { + map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE); } else { - map_projection_init(&_posRef, lat, lon); + map_projection_init(&_pos_ref, lat, lon); _gps_alt_ref = gps->alt * 1e-3f; } // Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient - const float filtTimeConst = 10.0f; - float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst); - float filterCoef = dt/filtTimeConst; + const float filt_time_const = 10.0f; + float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filt_time_const); + float filter_coef = dt/filt_time_const; // 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); + float vel_limit = 10.0f * _params.req_hdrift; + float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit); + float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit); // Apply a low pass filter - _gpsDriftVelN = velN * filterCoef + _gpsDriftVelN * (1.0f - filterCoef); - _gpsDriftVelE = velE * filterCoef + _gpsDriftVelE * (1.0f - filterCoef); + _gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef); + _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); // 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); + if(_vehicle_armed) { + float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); + _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); } else { - _gpsCheckFailStatus.flags.hdrift = false; + _gps_check_fail_status.flags.hdrift = false; } // Save current position as the reference for next time - map_projection_init(&_posRef, lat, lon); + map_projection_init(&_pos_ref, lat, lon); _last_gps_origin_time_us = _time_last_imu; // 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); + vel_limit = 10.0f * _params.req_vdrift; + float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit); // 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); + _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); // 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); + if(_vehicle_armed) { + _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); } else { - _gpsCheckFailStatus.flags.vdrift = false; + _gps_check_fail_status.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); + if (_vehicle_armed) { + vel_limit = 10.0f * _params.req_hdrift; + float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit); + float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit); + _gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); + _gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); + float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); + _gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift); } else { - _gpsCheckFailStatus.flags.hspeed = false; + _gps_check_fail_status.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); + vel_limit = 10.0f * _params.req_vdrift; + float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit); + _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); + _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift); // assume failed first time through - if (_lastGpsFail_us == 0) { - _lastGpsFail_us = _time_last_imu; + if (_last_gps_fail_us == 0) { + _last_gps_fail_us = _time_last_imu; } // 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)) + (_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) || + (_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) || + (_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) || + (_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) || + (_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) || + (_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) || + (_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) || + (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || + (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { - _lastGpsFail_us = _time_last_imu; + _last_gps_fail_us = _time_last_imu; } // continuous period without fail of 10 seconds required to return a healthy status - if (_time_last_imu - _lastGpsFail_us > 1e7) { + if (_time_last_imu - _last_gps_fail_us > 1e7) { return true; } return false;