From c66f7f4a09ce7adb4b75420ae401e69f4b66d0f2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 5 Mar 2019 12:55:26 -0500 Subject: [PATCH] EKF drop unused timestamp from collect_gps() and pass data by const reference --- CMakeLists.txt | 1 - EKF/ekf.h | 4 +-- EKF/estimator_interface.cpp | 28 ++++++++--------- EKF/estimator_interface.h | 4 +-- EKF/gps_checks.cpp | 48 ++++++++++++++--------------- EKF/tests/base/base.cpp | 5 ++- EKF/tests/ringbuffer/ringbuffer.cpp | 3 ++ 7 files changed, 49 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5873655370..96af1a9bdd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,7 +95,6 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) -Wextra -Werror - -Wno-unused-parameter -Wno-missing-field-initializers # ignore for GCC 4.8 support ) endif() diff --git a/EKF/ekf.h b/EKF/ekf.h index 853859f086..18d4987a75 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -126,7 +126,7 @@ public: void get_covariances(float *covariances); // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - bool collect_gps(uint64_t time_usec, struct gps_message *gps); + bool collect_gps(const gps_message &gps); bool collect_imu(const imuSample &imu); @@ -578,7 +578,7 @@ private: void calcEarthRateNED(Vector3f &omega, float lat_rad) const; // return true id the GPS quality is good enough to set an origin and start aiding - bool gps_is_good(struct gps_message *gps); + bool gps_is_good(const gps_message &gps); // Control the filter fusion modes void controlFusionModes(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index fd981c0e12..9d800db55f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -190,7 +190,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) } } -void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) +void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps) { if (!_initialised || _gps_buffer_fail) { return; @@ -210,35 +210,35 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) // limit data rate to prevent data being lost bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); - if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps->fix_type > 2) { + if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) { gpsSample gps_sample_new; - gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; + gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000; gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _time_last_gps = time_usec; gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); - gps_sample_new.vel = Vector3f(gps->vel_ned); + gps_sample_new.vel = Vector3f(gps.vel_ned); - _gps_speed_valid = gps->vel_ned_valid; - gps_sample_new.sacc = gps->sacc; - gps_sample_new.hacc = gps->eph; - gps_sample_new.vacc = gps->epv; + _gps_speed_valid = gps.vel_ned_valid; + gps_sample_new.sacc = gps.sacc; + gps_sample_new.hacc = gps.eph; + gps_sample_new.vacc = gps.epv; - gps_sample_new.hgt = (float)gps->alt * 1e-3f; + gps_sample_new.hgt = (float)gps.alt * 1e-3f; - gps_sample_new.yaw = gps->yaw; - if (ISFINITE(gps->yaw_offset)) { - _gps_yaw_offset = gps->yaw_offset; + gps_sample_new.yaw = gps.yaw; + if (ISFINITE(gps.yaw_offset)) { + _gps_yaw_offset = gps.yaw_offset; } else { _gps_yaw_offset = 0.0f; } // Only calculate the relative position if the WGS-84 location of the origin is set - if (collect_gps(time_usec, gps)) { + if (collect_gps(gps)) { float lpos_x = 0.0f; float lpos_y = 0.0f; - map_projection_project(&_pos_ref, (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; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1d7eb16793..a253504cdc 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -169,7 +169,7 @@ public: virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0; // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } + virtual bool collect_gps(const gps_message &gps) = 0; // accumulate and downsample IMU data to the EKF prediction rate virtual bool collect_imu(const imuSample &imu) = 0; @@ -184,7 +184,7 @@ public: void setMagData(uint64_t time_usec, float (&data)[3]); // set gps data - void setGpsData(uint64_t time_usec, struct gps_message *gps); + void setGpsData(uint64_t time_usec, const gps_message &gps); // set baro data void setBaroData(uint64_t time_usec, float data); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 7adc9bc9a4..3d63a5d92b 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -56,14 +56,14 @@ #define MASK_GPS_HSPD (1<<7) #define MASK_GPS_VSPD (1<<8) -bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) +bool Ekf::collect_gps(const gps_message &gps) { // Run GPS checks always bool gps_checks_pass = gps_is_good(gps); if (!_NED_origin_initialised && gps_checks_pass) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix - double lat = gps->lat / 1.0e7; - double lon = gps->lon / 1.0e7; + double lat = gps.lat / 1.0e7; + double lon = gps.lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating @@ -74,7 +74,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) } // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); + _gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2); _NED_origin_initialised = true; _last_gps_origin_time_us = _time_last_imu; @@ -86,8 +86,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) // request a reset of the yaw using the new declination _mag_yaw_reset_req = true; // save the horizontal and vertical position uncertainty of the origin - _gps_origin_eph = gps->eph; - _gps_origin_epv = gps->epv; + _gps_origin_eph = gps.eph; + _gps_origin_epv = gps.epv; // if the user has selected GPS as the primary height source, switch across to using it if (_primary_hgt_source == VDIST_SENSOR_GPS) { @@ -103,7 +103,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) } // start collecting GPS if there is a 3D fix and the NED origin has been set - return _NED_origin_initialised && (gps->fix_type >= 3); + return _NED_origin_initialised && (gps.fix_type >= 3); } /* @@ -113,27 +113,27 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) * Checks are activated using the EKF2_GPS_CHECK bitmask parameter * Checks are adjusted using the EKF2_REQ_* parameters */ -bool Ekf::gps_is_good(struct gps_message *gps) +bool Ekf::gps_is_good(const gps_message &gps) { // Check the fix type - _gps_check_fail_status.flags.fix = (gps->fix_type < 3); + _gps_check_fail_status.flags.fix = (gps.fix_type < 3); // Check the number of satellites - _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); + _gps_check_fail_status.flags.nsats = (gps.nsats < _params.req_nsats); // Check the geometric dilution of precision - _gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop); + _gps_check_fail_status.flags.gdop = (gps.gdop > _params.req_gdop); // Check the reported horizontal and vertical position accuracy - _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); + _gps_check_fail_status.flags.hacc = (gps.eph > _params.req_hacc); + _gps_check_fail_status.flags.vacc = (gps.epv > _params.req_vacc); // Check the reported speed accuracy - _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); + _gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); // check if GPS quality is degraded - _gps_error_norm = fmaxf((gps->eph / _params.req_hacc) , (gps->epv / _params.req_vacc)); - _gps_error_norm = fmaxf(_gps_error_norm , (gps->sacc / _params.req_sacc)); + _gps_error_norm = fmaxf((gps.eph / _params.req_hacc) , (gps.epv / _params.req_vacc)); + _gps_error_norm = fmaxf(_gps_error_norm , (gps.sacc / _params.req_sacc)); // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient const float filt_time_const = 10.0f; @@ -141,8 +141,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest - double lat = gps->lat * 1.0e-7; - double lon = gps->lon * 1.0e-7; + double lat = gps.lat * 1.0e-7; + double lon = gps.lon * 1.0e-7; if (!_control_status.flags.in_air && _vehicle_at_rest) { // Calculate position movement since last measurement float delta_posN = 0.0f; @@ -155,7 +155,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) } else { // no previous position has been set map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); - _gps_alt_prev = 1e-3f * (float)gps->alt; + _gps_alt_prev = 1e-3f * (float)gps.alt; } @@ -174,7 +174,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // Calculate the vertical drift velocity and limit to 10x the threshold float vz_drift_limit = 10.0f * _params.req_vdrift; - float gps_alt_m = 1e-3f * (float)gps->alt; + float gps_alt_m = 1e-3f * (float)gps.alt; float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vz_drift_limit, vz_drift_limit); // Apply a low pass filter to the vertical velocity @@ -186,8 +186,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) // Check the magnitude of the filtered horizontal GPS velocity float vxy_drift_limit = 10.0f * _params.req_hdrift; - float gps_velN = fminf(fmaxf(gps->vel_ned[0], -vxy_drift_limit), vxy_drift_limit); - float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vxy_drift_limit), vxy_drift_limit); + float gps_velN = fminf(fmaxf(gps.vel_ned[0], -vxy_drift_limit), vxy_drift_limit); + float gps_velE = fminf(fmaxf(gps.vel_ned[1], -vxy_drift_limit), vxy_drift_limit); _gps_velN_filt = gps_velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); _gps_velE_filt = gps_velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); _gps_drift_metrics[2] = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); @@ -211,11 +211,11 @@ bool Ekf::gps_is_good(struct gps_message *gps) // save GPS fix for next time map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); - _gps_alt_prev = 1e-3f * (float)gps->alt; + _gps_alt_prev = 1e-3f * (float)gps.alt; // Check the filtered difference between GPS and EKF vertical velocity float vz_diff_limit = 10.0f * _params.req_vdrift; - float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vz_diff_limit), vz_diff_limit); + float vertVel = fminf(fmaxf((gps.vel_ned[2] - _state.vel(2)), -vz_diff_limit), vz_diff_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); diff --git a/EKF/tests/base/base.cpp b/EKF/tests/base/base.cpp index 0cd4a7cb75..735d5d895c 100644 --- a/EKF/tests/base/base.cpp +++ b/EKF/tests/base/base.cpp @@ -44,6 +44,9 @@ int main(int argc, char *argv[]) { + (void)argc; // unused + (void)argv; // unused + Ekf *base = new Ekf(); // Test1: feed in fake imu data and check if delta angles are summed correclty @@ -131,7 +134,7 @@ int main(int argc, char *argv[]) base->setIMUData(timer, timer - timer_last, timer - timer_last, delta_ang, delta_vel); base->setMagData(timer, mag); base->setBaroData(timer, baro); - base->setGpsData(timer, &gps); + base->setGpsData(timer, gps); //base->print_imu_avg_time(); timer_last = timer; diff --git a/EKF/tests/ringbuffer/ringbuffer.cpp b/EKF/tests/ringbuffer/ringbuffer.cpp index fd1fde4e8f..b7e5d5a1bd 100644 --- a/EKF/tests/ringbuffer/ringbuffer.cpp +++ b/EKF/tests/ringbuffer/ringbuffer.cpp @@ -50,6 +50,9 @@ struct sample { int main(int argc, char *argv[]) { + (void)argc; // unused + (void)argv; // unused + sample x; x.time_us = 1000000; x.data[0] = x.data[1] = x.data[2] = 1.5f;