AttPosEKF: Refactor and code cleanup

3 files changed, 527 insertions, 343 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 2ecc104df..66eae6255 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -4,9 +4,9 @@
 # att & pos estimator, att & pos control.
 #
 
-attitude_estimator_ekf start
-#ekf_att_pos_estimator start
-position_estimator_inav start
+#attitude_estimator_ekf start
+ekf_att_pos_estimator start
+#position_estimator_inav start
 
 if mc_att_control start
 then
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index f9595a734..2bbc6c916 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -81,6 +81,12 @@
 #endif
 static const int ERROR = -1;
 
+//DEBUG BEGIN
+ #include <uORB/topics/manual_control_setpoint.h>
+static int sp_man_sub = -1;
+static struct manual_control_setpoint_s sp_man;
+//DEBUG END
+
 /* class for dynamic allocation of satellite info data */
 class GPS_Sat_Info
 {
@@ -162,7 +168,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
 namespace
 {
 
-GPS	*g_dev;
+GPS	*g_dev = nullptr;
 
 }
 
@@ -271,6 +277,27 @@ GPS::task_main_trampoline(void *arg)
 	g_dev->task_main();
 }
 
+static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	bool newData = false;
+
+	// check if there is new data to grab
+	if (orb_check(handle, &newData) != OK) {
+		return false;
+	}
+
+	if (!newData) {
+		return false;
+	}
+
+	if (orb_copy(meta, handle, buffer) != OK) {
+		return false;
+	}
+
+	return true;
+}
+
+
 void
 GPS::task_main()
 {
@@ -288,31 +315,62 @@ GPS::task_main()
 	uint64_t last_rate_measurement = hrt_absolute_time();
 	unsigned last_rate_count = 0;
 
+	//DEBUG BEGIN
+	sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	memset(&sp_man, 0, sizeof(sp_man));
+	//DEBUG END
+
 	/* loop handling received serial bytes and also configuring in between */
 	while (!_task_should_exit) {
 
 		if (_fake_gps) {
 
-			_report_gps_pos.timestamp_position = hrt_absolute_time();
-			_report_gps_pos.lat = (int32_t)47.378301e7f;
-			_report_gps_pos.lon = (int32_t)8.538777e7f;
-			_report_gps_pos.alt = (int32_t)1200e3f;
-			_report_gps_pos.timestamp_variance = hrt_absolute_time();
-			_report_gps_pos.s_variance_m_s = 10.0f;
-			_report_gps_pos.c_variance_rad = 0.1f;
-			_report_gps_pos.fix_type = 3;
-			_report_gps_pos.eph = 0.9f;
-			_report_gps_pos.epv = 1.8f;
-			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
-			_report_gps_pos.vel_n_m_s = 0.0f;
-			_report_gps_pos.vel_e_m_s = 0.0f;
-			_report_gps_pos.vel_d_m_s = 0.0f;
-			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
-			_report_gps_pos.cog_rad = 0.0f;
-			_report_gps_pos.vel_ned_valid = true;
+			//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.fix_type		= 0;
+				_report_gps_pos.satellites_used	= 0;
+
+				//Don't modify Lat/Lon/AMSL
+
+				_report_gps_pos.eph		      = (float)0xFFFF;
+				_report_gps_pos.epv		      = (float)0xFFFF;
+				_report_gps_pos.s_variance_m_s = (float)0xFFFF;
+
+				_report_gps_pos.vel_m_s		  = 0.0f;
+				_report_gps_pos.vel_n_m_s	  = 0.0f;
+				_report_gps_pos.vel_e_m_s	  = 0.0f;
+				_report_gps_pos.vel_d_m_s	  = 0.0f;
+				_report_gps_pos.vel_ned_valid  = false;
+
+				_report_gps_pos.cog_rad		  = 0.0f;
+				_report_gps_pos.c_variance_rad = (float)0xFFFF;
+			}
+			//DEBUG END	
+
+			else {
+				_report_gps_pos.timestamp_position = hrt_absolute_time();
+				_report_gps_pos.lat = (int32_t)47.378301e7f;
+				_report_gps_pos.lon = (int32_t)8.538777e7f;
+				_report_gps_pos.alt = (int32_t)1200e3f;
+				_report_gps_pos.timestamp_variance = hrt_absolute_time();
+				_report_gps_pos.s_variance_m_s = 10.0f;
+				_report_gps_pos.c_variance_rad = 0.1f;
+				_report_gps_pos.fix_type = 3;
+				_report_gps_pos.eph = 0.9f;
+				_report_gps_pos.epv = 1.8f;
+				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+				_report_gps_pos.vel_n_m_s = 0.0f;
+				_report_gps_pos.vel_e_m_s = 0.0f;
+				_report_gps_pos.vel_d_m_s = 0.0f;
+				_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+				_report_gps_pos.cog_rad = 0.0f;
+				_report_gps_pos.vel_ned_valid = true;				
+			}
 
 			//no time and satellite information simulated
 
+
 			if (!(_pub_blocked)) {
 				if (_report_gps_pos_pub > 0) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
@@ -364,6 +422,29 @@ GPS::task_main()
 
 					if (!(_pub_blocked)) {
 						if (helper_ret & 1) {
+
+							//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.fix_type			= 0;
+								_report_gps_pos.satellites_used	= 0;
+
+								//Don't modify Lat/Lon/AMSL
+
+								_report_gps_pos.eph		      = (float)0xFFFF;
+								_report_gps_pos.epv		      = (float)0xFFFF;
+								_report_gps_pos.s_variance_m_s = (float)0xFFFF;
+
+								_report_gps_pos.vel_m_s		  = 0.0f;
+								_report_gps_pos.vel_n_m_s	  = 0.0f;
+								_report_gps_pos.vel_e_m_s	  = 0.0f;
+								_report_gps_pos.vel_d_m_s	  = 0.0f;
+								_report_gps_pos.vel_ned_valid  = false;
+
+								_report_gps_pos.cog_rad		  = 0.0f;
+								_report_gps_pos.c_variance_rad = (float)0xFFFF;
+							}
+
 							if (_report_gps_pos_pub > 0) {
 								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
@@ -478,25 +559,35 @@ GPS::cmd_reset()
 void
 GPS::print_info()
 {
-	switch (_mode) {
-	case GPS_DRIVER_MODE_UBX:
-		warnx("protocol: UBX");
-		break;
+	//GPS Mode
+	if(_fake_gps) {
+		warnx("protocol: SIMULATED");
+	}
 
-	case GPS_DRIVER_MODE_MTK:
-		warnx("protocol: MTK");
-		break;
+	else {
+		switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;
 
 	case GPS_DRIVER_MODE_ASHTECH:
 		warnx("protocol: ASHTECH");
 		break;
 
-	default:
-		break;
+		default:
+			break;
+		}		
 	}
 
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
-	warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
+	warnx("sat info: %s, noise: %d, jamming detected: %s", 
+		(_p_report_sat_info != nullptr) ? "enabled" : "disabled", 
+		_report_gps_pos.noise_per_ms, 
+		_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
 
 	if (_report_gps_pos.timestamp_position != 0) {
 		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
@@ -520,7 +611,7 @@ GPS::print_info()
 namespace gps
 {
 
-GPS	*g_dev;
+GPS	*g_dev = nullptr;
 
 void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
 void	stop();
@@ -664,6 +755,14 @@ gps_main(int argc, char *argv[])
 		gps::start(device_name, fake_gps, enable_sat_info);
 	}
 
+	if (!strcmp(argv[1], "fake")) {
+		if(g_dev != nullptr) {
+			delete g_dev;
+			g_dev = nullptr;			
+		}
+		gps::start(GPS_DEFAULT_UART_PORT, true, false);
+	}
+
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();
 
@@ -686,5 +785,5 @@ gps_main(int argc, char *argv[])
 		gps::info();
 
 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'fake', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
 }
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 fbd695eac..2e1750b9e 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
@@ -103,6 +103,8 @@ static uint64_t IMUmsec = 0;
 static uint64_t IMUusec = 0;
 static const 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
+
 uint32_t millis()
 {
 	return IMUmsec;
@@ -169,7 +171,6 @@ public:
 	int		set_debuglevel(unsigned debug) { _debug = debug; return 0; }
 
 private:
-
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_estimator_task;		/**< task handle for sensor task */
@@ -185,8 +186,8 @@ private:
 	int		_baro_sub;			/**< barometer subscription */
 	int		_gps_sub;			/**< GPS subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_control_sub;		/**< notification of manual control updates */
 	int		_mission_sub;
 	int		_home_sub;			/**< home position as defined by commander / user */
 
@@ -230,26 +231,28 @@ private:
 	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
 	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
 	perf_counter_t	_perf_baro;			///<local performance counter for baro updates
-	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
-	perf_counter_t	_perf_reset;			///<local performance counter for filter resets
-
-	bool						_baro_init;
-	bool						_gps_initialized;
-	hrt_abstime					_gps_start_time;
-	hrt_abstime					_filter_start_time;
-	hrt_abstime					_last_sensor_timestamp;
-	hrt_abstime					_last_run;
-	hrt_abstime					_distance_last_valid;
-	bool						_gyro_valid;
-	bool						_accel_valid;
-	bool						_mag_valid;
-	int						_gyro_main;		///< index of the main gyroscope
-	int						_accel_main;		///< index of the main accelerometer
-	int						_mag_main;		///< index of the main magnetometer
-	bool						_ekf_logging;		///< log EKF state
-	unsigned					_debug;			///< debug level - default 0
-
-	int						_mavlink_fd;
+	perf_counter_t	_perf_airspeed;		///<local performance counter for airspeed updates
+	perf_counter_t	_perf_reset;		///<local performance counter for filter resets
+
+	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
+	bool			_baro_init;
+	bool			_gps_initialized;
+	hrt_abstime		_filter_start_time;
+	hrt_abstime		_last_sensor_timestamp;
+	hrt_abstime		_last_run;
+	hrt_abstime		_distance_last_valid;
+	bool			_gyro_valid;
+	bool			_accel_valid;
+	bool			_mag_valid;
+	int				_gyro_main;			///< index of the main gyroscope
+	int				_accel_main;		///< index of the main accelerometer
+	int				_mag_main;			///< index of the main magnetometer
+	bool			_ekf_logging;		///< log EKF state
+	unsigned		_debug;				///< debug level - default 0
+
+	int							_mavlink_fd;
 
 	struct {
 		int32_t	vel_delay_ms;
@@ -295,6 +298,7 @@ private:
 
 	AttPosEKF					*_ekf;
 
+private:
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -327,6 +331,38 @@ private:
 	 * @return zero if ok, non-zero for a filter error condition.
 	 */
 	int		check_filter_state();
+
+	/**
+	* @brief
+	*	Publish the euler and quaternions for attitude estimation
+	**/
+	void publishAttitude();
+
+	/**
+	* @brief
+	*	Publish local position relative to reference point where filter was initialized
+	**/
+	void publishLocalPosition();
+
+	/**
+	* @brief
+	*	Publish global position estimation (WSG84 and AMSL).
+	*	A global position estimate is only valid if we have a good GPS fix
+	**/
+	void publishGlobalPosition();
+
+	/**
+	* @brief
+	*	Publish wind estimates for north and east in m/s
+	**/
+	void publishWindEstimate();
+
+	/**
+	* @brief
+	*	
+	**/
+	void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
+			const bool fuseBaro, const bool fuseAirSpeed);
 };
 
 namespace estimator
@@ -342,12 +378,11 @@ namespace estimator
 }
 
 AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
-
 	_task_should_exit(false),
 	_task_running(false),
 	_estimator_task(-1),
 
-/* subscriptions */
+	/* subscriptions */
 #ifndef SENSOR_COMBINED_SUB
 	_gyro_sub(-1),
 	_accel_sub(-1),
@@ -365,7 +400,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mission_sub(-1),
 	_home_sub(-1),
 
-/* publications */
+	/* publications */
 	_att_pub(-1),
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -389,16 +424,16 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_accel_offsets({}),
 	_mag_offsets({}),
 
-	#ifdef SENSOR_COMBINED_SUB
+#ifdef SENSOR_COMBINED_SUB
 	_sensor_combined{},
-	#endif
+#endif
 
 	_pos_ref{},
 	_baro_ref(0.0f),
 	_baro_ref_offset(0.0f),
 	_baro_gps_offset(0.0f),
 
-/* performance counters */
+	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
 	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
 	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
@@ -408,10 +443,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
 	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
-/* states */
+	/* states */
+	_covariancePredictionDt(0.0f),
+	_gpsIsGood(false),
+	_lastGPSTimestamp(0),
 	_baro_init(false),
 	_gps_initialized(false),
-	_gps_start_time(0),
 	_filter_start_time(0),
 	_last_sensor_timestamp(0),
 	_last_run(0),
@@ -429,7 +466,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameter_handles{},
 	_ekf(nullptr)
 {
-
 	_last_run = hrt_absolute_time();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
@@ -455,7 +491,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	parameters_update();
 
 	/* get offsets */
-
 	int fd, res;
 
 	for (unsigned s = 0; s < 3; s++) {
@@ -715,7 +750,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	_ekf = new AttPosEKF();
-	float dt = 0.0f; // time lapsed since last covariance prediction
+
 	_filter_start_time = hrt_absolute_time();
 
 	if (!_ekf) {
@@ -779,7 +814,6 @@ void AttitudePositionEstimatorEKF::task_main()
 
 	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
 
-	uint64_t last_gps = 0;
 	_gps.vel_n_m_s = 0.0f;
 	_gps.vel_e_m_s = 0.0f;
 	_gps.vel_d_m_s = 0.0f;
@@ -1059,6 +1093,9 @@ void AttitudePositionEstimatorEKF::task_main()
 				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);
 
@@ -1067,21 +1104,22 @@ void AttitudePositionEstimatorEKF::task_main()
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
-				if (_gps.fix_type < 3) {
-					//Too poor GPS fix to use
-					newDataGps = false;
-
-				} else {
-
-					/* store time of valid GPS measurement */
-					_gps_start_time = hrt_absolute_time();
-
-					const float pos_reset_threshold = 5.0f; // seconds
+				//We are more strict for our first fix
+				float ephRequired = _parameters.pos_stddev_threshold;
+				if(_gpsIsGood) {
+					ephRequired = _parameters.pos_stddev_threshold * 2.0f;
+				}
 
-					//Calculate time since last good GPS fix
-					float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
+				//Check if the GPS fix is good enough for us to use
+				if(_gps.fix_type >= 3 && _gps.eph < ephRequired) {
+					_gpsIsGood = true;
+				}
+				else {
+					_gpsIsGood = false;
+				}
 
-					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
+				if (_gpsIsGood) {
+					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD));
 
 					/* fuse GPS updates */
 
@@ -1103,9 +1141,9 @@ void AttitudePositionEstimatorEKF::task_main()
 					_ekf->posNE[1] = posNED[1];
 
 					// update LPF
-					_gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt);
+					_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
 
-					//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed);
+					//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);
@@ -1122,18 +1160,24 @@ void AttitudePositionEstimatorEKF::task_main()
 					// 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 */
-					/* timeout of 5 seconds */
-					if (gps_elapsed > pos_reset_threshold) {
+					if (dtGoodGPS > POS_RESET_THRESHOLD) {
 						_ekf->ResetPosition();
 						_ekf->ResetVelocity();
 						_ekf->ResetStoredStates();
 					}
 
 					newDataGps = true;
-					last_gps = _gps.timestamp_position;
-
+					_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;
@@ -1276,7 +1320,7 @@ void AttitudePositionEstimatorEKF::task_main()
 //				}
 
 				/* Initialize the filter first */
-				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
+				if (!_gps_initialized && _gpsIsGood && _gps.epv < _parameters.pos_stddev_threshold) {
 
 					// GPS is in scaled integers, convert
 					double lat = _gps.lat / 1.0e7;
@@ -1344,289 +1388,330 @@ void AttitudePositionEstimatorEKF::task_main()
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
 					int check = check_filter_state();
-
 					if (check) {
 						// Let the system re-initialize itself
 						continue;
 					}
 
-					// Run the strapdown INS equations every IMU update
-					_ekf->UpdateStrapdownEquationsNED();
-
-					// store the predicted states for subsequent use by measurement fusion
-					_ekf->StoreStates(IMUmsec);
-					// Check if on ground - status is used by covariance prediction
-					_ekf->OnGroundCheck();
-					// sum delta angles and time used by covariance prediction
-					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
-					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
-					dt += _ekf->dtIMU;
-
-					// perform a covariance prediction if the total delta angle has exceeded the limit
-					// or the time limit will be exceeded at the next IMU update
-					if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
-						_ekf->CovariancePrediction(dt);
-						_ekf->summedDelAng.zero();
-						_ekf->summedDelVel.zero();
-						dt = 0.0f;
-					}
+					//Run EKF data fusion steps
+					updateSensorFusion(newDataGps, newDataMag, newRangeData, newHgtData, newAdsData);
+
+					//Publish attitude estimations
+					publishAttitude();
 
-					// Fuse GPS Measurements
-					if (newDataGps && _gps_initialized) {
-						// Convert GPS measurements to Pos NE, hgt and Vel NED
-
-						float gps_dt = (_gps.timestamp_position - last_gps) / 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;
-
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else if (!_gps_initialized) {
-
-						// force static mode
-						_ekf->staticMode = true;
-
-						// Convert GPS measurements to Pos NE, hgt and Vel NED
-						_ekf->velNED[0] = 0.0f;
-						_ekf->velNED[1] = 0.0f;
-						_ekf->velNED[2] = 0.0f;
-
-						_ekf->posNE[0] = 0.0f;
-						_ekf->posNE[1] = 0.0f;
-						// set fusion flags
-						_ekf->fuseVelData = true;
-						_ekf->fusePosData = true;
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else {
-						_ekf->fuseVelData = false;
-						_ekf->fusePosData = false;
+					//Publish Local Position estimations
+					publishLocalPosition();
+
+					//Publish Global Position, but only if it's any good
+					if(_gps_initialized && _gpsIsGood)
+					{
+						publishGlobalPosition();
 					}
 
-					if (newHgtData) {
-						// Could use a blend of GPS and baro alt data if desired
-						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
-						_ekf->fuseHgtData = true;
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else {
-						_ekf->fuseHgtData = false;
+					//Publish wind estimates
+					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+						publishWindEstimate();
 					}
+				}
+			}
 
-					// Fuse Magnetometer Measurements
-					if (newDataMag) {
-						_ekf->fuseMagData = true;
-						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+		}
 
-						_ekf->magstate.obsIndex = 0;
-						_ekf->FuseMagnetometer();
-						_ekf->FuseMagnetometer();
-						_ekf->FuseMagnetometer();
+		perf_end(_loop_perf);
+	}
 
-					} else {
-						_ekf->fuseMagData = false;
-					}
+	_task_running = false;
 
-					// Fuse Airspeed Measurements
-					if (newAdsData && _ekf->VtasMeas > 7.0f) {
-						_ekf->fuseVtasData = true;
-						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-						_ekf->FuseAirspeed();
+	warnx("exiting.\n");
 
-					} else {
-						_ekf->fuseVtasData = false;
-					}
+	_estimator_task = -1;
+	_exit(0);
+}
 
-					if (newRangeData) {
+void AttitudePositionEstimatorEKF::publishAttitude()
+{
+	// Output results
+	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
 
-						if (_ekf->Tnb.z.z > 0.9f) {
-							// _ekf->rngMea is set in sensor readout already
-							_ekf->fuseRngData = true;
-							_ekf->fuseOptFlowData = false;
-							_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
-							_ekf->OpticalFlowEKF();
-							_ekf->fuseRngData = false;
-						}
-					}
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(_att.R, i, j) = R(i, j);
+		}
+	}
 
+	_att.timestamp = _last_sensor_timestamp;
+	_att.q[0] = _ekf->states[0];
+	_att.q[1] = _ekf->states[1];
+	_att.q[2] = _ekf->states[2];
+	_att.q[3] = _ekf->states[3];
+	_att.q_valid = true;
+	_att.R_valid = true;
+
+	_att.timestamp = _last_sensor_timestamp;
+	_att.roll = euler(0);
+	_att.pitch = euler(1);
+	_att.yaw = euler(2);
+
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+
+	// gyro offsets
+	_att.rate_offsets[0] = _ekf->states[10];
+	_att.rate_offsets[1] = _ekf->states[11];
+	_att.rate_offsets[2] = _ekf->states[12];
+
+	/* lazily publish the attitude only once available */
+	if (_att_pub > 0) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
 
-					// Output results
-					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-					math::Matrix<3, 3> R = q.to_dcm();
-					math::Vector<3> euler = R.to_euler();
-
-					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-							PX4_R(_att.R, i, j) = R(i, j);
-
-					_att.timestamp = _last_sensor_timestamp;
-					_att.q[0] = _ekf->states[0];
-					_att.q[1] = _ekf->states[1];
-					_att.q[2] = _ekf->states[2];
-					_att.q[3] = _ekf->states[3];
-					_att.q_valid = true;
-					_att.R_valid = true;
-
-					_att.timestamp = _last_sensor_timestamp;
-					_att.roll = euler(0);
-					_att.pitch = euler(1);
-					_att.yaw = euler(2);
-
-					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
-					// gyro offsets
-					_att.rate_offsets[0] = _ekf->states[10];
-					_att.rate_offsets[1] = _ekf->states[11];
-					_att.rate_offsets[2] = _ekf->states[12];
-
-					/* lazily publish the attitude only once available */
-					if (_att_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
-
-					} else {
-						/* advertise and publish */
-						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
-					}
+	} else {
+		/* advertise and publish */
+		_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+	}
+}
 
-					//Publish position estimations
-					_local_pos.timestamp = _last_sensor_timestamp;
-					_local_pos.x = _ekf->states[7];
-					_local_pos.y = _ekf->states[8];
-					
-					// XXX need to announce change of Z reference somehow elegantly
-					_local_pos.z = _ekf->states[9] - _baro_ref_offset;
-
-					_local_pos.vx = _ekf->states[4];
-					_local_pos.vy = _ekf->states[5];
-					_local_pos.vz = _ekf->states[6];
-
-					_local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3;
-					_local_pos.z_valid = true;
-					_local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3;
-					_local_pos.v_z_valid = true;
-					_local_pos.xy_global = _gps_initialized;
-
-					_local_pos.z_global = false;
-					_local_pos.yaw = _att.yaw;
-
-					/* lazily publish the local position only once available */
-					if (_local_pos_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
-
-					} else {
-						/* advertise and publish */
-						_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
-					}
+void AttitudePositionEstimatorEKF::publishLocalPosition()
+{
+	_local_pos.timestamp = _last_sensor_timestamp;
+	_local_pos.x = _ekf->states[7];
+	_local_pos.y = _ekf->states[8];
 
-					//Publish Global Position, but only if it's any good
-					if(_gps_initialized && _gps.fix_type >= 3 && _gps.eph < _parameters.pos_stddev_threshold * 2.0f)
-					{
-						_global_pos.timestamp = _local_pos.timestamp;
-
-						if (_local_pos.xy_global) {
-							double est_lat, est_lon;
-							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
-							_global_pos.lat = est_lat;
-							_global_pos.lon = est_lon;
-							_global_pos.time_utc_usec = _gps.time_utc_usec;
-						}
-
-						if (_local_pos.v_xy_valid) {
-							_global_pos.vel_n = _local_pos.vx;
-							_global_pos.vel_e = _local_pos.vy;
-						} else {
-							_global_pos.vel_n = 0.0f;
-							_global_pos.vel_e = 0.0f;
-						}
-
-						/* local pos alt is negative, change sign and add alt offsets */
-						_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
-
-						if (_local_pos.v_z_valid) {
-							_global_pos.vel_d = _local_pos.vz;
-						}
-
-						/* terrain altitude */
-						_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
-						_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-							(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
-
-						_global_pos.yaw = _local_pos.yaw;
-
-						_global_pos.eph = _gps.eph;
-						_global_pos.epv = _gps.epv;
-
-						/* lazily publish the global position only once available */
-						if (_global_pos_pub > 0) {
-							/* publish the global position */
-							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
-
-						} else {
-							/* advertise and publish */
-							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
-						}
-					}
+	// XXX need to announce change of Z reference somehow elegantly
+	_local_pos.z = _ekf->states[9] - _baro_ref_offset;
 
-					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-						_wind.timestamp = _global_pos.timestamp;
-						_wind.windspeed_north = _ekf->states[14];
-						_wind.windspeed_east = _ekf->states[15];
-						// XXX we need to do something smart about the covariance here
-						// but we default to the estimate covariance for now
-						_wind.covariance_north = _ekf->P[14][14];
-						_wind.covariance_east = _ekf->P[15][15];
-
-						/* lazily publish the wind estimate only once available */
-						if (_wind_pub > 0) {
-							/* publish the wind estimate */
-							orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
-
-						} else {
-							/* advertise and publish */
-							_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
-						}
-					}
-				}
+	_local_pos.vx = _ekf->states[4];
+	_local_pos.vy = _ekf->states[5];
+	_local_pos.vz = _ekf->states[6];
 
-			}
+	_local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3;
+	_local_pos.z_valid = true;
+	_local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3;
+	_local_pos.v_z_valid = true;
+	_local_pos.xy_global = _gps_initialized;
+
+	_local_pos.z_global = false;
+	_local_pos.yaw = _att.yaw;
+
+	/* lazily publish the local position only once available */
+	if (_local_pos_pub > 0) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+	} else {
+		/* advertise and publish */
+		_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+	}
+}
+
+void AttitudePositionEstimatorEKF::publishGlobalPosition()
+{
+	_global_pos.timestamp = _local_pos.timestamp;
+
+	if (_local_pos.xy_global) {
+		double est_lat, est_lon;
+		map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+		_global_pos.lat = est_lat;
+		_global_pos.lon = est_lon;
+		_global_pos.time_utc_usec = _gps.time_utc_usec;
+	}
 
+	if (_local_pos.v_xy_valid) {
+		_global_pos.vel_n = _local_pos.vx;
+		_global_pos.vel_e = _local_pos.vy;
+	} else {
+		_global_pos.vel_n = 0.0f;
+		_global_pos.vel_e = 0.0f;
+	}
+
+	/* local pos alt is negative, change sign and add alt offsets */
+	_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
+
+	if (_local_pos.v_z_valid) {
+		_global_pos.vel_d = _local_pos.vz;
+	}
+
+	/* terrain altitude */
+	_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
+	_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
+		(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+
+	_global_pos.yaw = _local_pos.yaw;
+	_global_pos.eph = _gps.eph;
+	_global_pos.epv = _gps.epv;
+
+	/* lazily publish the global position only once available */
+	if (_global_pos_pub > 0) {
+		/* publish the global position */
+		orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+	} else {
+		/* advertise and publish */
+		_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+	}
+}
+
+void AttitudePositionEstimatorEKF::publishWindEstimate()
+{
+	_wind.timestamp = _global_pos.timestamp;
+	_wind.windspeed_north = _ekf->states[14];
+	_wind.windspeed_east = _ekf->states[15];
+	// XXX we need to do something smart about the covariance here
+	// but we default to the estimate covariance for now
+	_wind.covariance_north = _ekf->P[14][14];
+	_wind.covariance_east = _ekf->P[15][15];
+
+	/* lazily publish the wind estimate only once available */
+	if (_wind_pub > 0) {
+		/* publish the wind estimate */
+		orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+	} else {
+		/* advertise and publish */
+		_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+	}
+
+}
+
+void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
+	const bool fuseBaro, const bool fuseAirSpeed)
+{
+	// Run the strapdown INS equations every IMU update
+	_ekf->UpdateStrapdownEquationsNED();
+
+	// store the predicted states for subsequent use by measurement fusion
+	_ekf->StoreStates(IMUmsec);
+
+	// Check if on ground - status is used by covariance prediction
+	_ekf->OnGroundCheck();
+
+	// sum delta angles and time used by covariance prediction
+	_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+	_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+	_covariancePredictionDt += _ekf->dtIMU;
+
+	// perform a covariance prediction if the total delta angle has exceeded the limit
+	// or the time limit will be exceeded at the next IMU update
+	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) 
+		|| (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+		_ekf->CovariancePrediction(_covariancePredictionDt);
+		_ekf->summedDelAng.zero();
+		_ekf->summedDelVel.zero();
+		_covariancePredictionDt = 0.0f;
+	}
+
+	// Fuse GPS Measurements
+	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;
 		}
 
-		perf_end(_loop_perf);
+		// set fusion flags
+		_ekf->fuseVelData = true;
+		_ekf->fusePosData = true;
+
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+
+		// run the fusion step
+		_ekf->FuseVelposNED();
+
+	} 
+	else if (!_gps_initialized) {
+
+		// force static mode
+		_ekf->staticMode = true;
+
+		// Convert GPS measurements to Pos NE, hgt and Vel NED
+		_ekf->velNED[0] = 0.0f;
+		_ekf->velNED[1] = 0.0f;
+		_ekf->velNED[2] = 0.0f;
+
+		_ekf->posNE[0] = 0.0f;
+		_ekf->posNE[1] = 0.0f;
+
+		// set fusion flags
+		_ekf->fuseVelData = true;
+		_ekf->fusePosData = true;
+
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+
+		// run the fusion step
+		_ekf->FuseVelposNED();
+
+	} 
+	else {
+		_ekf->fuseVelData = false;
+		_ekf->fusePosData = false;
 	}
 
-	_task_running = false;
+	if (fuseBaro) {
+		// Could use a blend of GPS and baro alt data if desired
+		_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+		_ekf->fuseHgtData = true;
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+		// run the fusion step
+		_ekf->FuseVelposNED();
+
+	} 
+	else {
+		_ekf->fuseHgtData = false;
+	}
 
-	warnx("exiting.\n");
+	// Fuse Magnetometer Measurements
+	if (fuseMag) {
+		_ekf->fuseMagData = true;
+		_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
-	_estimator_task = -1;
-	_exit(0);
+		_ekf->magstate.obsIndex = 0;
+		_ekf->FuseMagnetometer();
+		_ekf->FuseMagnetometer();
+		_ekf->FuseMagnetometer();
+
+	} 
+	else {
+		_ekf->fuseMagData = false;
+	}
+
+	// Fuse Airspeed Measurements
+	if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
+		_ekf->fuseVtasData = true;
+		_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+		_ekf->FuseAirspeed();
+
+	} 
+	else {
+		_ekf->fuseVtasData = false;
+	}
+
+	// Fuse Rangefinder Measurements
+	if (fuseRangeSensor) {
+		if (_ekf->Tnb.z.z > 0.9f) {
+			// _ekf->rngMea is set in sensor readout already
+			_ekf->fuseRngData = true;
+			_ekf->fuseOptFlowData = false;
+			_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
+			_ekf->OpticalFlowEKF();
+			_ekf->fuseRngData = false;
+		}
+	}
 }
 
 int AttitudePositionEstimatorEKF::start()