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

@@ -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;
 		}
 	}

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;

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;
 
 };

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;