AttPosEKF: Moved data collection to separate function

2 files changed, 484 insertions, 463 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index b2d9c9da3..242d8a486 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1395,11 +1395,10 @@ int commander_thread_main(int argc, char *argv[])
 			if(status.condition_global_position_valid) {
 				set_tune_override(TONE_GPS_WARNING_TUNE);
 				status_changed = true;
+				status.condition_global_position_valid = false;		
 			}
-
-			status.condition_global_position_valid = false;		
 		}
-		else if(hrt_absolute_time() > POSITION_TIMEOUT) {
+		else if(global_position.timestamp != 0) {
 			//Got good global position estimate
 			if(!status.condition_global_position_valid) {
 				status_changed = true;
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 bcd6e7fff..ac9f679c2 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
@@ -93,6 +93,7 @@ static uint64_t IMUmsec = 0;
 static uint64_t IMUusec = 0;
 
 //Constants
+static constexpr float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
 static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; 	///< units: microseconds
 static constexpr float POS_RESET_THRESHOLD = 5.0f; 				///< Seconds before we signal a total GPS failure
 
@@ -236,6 +237,8 @@ private:
 	perf_counter_t	_perf_airspeed;		///<local performance counter for airspeed updates
 	perf_counter_t	_perf_reset;		///<local performance counter for filter resets
 
+	float 			_gps_alt_filt;
+	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
@@ -254,6 +257,12 @@ private:
 	bool			_ekf_logging;		///< log EKF state
 	unsigned		_debug;				///< debug level - default 0
 
+	bool 			_newDataGps;
+	bool 			_newHgtData;
+	bool 			_newAdsData;
+	bool 			_newDataMag;
+	bool 			_newRangeData;
+
 	int				_mavlink_fd;
 
 	struct {
@@ -361,10 +370,21 @@ private:
 
 	/**
 	* @brief
-	*	
+	*	Runs the sensor fusion step of the filter. The parameters determine which of the sensors
+	*	are fused with each other
 	**/
 	void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
 			const bool fuseBaro, const bool fuseAirSpeed);
+
+	/**
+	* @brief
+	*	Initialize first time good GPS fix so we can get a reference point to calculate local position from
+	*	Should only be required to call once
+	**/
+	void initializeGPS();
+
+	void pollData();
+
 };
 
 namespace estimator
@@ -446,6 +466,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
 	/* states */
+	_gps_alt_filt(0.0f),
+	_baro_alt_filt(0.0f),
 	_covariancePredictionDt(0.0f),
 	_gpsIsGood(false),
 	_lastGPSTimestamp(0),
@@ -463,6 +485,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mag_main(0),
 	_ekf_logging(true),
 	_debug(0),
+
+	_newDataGps(false),
+	_newHgtData(false),
+	_newAdsData(false),
+	_newDataMag(false),
+	_newRangeData(false),
+
 	_mavlink_fd(-1),
 	_parameters{},
 	_parameter_handles{},
@@ -791,9 +820,6 @@ void AttitudePositionEstimatorEKF::task_main()
 	/* sets also parameters in the EKF object */
 	parameters_update();
 
-	Vector3f lastAngRate;
-	Vector3f lastAccel;
-
 	/* wakeup source(s) */
 	struct pollfd fds[2];
 
@@ -808,24 +834,10 @@ void AttitudePositionEstimatorEKF::task_main()
 	fds[1].events = POLLIN;
 #endif
 
-	bool newDataGps = false;
-	bool newHgtData = false;
-	bool newAdsData = false;
-	bool newDataMag = false;
-	bool newRangeData = false;
-
-	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
-
 	_gps.vel_n_m_s = 0.0f;
 	_gps.vel_e_m_s = 0.0f;
 	_gps.vel_d_m_s = 0.0f;
 
-	// init lowpass filters for baro and gps altitude
-	float _gps_alt_filt = 0;
-	float _baro_alt_filt = 0;
-	const float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
-	hrt_abstime baro_last = 0;
-
 	_task_running = true;
 
 	while (!_task_should_exit) {
@@ -863,9 +875,6 @@ void AttitudePositionEstimatorEKF::task_main()
 			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
 			vehicle_status_poll();
 
-			bool accel_updated;
-			bool mag_updated;
-
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
@@ -903,389 +912,7 @@ void AttitudePositionEstimatorEKF::task_main()
 			/**
 			 *    PART ONE: COLLECT ALL DATA
 			 **/
-
-			/* load local copies */
-#ifndef SENSOR_COMBINED_SUB
-			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
-
-
-			orb_check(_accel_sub, &accel_updated);
-
-			if (accel_updated) {
-				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
-			}
-
-			_last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3;
-			IMUusec = _gyro.timestamp;
-
-			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
-			_last_run = _gyro.timestamp;
-
-			/* guard against too large deltaT's */
-			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-				deltaT = 0.01f;
-			}
-
-
-			// Always store data, independent of init status
-			/* fill in last data set */
-			_ekf->dtIMU = deltaT;
-
-			if (isfinite(_gyro.x) &&
-				isfinite(_gyro.y) &&
-				isfinite(_gyro.z)) {
-				_ekf->angRate.x = _gyro.x;
-				_ekf->angRate.y = _gyro.y;
-				_ekf->angRate.z = _gyro.z;
-
-				if (!_gyro_valid) {
-					lastAngRate = _ekf->angRate;
-				}
-
-				_gyro_valid = true;
-			}
-
-			if (accel_updated) {
-				_ekf->accel.x = _accel.x;
-				_ekf->accel.y = _accel.y;
-				_ekf->accel.z = _accel.z;
-
-				if (!_accel_valid) {
-					lastAccel = _ekf->accel;
-				}
-
-				_accel_valid = true;
-			}
-
-			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
-			_ekf->lastAngRate = angRate;
-			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
-			_ekf->lastAccel = accel;
-
-
-#else
-			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-
-			static hrt_abstime last_accel = 0;
-			static hrt_abstime last_mag = 0;
-
-			if (last_accel != _sensor_combined.accelerometer_timestamp) {
-				accel_updated = true;
-			} else {
-				accel_updated = false;
-			}
-
-			last_accel = _sensor_combined.accelerometer_timestamp;
-
-
-			// Copy gyro and accel
-			_last_sensor_timestamp = _sensor_combined.timestamp;
-			IMUmsec = _sensor_combined.timestamp / 1e3;
-			IMUusec = _sensor_combined.timestamp;
-
-			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
-
-			/* guard against too large deltaT's */
-			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-				deltaT = 0.01f;
-			}
-
-			_last_run = _sensor_combined.timestamp;
-
-			// Always store data, independent of init status
-			/* fill in last data set */
-			_ekf->dtIMU = deltaT;
-
-			int last_gyro_main = _gyro_main;
-
-			if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
-				isfinite(_sensor_combined.gyro_rad_s[1]) &&
-				isfinite(_sensor_combined.gyro_rad_s[2]) &&
-				(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
-
-				_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
-				_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
-				_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
-				_gyro_main = 0;
-				_gyro_valid = true;
-
-			} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
-				isfinite(_sensor_combined.gyro1_rad_s[1]) &&
-				isfinite(_sensor_combined.gyro1_rad_s[2])) {
-
-				_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
-				_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
-				_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
-				_gyro_main = 1;
-				_gyro_valid = true;
-
-			} else {
-				_gyro_valid = false;
-			}
-
-			if (last_gyro_main != _gyro_main) {
-				mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
-			}
-
-			if (!_gyro_valid) {
-				/* keep last value if he hit an out of band value */
-				lastAngRate = _ekf->angRate;
-			} else {
-				perf_count(_perf_gyro);
-			}
-
-			if (accel_updated) {
-
-				int last_accel_main = _accel_main;
-
-				/* fail over to the 2nd accel if we know the first is down */
-				if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
-					_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
-					_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
-					_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
-					_accel_main = 0;
-				} else {
-					_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
-					_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
-					_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
-					_accel_main = 1;
-				}
-
-				if (!_accel_valid) {
-					lastAccel = _ekf->accel;
-				}
-
-				if (last_accel_main != _accel_main) {
-					mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
-				}
-
-				_accel_valid = true;
-			}
-
-			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
-			lastAngRate = _ekf->angRate;
-			_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
-			lastAccel = _ekf->accel;
-
-			if (last_mag != _sensor_combined.magnetometer_timestamp) {
-				mag_updated = true;
-				newDataMag = true;
-
-			} else {
-				newDataMag = false;
-			}
-
-			last_mag = _sensor_combined.magnetometer_timestamp;
-
-#endif
-
-			//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
-
-			bool airspeed_updated;
-			orb_check(_airspeed_sub, &airspeed_updated);
-
-			if (airspeed_updated) {
-				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-				perf_count(_perf_airspeed);
-
-				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
-				newAdsData = true;
-
-			} else {
-				newAdsData = false;
-			}
-
-			//Calculate time since last good GPS fix
-			const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f;
-
-			bool gps_updated;
-			orb_check(_gps_sub, &gps_updated);
-
-			if (gps_updated) {
-
-				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
-				perf_count(_perf_gps);
-
-				//We are more strict for our first fix
-				float requiredAccuracy = _parameters.pos_stddev_threshold;
-				if(_gpsIsGood) {
-					requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
-				}
-
-				//Check if the GPS fix is good enough for us to use
-				if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
-					_gpsIsGood = true;
-				}
-				else {
-					_gpsIsGood = false;
-				}
-
-				if (_gpsIsGood) {
-					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD));
-
-					/* fuse GPS updates */
-
-					//_gps.timestamp / 1e3;
-					_ekf->GPSstatus = _gps.fix_type;
-					_ekf->velNED[0] = _gps.vel_n_m_s;
-					_ekf->velNED[1] = _gps.vel_e_m_s;
-					_ekf->velNED[2] = _gps.vel_d_m_s;
-
-					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
-
-					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
-					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
-					_ekf->gpsHgt = _gps.alt / 1e3f;
-
-					//Convert from global frame to local frame
-					_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];
-
-					// update LPF
-					_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
-
-					//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
-
-					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
-					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
-					// } else {
-					// 	_ekf->vneSigma = _parameters.velne_noise;
-					// }
-
-					// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
-					// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
-					// } else {
-					// 	_ekf->posNeSigma = _parameters.posne_noise;
-					// }
-
-					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
-
-					/* check if we had a GPS outage for a long time */
-					if (dtGoodGPS > POS_RESET_THRESHOLD) {
-						_ekf->ResetPosition();
-						_ekf->ResetVelocity();
-						_ekf->ResetStoredStates();
-					}
-
-					newDataGps = true;
-					_lastGPSTimestamp = _gps.timestamp_position;
-				}
-				else {
-					//Too poor GPS fix to use
-					newDataGps = false;
-				}
-			}
-			// 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) {
-				_gpsIsGood = false;
-			}
-
-			bool baro_updated;
-			orb_check(_baro_sub, &baro_updated);
-
-			if (baro_updated) {
-
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-
-				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
-				baro_last = _baro.timestamp;
-
-				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
-
-				_ekf->baroHgt = _baro.altitude;
-				_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
-
-				if (!_baro_init) {
-					_baro_ref = _baro.altitude;
-					_baro_init = true;
-					warnx("ALT REF INIT");
-				}
-
-				perf_count(_perf_baro);
-
-				newHgtData = true;
-			} else {
-				newHgtData = false;
-			}
-
-#ifndef SENSOR_COMBINED_SUB
-			orb_check(_mag_sub, &mag_updated);
-#endif
-
-			if (mag_updated) {
-
-				_mag_valid = true;
-
-				perf_count(_perf_mag);
-
-#ifndef SENSOR_COMBINED_SUB
-				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
-
-				// XXX we compensate the offsets upfront - should be close to zero.
-				// 0.001f
-				_ekf->magData.x = _mag.x;
-				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
-				_ekf->magData.y = _mag.y;
-				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-				_ekf->magData.z = _mag.z;
-				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-
-#else
-				int last_mag_main = _mag_main;
-
-				// XXX we compensate the offsets upfront - should be close to zero.
-
-				/* fail over to the 2nd mag if we know the first is down */
-				if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
-					_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
-					_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
-					_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
-					_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-					_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
-					_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-					_mag_main = 0;
-				} else {
-					_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
-					_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
-					_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
-					_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-					_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
-					_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-					_mag_main = 1;
-				}
-
-				if (last_mag_main != _mag_main) {
-					mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
-				}
-#endif
-
-				newDataMag = true;
-
-			} else {
-				newDataMag = false;
-			}
-
-			orb_check(_distance_sub, &newRangeData);
-
-			if (newRangeData) {
-				orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
-
-				if (_distance.valid) {
-					_ekf->rngMea = _distance.distance;
-					_distance_last_valid = _distance.timestamp;
-				} else {
-					newRangeData = false;
-				}
-			}
+			 pollData();
 
 			/*
 			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
@@ -1322,61 +949,13 @@ void AttitudePositionEstimatorEKF::task_main()
 
 				/* Initialize the filter first */
 				if (!_gps_initialized && _gpsIsGood) {
-
-					// GPS is in scaled integers, convert
-					double lat = _gps.lat / 1.0e7;
-					double lon = _gps.lon / 1.0e7;
-					float gps_alt = _gps.alt / 1e3f;
-
-					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
-
-					// init filtered gps and baro altitudes
-					_gps_alt_filt = gps_alt;
-					_baro_alt_filt = _baro.altitude;
-
-					_ekf->baroHgt = _baro.altitude;
-					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
-
-					// Set up position variables correctly
-					_ekf->GPSstatus = _gps.fix_type;
-
-					_ekf->gpsLat = math::radians(lat);
-					_ekf->gpsLon = math::radians(lon) - M_PI;
-					_ekf->gpsHgt = gps_alt;
-
-					// Look up mag declination based on current position
-					float declination = math::radians(get_mag_declination(lat, lon));
-
-					float initVelNED[3];
-					initVelNED[0] = _gps.vel_n_m_s;
-					initVelNED[1] = _gps.vel_e_m_s;
-					initVelNED[2] = _gps.vel_d_m_s;
-
-					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
-
-					// Initialize projection
-					_local_pos.ref_lat = lat;
-					_local_pos.ref_lon = lon;
-					_local_pos.ref_alt = gps_alt;
-					_local_pos.ref_timestamp = _gps.timestamp_position;
-
-					map_projection_init(&_pos_ref, lat, lon);
-					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
-
-					#if 0
-					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
-						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
-					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
-					#endif
-
-					_gps_initialized = true;
-
-				} else if (!_ekf->statesInitialised) {
+					initializeGPS();
+				} 
+				else if (!_ekf->statesInitialised) {
+					// North, East Down position (m)
+					float posNED[3] = {0.0f, 0.0f, 0.0f}; 
 					float initVelNED[3];
-
+	
 					initVelNED[0] = 0.0f;
 					initVelNED[1] = 0.0f;
 					initVelNED[2] = 0.0f;
@@ -1390,7 +969,8 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
-				} else if (_ekf->statesInitialised) {
+				} 
+				else if (_ekf->statesInitialised) {
 
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
@@ -1401,7 +981,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					}
 
 					//Run EKF data fusion steps
-					updateSensorFusion(newDataGps, newDataMag, newRangeData, newHgtData, newAdsData);
+					updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
 					//Publish attitude estimations
 					publishAttitude();
@@ -1435,6 +1015,60 @@ void AttitudePositionEstimatorEKF::task_main()
 	_exit(0);
 }
 
+void AttitudePositionEstimatorEKF::initializeGPS()
+{
+	// GPS is in scaled integers, convert
+	double lat = _gps.lat / 1.0e7;
+	double lon = _gps.lon / 1.0e7;
+	float gps_alt = _gps.alt / 1e3f;
+
+	// Set up height correctly
+	orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+	_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
+
+	// init filtered gps and baro altitudes
+	_gps_alt_filt = gps_alt;
+	_baro_alt_filt = _baro.altitude;
+
+	_ekf->baroHgt = _baro.altitude;
+	_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
+
+	// Set up position variables correctly
+	_ekf->GPSstatus = _gps.fix_type;
+
+	_ekf->gpsLat = math::radians(lat);
+	_ekf->gpsLon = math::radians(lon) - M_PI;
+	_ekf->gpsHgt = gps_alt;
+
+	// Look up mag declination based on current position
+	float declination = math::radians(get_mag_declination(lat, lon));
+
+	float initVelNED[3];
+	initVelNED[0] = _gps.vel_n_m_s;
+	initVelNED[1] = _gps.vel_e_m_s;
+	initVelNED[2] = _gps.vel_d_m_s;
+
+	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
+
+	// Initialize projection
+	_local_pos.ref_lat = lat;
+	_local_pos.ref_lon = lon;
+	_local_pos.ref_alt = gps_alt;
+	_local_pos.ref_timestamp = _gps.timestamp_position;
+
+	map_projection_init(&_pos_ref, lat, lon);
+	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+
+	#if 0
+	warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
+		(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
+	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
+	#endif
+
+	_gps_initialized = true;
+}
+
 void AttitudePositionEstimatorEKF::publishAttitude()
 {
 	// Output results
@@ -1798,6 +1432,394 @@ void AttitudePositionEstimatorEKF::print_status()
 		       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
+void AttitudePositionEstimatorEKF::pollData()
+{
+	static Vector3f lastAngRate;
+	static Vector3f lastAccel;
+	bool accel_updated = false;
+
+	//Update Gyro and Accelerometer
+#ifndef SENSOR_COMBINED_SUB
+	orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+
+
+	orb_check(_accel_sub, &accel_updated);
+
+	if (accel_updated) {
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	}
+
+	_last_sensor_timestamp = _gyro.timestamp;
+	IMUmsec = _gyro.timestamp / 1e3;
+	IMUusec = _gyro.timestamp;
+
+	float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
+	_last_run = _gyro.timestamp;
+
+	/* guard against too large deltaT's */
+	if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+		deltaT = 0.01f;
+	}
+
+
+	// Always store data, independent of init status
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
+
+	if (isfinite(_gyro.x) &&
+		isfinite(_gyro.y) &&
+		isfinite(_gyro.z)) {
+		_ekf->angRate.x = _gyro.x;
+		_ekf->angRate.y = _gyro.y;
+		_ekf->angRate.z = _gyro.z;
+
+		if (!_gyro_valid) {
+			lastAngRate = _ekf->angRate;
+		}
+
+		_gyro_valid = true;
+	}
+
+	if (accel_updated) {
+		_ekf->accel.x = _accel.x;
+		_ekf->accel.y = _accel.y;
+		_ekf->accel.z = _accel.z;
+
+		if (!_accel_valid) {
+			lastAccel = _ekf->accel;
+		}
+
+		_accel_valid = true;
+	}
+
+	_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+	_ekf->lastAngRate = angRate;
+	_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+	_ekf->lastAccel = accel;
+
+
+#else
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+
+	static hrt_abstime last_accel = 0;
+	static hrt_abstime last_mag = 0;
+
+	if (last_accel != _sensor_combined.accelerometer_timestamp) {
+		accel_updated = true;
+	} else {
+		accel_updated = false;
+	}
+
+	last_accel = _sensor_combined.accelerometer_timestamp;
+
+
+	// Copy gyro and accel
+	_last_sensor_timestamp = _sensor_combined.timestamp;
+	IMUmsec = _sensor_combined.timestamp / 1e3;
+	IMUusec = _sensor_combined.timestamp;
+
+	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
+
+	/* guard against too large deltaT's */
+	if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+		deltaT = 0.01f;
+	}
+
+	_last_run = _sensor_combined.timestamp;
+
+	// Always store data, independent of init status
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
+
+	int last_gyro_main = _gyro_main;
+
+	if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
+		isfinite(_sensor_combined.gyro_rad_s[1]) &&
+		isfinite(_sensor_combined.gyro_rad_s[2]) &&
+		(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
+
+		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
+		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
+		_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
+		_gyro_main = 0;
+		_gyro_valid = true;
+
+	} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
+		isfinite(_sensor_combined.gyro1_rad_s[1]) &&
+		isfinite(_sensor_combined.gyro1_rad_s[2])) {
+
+		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
+		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
+		_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
+		_gyro_main = 1;
+		_gyro_valid = true;
+
+	} else {
+		_gyro_valid = false;
+	}
+
+	if (last_gyro_main != _gyro_main) {
+		mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
+	}
+
+	if (!_gyro_valid) {
+		/* keep last value if he hit an out of band value */
+		lastAngRate = _ekf->angRate;
+	} else {
+		perf_count(_perf_gyro);
+	}
+
+	if (accel_updated) {
+
+		int last_accel_main = _accel_main;
+
+		/* fail over to the 2nd accel if we know the first is down */
+		if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
+			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
+			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
+			_accel_main = 0;
+		} else {
+			_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
+			_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
+			_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
+			_accel_main = 1;
+		}
+
+		if (!_accel_valid) {
+			lastAccel = _ekf->accel;
+		}
+
+		if (last_accel_main != _accel_main) {
+			mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
+		}
+
+		_accel_valid = true;
+	}
+
+	_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
+	lastAngRate = _ekf->angRate;
+	_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
+	lastAccel = _ekf->accel;
+
+	if (last_mag != _sensor_combined.magnetometer_timestamp) {
+		_newDataMag = true;
+
+	} else {
+		_newDataMag = false;
+	}
+
+	last_mag = _sensor_combined.magnetometer_timestamp;
+
+#endif
+
+	//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
+
+	//Update AirSpeed
+	orb_check(_airspeed_sub, &_newAdsData);
+	if (_newAdsData) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		perf_count(_perf_airspeed);
+
+		_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) {
+
+		orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
+		perf_count(_perf_gps);
+
+		//We are more strict for our first fix
+		float requiredAccuracy = _parameters.pos_stddev_threshold;
+		if(_gpsIsGood) {
+			requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
+		}
+
+		//Check if the GPS fix is good enough for us to use
+		if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
+			_gpsIsGood = true;
+		}
+		else {
+			_gpsIsGood = false;
+		}
+
+		if (_gpsIsGood) {
+			_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD));
+
+			/* fuse GPS updates */
+
+			//_gps.timestamp / 1e3;
+			_ekf->GPSstatus = _gps.fix_type;
+			_ekf->velNED[0] = _gps.vel_n_m_s;
+			_ekf->velNED[1] = _gps.vel_e_m_s;
+			_ekf->velNED[2] = _gps.vel_d_m_s;
+
+			// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+
+			_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
+			_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+			_ekf->gpsHgt = _gps.alt / 1e3f;
+
+			//check if we had a GPS outage for a long time
+			if(_gps_initialized) {
+
+				//Convert from global frame to local frame
+				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];	
+
+				if (dtGoodGPS > POS_RESET_THRESHOLD) {
+					_ekf->ResetPosition();
+					_ekf->ResetVelocity();
+					_ekf->ResetStoredStates();
+				}
+			}
+
+			// update LPF
+			_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+
+			//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
+
+			// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
+			// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
+			// } else {
+			// 	_ekf->vneSigma = _parameters.velne_noise;
+			// }
+
+			// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
+			// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
+			// } else {
+			// 	_ekf->posNeSigma = _parameters.posne_noise;
+			// }
+
+			// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
+
+			_lastGPSTimestamp = _gps.timestamp_position;
+		}
+		else {
+			//Too poor GPS fix to use
+			_newDataGps = false;
+		}
+	}
+
+	// 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) {
+		_gpsIsGood = false;
+	}
+
+	//Update barometer
+	orb_check(_baro_sub, &_newHgtData);
+
+	if (_newHgtData) {
+		static hrt_abstime baro_last = 0;
+
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+		// init lowpass filters for baro and gps altitude
+		float baro_elapsed;
+		if(baro_last == 0) {
+			baro_elapsed = 0.0f;
+		}
+		else {
+			baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+		}
+		baro_last = _baro.timestamp;
+
+		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
+
+		_ekf->baroHgt = _baro.altitude;
+		_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+
+		if (!_baro_init) {
+			_baro_ref = _baro.altitude;
+			_baro_init = true;
+			warnx("ALT REF INIT");
+		}
+
+		perf_count(_perf_baro);
+
+		_newHgtData = true;
+	}
+
+	//Update Magnetometer
+#ifndef SENSOR_COMBINED_SUB
+	orb_check(_mag_sub, &_newDataMag);
+#endif
+
+	if (_newDataMag) {
+
+		_mag_valid = true;
+
+		perf_count(_perf_mag);
+
+#ifndef SENSOR_COMBINED_SUB
+		orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+
+		// XXX we compensate the offsets upfront - should be close to zero.
+		// 0.001f
+		_ekf->magData.x = _mag.x;
+		_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+		_ekf->magData.y = _mag.y;
+		_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+		_ekf->magData.z = _mag.z;
+		_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#else
+		int last_mag_main = _mag_main;
+
+		// XXX we compensate the offsets upfront - should be close to zero.
+
+		/* fail over to the 2nd mag if we know the first is down */
+		if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
+			_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+			_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+			_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+			_mag_main = 0;
+		} else {
+			_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
+			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+			_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
+			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+			_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
+			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+			_mag_main = 1;
+		}
+
+		if (last_mag_main != _mag_main) {
+			mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
+		}
+#endif
+	}
+
+	//Update range data
+	orb_check(_distance_sub, &_newRangeData);
+	if (_newRangeData) {
+		orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
+
+		if (_distance.valid) {
+			_ekf->rngMea = _distance.distance;
+			_distance_last_valid = _distance.timestamp;
+		} else {
+			_newRangeData = false;
+		}
+	}
+}
+
 int AttitudePositionEstimatorEKF::trip_nan() {
 
 	int ret = 0;