EKF drop unused timestamp from collect_gps() and pass data by const r…

…eference

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()

EKF/ekf.h

@@ -126,7 +126,7 @@ class Ekf : public EstimatorInterface
 	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 @@ class Ekf : public EstimatorInterface
 	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();

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;
 

EKF/estimator_interface.h

@@ -169,7 +169,7 @@ class EstimatorInterface
 	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 @@ class EstimatorInterface
 	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);

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,36 +113,36 @@ 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;
 	float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
 	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);
 

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;

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;