AttPosEKF: Remove unused gps accel estimation

4 files changed, 18 insertions, 22 deletions

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 2bbc6c916..1a190bf40 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -328,6 +328,9 @@ GPS::task_main()
 			//DEBUG BEGIN: Disable GPS using aux1
 			orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 			if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) {
+				_report_gps_pos.timestamp_position = hrt_absolute_time();
+				_report_gps_pos.timestamp_variance = hrt_absolute_time();
+				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
 				_report_gps_pos.fix_type		= 0;
 				_report_gps_pos.satellites_used	= 0;
 
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 936092195..8ec7c43a1 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -230,7 +230,7 @@ private:
 	float 			_baro_alt_filt;
 	float 			_covariancePredictionDt;  ///< time lapsed since last covariance prediction
 	bool			_gpsIsGood;				  ///< True if the current GPS fix is good enough for us to use
-	uint64_t 		_lastGPSTimestamp;		  ///< Timestamp of last good GPS fix we have received
+	uint64_t 		_previousGPSTimestamp;	  ///< Timestamp of last good GPS fix we have received
 	bool			_baro_init;
 	bool			_gps_initialized;
 	hrt_abstime		_filter_start_time;
@@ -372,6 +372,11 @@ private:
 	**/
 	void initializeGPS();
 
+	/**
+	* @brief
+	*	Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
+	*	flags to true (e.g newDataGps)
+	**/
 	void pollData();
 
 };
@@ -451,7 +456,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_baro_alt_filt(0.0f),
 	_covariancePredictionDt(0.0f),
 	_gpsIsGood(false),
-	_lastGPSTimestamp(0),
+	_previousGPSTimestamp(0),
 	_baro_init(false),
 	_gps_initialized(false),
 	_filter_start_time(0),
@@ -1209,18 +1214,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	if (fuseGPS && _gps_initialized) {
 		// Convert GPS measurements to Pos NE, hgt and Vel NED
 
-		float gps_dt = (_gps.timestamp_position - _lastGPSTimestamp) / 1e6f;
-
-		// Calculate acceleration predicted by GPS velocity change
-		if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
-			(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
-			(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
-
-			_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
-			_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
-			_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
-		}
-
 		// set fusion flags
 		_ekf->fuseVelData = true;
 		_ekf->fusePosData = true;
@@ -1522,8 +1515,6 @@ void AttitudePositionEstimatorEKF::pollData()
 		_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
 	}
 
-	//Calculate time since last good GPS fix
-	const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f;
 
 	orb_check(_gps_sub, &_newDataGps);
 	if (_newDataGps) {
@@ -1546,7 +1537,11 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		if (_gpsIsGood) {
-			_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD));
+
+			//Calculate time since last good GPS fix
+			const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+
+			_ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD));
 
 			/* fuse GPS updates */
 
@@ -1569,7 +1564,7 @@ void AttitudePositionEstimatorEKF::pollData()
 				float posNED[3] = {0.0f, 0.0f, 0.0f}; 
 				_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
 				_ekf->posNE[0] = posNED[0];
-				_ekf->posNE[1] = posNED[1];	
+				_ekf->posNE[1] = posNED[1];
 
 				if (dtGoodGPS > POS_RESET_THRESHOLD) {
 					_ekf->ResetPosition();
@@ -1597,7 +1592,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 			// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
-			_lastGPSTimestamp = _gps.timestamp_position;
+			_previousGPSTimestamp = _gps.timestamp_position;
 		}
 		else {
 			//Too poor GPS fix to use
@@ -1607,7 +1602,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
 	// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
-	else if(dtGoodGPS > POS_RESET_THRESHOLD) {
+	else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) {
 		_gpsIsGood = false;
 	}
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 47706c37b..e912e699e 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -111,7 +111,6 @@ AttPosEKF::AttPosEKF() :
     innovVelPos{},
     varInnovVelPos{},
     velNED{},
-    accelGPSNED{},
     posNE{},
     hgtMea(0.0f),
     baroHgtOffset(0.0f),
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index 5de9d4c5a..19ef52145 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -161,7 +161,6 @@ public:
     float varInnovVelPos[6]; // innovation variance output
 
     float velNED[3]; // North, East, Down velocity obs (m/s)
-    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude