Merge branch 'mtecs' into mtecs_takeoff

30 files changed, 1007 insertions, 695 deletions

diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index 6e1a531cf..fff4b58df 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -33,3 +33,6 @@ then
 fi
 
 set MIXER FMU_Q
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index e4a8b3c05..985e6ffd9 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -389,18 +389,22 @@ class uploader(object):
                     self.otp_pid = self.otp[12:8:-1]
                     self.otp_coa = self.otp[32:160]
                     # show user:
-                    print("type: " + self.otp_id.decode('Latin-1'))
-                    print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
-                    print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
-                    print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
-                    print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
-                    print("sn: ", end='')
-                    for byte in range(0,12,4):
-                        x = self.__getSN(byte)
-                        x = x[::-1]  # reverse the bytes
-                        self.sn  = self.sn + x
-                        print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
-                    print('')
+                    try:
+                            print("type: " + self.otp_id.decode('Latin-1'))
+                            print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
+                            print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
+                            print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
+                            print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
+                            print("sn: ", end='')
+                            for byte in range(0,12,4):
+                                    x = self.__getSN(byte)
+                                    x = x[::-1]  # reverse the bytes
+                                    self.sn  = self.sn + x
+                                    print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
+                            print('')
+                    except Exception:
+                            # ignore bad character encodings
+                            pass
                 print("erase...")
                 self.__erase()
 
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index cb5fd53a5..147d7123a 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -147,6 +147,7 @@ enum {
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
 	TONE_GPS_WARNING_TUNE,
+	TONE_ARMING_FAILURE_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4c85c0cda..cb97354ec 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -169,6 +169,8 @@ private:
 
 	int			_bus;			/**< the bus the device is connected to */
 
+	struct mag_report	_last_report;           /**< used for info() */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -870,6 +872,8 @@ HMC5883::collect()
 		}
 	}
 
+	_last_report = new_report;
+
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
@@ -1042,31 +1046,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-		goto out;
-	}
-
-	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
-		goto out;
-	}
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
 	mscale_previous.z_scale = scaling[2];
 
+	ret = OK;
+
+out:
+
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
 		warn("WARNING: failed to set new scale / offsets for mag");
-		goto out;
 	}
 
-	ret = OK;
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+	}
 
-out:
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+	}
 
 	if (ret == OK) {
 		if (!check_scale()) {
@@ -1221,6 +1222,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("output  (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
 	printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
 	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
 	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 2c3efdc35..1ad383ee0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -87,6 +87,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
 
 #include <drivers/airspeed/airspeed.h>
 
@@ -121,6 +122,14 @@ protected:
 	virtual int	collect();
 
 	math::LowPassFilter2p	_filter;
+
+	/**
+	 * Correct for 5V rail voltage variations
+	 */
+	void voltage_correction(float &diff_pres_pa, float &temperature);
+
+	int _t_system_power;
+	struct system_power_s system_power;
 };
 
 /*
@@ -129,10 +138,11 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
-			CONVERSION_INTERVAL, path),
-			_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
+	CONVERSION_INTERVAL, path),
+	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
+	_t_system_power(-1)
 {
-
+	memset(&system_power, 0, sizeof(system_power));
 }
 
 int
@@ -194,19 +204,48 @@ MEASAirspeed::collect()
 	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
-	float temperature = ((200 * dT_raw) / 2047) - 50;
+	float temperature = ((200.0f * dT_raw) / 2047) - 50;
 
-	/* calculate differential pressure. As its centered around 8000
-	 * and can go positive or negative, enforce absolute value
-	*/
+	// Calculate differential pressure. As its centered around 8000
+	// and can go positive or negative
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+	const float PSI_to_Pa = 6894.757f;
+	/*
+	  this equation is an inversion of the equation in the
+	  pressure transfer function figure on page 4 of the datasheet
 
-	if (diff_press_pa < 0.0f) {
-		diff_press_pa = 0.0f;
-	}
+	  We negate the result so that positive differential pressures
+	  are generated when the bottom port is used as the static
+	  port on the pitot and top port is used as the dynamic port
+	 */
+	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
+	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
 
+        // correct for 5V rail voltage if possible
+        voltage_correction(diff_press_pa_raw, temperature);
+
+	float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
+	
+	/*
+	  note that we return both the absolute value with offset
+	  applied and a raw value without the offset applied. This
+	  makes it possible for higher level code to detect if the
+	  user has the tubes connected backwards, and also makes it
+	  possible to correctly use offsets calculated by a higher
+	  level airspeed driver.
+
+	  With the above calculation the MS4525 sensor will produce a
+	  positive number when the top port is used as a dynamic port
+	  and bottom port is used as the static port
+
+	  Also note that the _diff_pres_offset is applied before the
+	  fabsf() not afterwards. It needs to be done this way to
+	  prevent a bias at low speeds, but this also means that when
+	  setting a offset you must set it based on the raw value, not
+	  the offset value
+	 */
+	
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
@@ -219,6 +258,13 @@ MEASAirspeed::collect()
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
+
+	/* the dynamics of the filter can make it overshoot into the negative range */
+	if (report.differential_pressure_filtered_pa < 0.0f) {
+		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
+	}
+
+	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -288,6 +334,62 @@ MEASAirspeed::cycle()
 }
 
 /**
+   correct for 5V rail voltage if the system_power ORB topic is
+   available
+   
+   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
+   offset versus voltage for 3 sensors
+ */
+void
+MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	if (_t_system_power == -1) {
+		_t_system_power = orb_subscribe(ORB_ID(system_power));
+	}
+	if (_t_system_power == -1) {
+		// not available
+		return;
+	}
+	bool updated = false;
+	orb_check(_t_system_power, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
+	}
+	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
+		// not valid, skip correction
+		return;
+	}
+
+	const float slope = 65.0f;
+	/*
+	  apply a piecewise linear correction, flattening at 0.5V from 5V
+	 */
+	float voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -0.5f) {
+		voltage_diff = -0.5f;
+	}
+	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+	temperature -= voltage_diff * temp_slope;	
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+}
+
+/**
  * Local functions in support of the shell command.
  */
 namespace meas_airspeed
@@ -409,7 +511,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -437,7 +539,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82f3ba044..e318e206a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -944,8 +944,23 @@ PX4IO::task_main()
 				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
 
 				if (pret != OK) {
-					log("voltage scaling upload failed");
+					log("vscale upload failed");
 				}
+
+				/* send RC throttle failsafe value to IO */
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
+
+				if (failsafe_param > 0) {
+
+					param_get(failsafe_param, &failsafe_param_val);
+					uint16_t failsafe_thr = failsafe_param_val;
+					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+					if (pret != OK) {
+						log("failsafe upload failed");
+					}
+				}
+
 			}
 
 		}
@@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, only publish if RC OK flag is set */
-		/* if no raw RC, just don't publish */
-		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		/* only keep publishing RC input if we ever got a valid input */
+		if (_rc_last_valid == 0) {
+			/* we have never seen valid RC signals, abort */
 			return OK;
+		}
 	}
 
 	/* lazily advertise on first publication */
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 70cd1ab1e..a0cf98340 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -124,6 +124,8 @@ private:
 
 	orb_advert_t			_range_finder_topic;
 
+	unsigned			_consecutive_fail_count;
+
 	perf_counter_t			_sample_perf;
 	perf_counter_t			_comms_errors;
 	perf_counter_t			_buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_last_read(0),
 	_range_finder_topic(-1),
+	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -276,34 +279,11 @@ SF0X::init()
 		warnx("advert err");
 	}
 
-	/* attempt to get a measurement 5 times */
-	while (ret != OK && i < 5) {
-
-		if (measure()) {
-			ret = ERROR;
-			_sensor_ok = false;
-		}
-
-		usleep(100000);
-
-		if (collect()) {
-			ret = ERROR;
-			_sensor_ok = false;
-
-		} else {
-			ret = OK;
-			/* sensor is ok, but we don't really know if it is within range */
-			_sensor_ok = true;
-		}
-
-		i++;
-	}
-
 	/* close the fd */
 	::close(_fd);
 	_fd = -1;
 out:
-	return ret;
+	return OK;
 }
 
 int
@@ -376,6 +356,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			/* adjust to a legal polling interval in Hz */
 			default: {
+
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
 
@@ -544,10 +525,16 @@ SF0X::collect()
 
 	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
 		_linebuf_index = 0;
+	} else if (_linebuf_index > 0) {
+		/* increment to next read position */
+		_linebuf_index++;
 	}
 
+	/* the buffer for read chars is buflen minus null termination */
+	unsigned readlen = sizeof(_linebuf) - 1;
+
 	/* read from the sensor (uart buffer) */
-	ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
+	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
 
 	if (ret < 0) {
 		_linebuf[sizeof(_linebuf) - 1] = '\0';
@@ -562,19 +549,30 @@ SF0X::collect()
 		} else {
 			return -EAGAIN;
 		}
+	} else if (ret == 0) {
+		return -EAGAIN;
 	}
 
-	_linebuf_index += ret;
-
-	if (_linebuf_index >= sizeof(_linebuf)) {
-		_linebuf_index = 0;
-	}
+	/* we did increment the index to the next position already, so just add the additional fields */
+	_linebuf_index += (ret - 1);
 
 	_last_read = hrt_absolute_time();
 
-	if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
-		/* incomplete read, reschedule ourselves */
+	if (_linebuf_index < 1) {
+		/* we need at least the two end bytes to make sense of this string */
 		return -EAGAIN;
+
+	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
+
+		if (_linebuf_index >= readlen - 1) {
+			/* we have a full buffer, but no line ending - abort */
+			_linebuf_index = 0;
+			perf_count(_comms_errors);
+			return -ENOMEM;
+		} else {
+			/* incomplete read, reschedule ourselves */
+			return -EAGAIN;
+		}
 	}
 
 	char *end;
@@ -582,22 +580,56 @@ SF0X::collect()
 	bool valid;
 
 	/* enforce line ending */
-	_linebuf[sizeof(_linebuf) - 1] = '\0';
+	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
+
+	_linebuf[lend] = '\0';
 
 	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
 		si_units = -1.0f;
 		valid = false;
 
 	} else {
-		si_units = strtod(_linebuf, &end);
-		valid = true;
+
+		/* we need to find a dot in the string, as we're missing the meters part else */
+		valid = false;
+
+		/* wipe out partially read content from last cycle(s), check for dot */
+		for (int i = 0; i < (lend - 2); i++) {
+			if (_linebuf[i] == '\n') {
+				char buf[sizeof(_linebuf)];
+				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
+				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
+			}
+
+			if (_linebuf[i] == '.') {
+				valid = true;
+			}
+		}
+
+		if (valid) {
+			si_units = strtod(_linebuf, &end);
+
+			/* we require at least 3 characters for a valid number */
+			if (end > _linebuf + 3) {
+				valid = true;
+			} else {
+				si_units = -1.0f;
+				valid = false;
+			}
+		}
 	}
 
-	debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
+	debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
 
-	/* done with this chunk, resetting */
+	/* done with this chunk, resetting - even if invalid */
 	_linebuf_index = 0;
 
+	/* if its invalid, there is no reason to forward the value */
+	if (!valid) {
+		perf_count(_comms_errors);
+		return -EINVAL;
+	}
+
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -689,10 +721,19 @@ SF0X::cycle()
 		}
 
 		if (OK != collect_ret) {
-			log("collection error");
+
+			/* we know the sensor needs about four seconds to initialize */
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				log("collection error #%u", _consecutive_fail_count);
+			}
+			_consecutive_fail_count++;
+
 			/* restart the measurement state machine */
 			start();
 			return;
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
 		}
 
 		/* next phase is measurement */
@@ -848,10 +889,10 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
-	warnx("time:        %lld", report.timestamp);
+	warnx("val:  %0.2f m", (double)report.distance);
+	warnx("time: %lld", report.timestamp);
 
-	/* start the sensor polling at 2Hz */
+	/* start the sensor polling at 2 Hz rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
 	}
@@ -866,24 +907,26 @@ test()
 		int ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			warnx("timed out");
+			break;
 		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			err(1, "periodic read failed");
+			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
+			break;
 		}
 
-		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
-		warnx("time:        %lld", report.timestamp);
+		warnx("read #%u", i);
+		warnx("val:  %0.3f m", (double)report.distance);
+		warnx("time: %lld", report.timestamp);
 	}
 
-	/* reset the sensor polling to default rate */
+	/* reset the sensor polling to the default rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		errx(1, "failed to set default poll rate");
+		errx(1, "ERR: DEF RATE");
 	}
 
 	errx(0, "PASS");
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 0b8a275e6..3a60d2cae 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -41,6 +41,7 @@
  */
 
 #include <nuttx/config.h>
+#include <board_config.h>
 #include <drivers/device/device.h>
 
 #include <sys/types.h>
@@ -64,6 +65,8 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 
+#include <uORB/topics/system_power.h>
+
 /*
  * Register accessors.
  * For now, no reason not to just use ADC1.
@@ -119,6 +122,8 @@ private:
 	unsigned		_channel_count;
 	adc_msg_s		*_samples;		/**< sample buffer */
 
+	orb_advert_t		_to_system_power;
+
 	/** work trampoline */
 	static void		_tick_trampoline(void *arg);
 
@@ -134,13 +139,16 @@ private:
 	 */
 	uint16_t		_sample(unsigned channel);
 
+	// update system_power ORB topic, only on FMUv2
+	void update_system_power(void);
 };
 
 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
 	_channel_count(0),
-	_samples(nullptr)
+	_samples(nullptr),
+	_to_system_power(0)
 {
 	_debug_enabled = true;
 
@@ -290,6 +298,43 @@ ADC::_tick()
 	/* scan the channel set and sample each */
 	for (unsigned i = 0; i < _channel_count; i++)
 		_samples[i].am_data = _sample(_samples[i].am_channel);
+	update_system_power();
+}
+
+void
+ADC::update_system_power(void)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	system_power_s system_power;
+	system_power.timestamp = hrt_absolute_time();
+
+	system_power.voltage5V_v = 0;
+	for (unsigned i = 0; i < _channel_count; i++) {
+		if (_samples[i].am_channel == 4) {
+			// it is 2:1 scaled
+			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
+		}
+	}
+
+	// these are not ADC related, but it is convenient to
+	// publish these to the same topic
+	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+
+	// note that the valid pins are active low
+	system_power.brick_valid   = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
+	system_power.servo_valid   = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
+
+	// OC pins are active low
+	system_power.periph_5V_OC  = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
+	system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
+
+	/* lazily publish */
+	if (_to_system_power > 0) {
+		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+	} else {
+		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
+	}
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
 uint16_t
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index f36f2091e..8ed0de58c 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
+	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -344,6 +345,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
+	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index 3699d9bce..6f640c9f9 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
+        // don't allow bad values to propagate via the filter
         delay_element_0 = sample;
     }
     float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
     return output;
 }
 
+float LowPassFilter2p::reset(float sample) {
+    _delay_element_1 = _delay_element_2 = sample;
+    return apply(sample);
+}
+
 } // namespace math
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..74cd5d78c 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -52,18 +52,30 @@ public:
         _delay_element_1 = _delay_element_2 = 0;
     }
 
-    // change parameters
+    /**
+     * Change filter parameters
+     */
     void set_cutoff_frequency(float sample_freq, float cutoff_freq);
 
-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
+    /**
+     * Add a new raw value to the filter
+     *
+     * @return retrieve the filtered result
+     */
     float apply(float sample);
 
-    // return the cutoff frequency
+    /**
+     * Return the cutoff frequency
+     */
     float get_cutoff_freq(void) const {
         return _cutoff_freq;
     }
 
+    /**
+     * Reset the filter state to this value
+     */
+    float reset(float sample);
+
 private:
     float           _cutoff_freq; 
     float           _a1;
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 6039d92a7..c8c7a42e7 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
 
-	const int calibration_count = 2500;
+	const int calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+		} else {
+			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
 		close(fd);
 	}
 
 	if (!paramreset_successful) {
-		warn("WARNING: failed to set scale / offsets for airspeed sensor");
-		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		warn("FAILED to set scale / offsets for airspeed");
+		mavlink_log_critical(mavlink_fd, "dpress reset failed");
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		return ERROR;
 	}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cf7ba757e..3720b5a3b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -118,8 +118,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
 #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
-#define RC_TIMEOUT 100000
-#define RC_TIMEOUT_HIL 500000
+#define RC_TIMEOUT 500000
 #define DIFFPRESS_TIMEOUT 2000000
 
 #define PRINT_INTERVAL	5000000
@@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-
-		/*
-		 * XXX workaround:
-		 * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
-		 * which can trigger RC loss if the computer/simulator lags.
-		 */
-		uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
-
 		/* start RC input check */
-		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index 46e405526..c31217393 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -2,85 +2,6 @@
 
 #include <string.h>
 
-// Global variables
-float KH[n_states][n_states]; //  intermediate result used for covariance updates
-float KHP[n_states][n_states]; // intermediate result used for covariance updates
-float P[n_states][n_states]; // covariance matrix
-float Kfusion[n_states]; // Kalman gains
-float states[n_states]; // state matrix
-Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
-Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
-Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
-Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
-float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
-Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
-Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
-Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
-Vector3f dVelIMU;
-Vector3f dAngIMU;
-float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
-uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
-float innovVelPos[6]; // innovation output
-float varInnovVelPos[6]; // innovation variance output
-
-float velNED[3]; // North, East, Down velocity obs (m/s)
-float posNE[2]; // North, East position obs (m)
-float hgtMea; //  measured height (m)
-float posNED[3]; // North, East Down position (m)
-
-float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
-float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
-float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
-
-float innovMag[3]; // innovation output
-float varInnovMag[3]; // innovation variance output
-Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-float innovVtas; // innovation output
-float varInnovVtas; // innovation variance output
-float VtasMeas; // true airspeed measurement (m/s)
-float latRef; // WGS-84 latitude of reference point (rad)
-float lonRef; // WGS-84 longitude of reference point (rad)
-float hgtRef; // WGS-84 height of reference point (m)
-Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
-uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
-float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
-
-// GPS input data variables
-float gpsCourse;
-float gpsVelD;
-float gpsLat;
-float gpsLon;
-float gpsHgt;
-uint8_t GPSstatus;
-
-float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
-uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
-
-// Baro input
-float baroHgt;
-
-bool statesInitialised = false;
-
-bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
-bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
-bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
-bool fuseMagData = false; // boolean true when magnetometer data is to be fused
-bool fuseVtasData = false; // boolean true when airspeed data is to be fused
-
-bool onGround    = true;    ///< boolean true when the flight vehicle is on the ground (not flying)
-bool staticMode  = true;    ///< boolean true if no position feedback is fused
-bool useAirspeed = true;    ///< boolean true if airspeed data is being used
-bool useCompass  = true;    ///< boolean true if magnetometer data is being used
-
-struct ekf_status_report current_ekf_state;
-struct ekf_status_report last_ekf_error;
-
-bool numericalProtection = true;
-
-static unsigned storeIndex = 0;
-
 float Vector3f::length(void) const
 {
     return sqrt(x*x + y*y + z*z);
@@ -185,7 +106,31 @@ void swap_var(float &d1, float &d2)
     d2 = tmp;
 }
 
-void  UpdateStrapdownEquationsNED()
+AttPosEKF::AttPosEKF() :
+    fusionModeGPS(0),
+    covSkipCount(0),
+    EAS2TAS(1.0f),
+    statesInitialised(false),
+    fuseVelData(false),
+    fusePosData(false),
+    fuseHgtData(false),
+    fuseMagData(false),
+    fuseVtasData(false),
+    onGround(true),
+    staticMode(true),
+    useAirspeed(true),
+    useCompass(true),
+    numericalProtection(true),
+    storeIndex(0)
+{
+
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
 {
     Vector3f delVelNav;
     float q00;
@@ -247,7 +192,7 @@ void  UpdateStrapdownEquationsNED()
     qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
 
 // Normalise the quaternions and update the quaternion states
-    quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
     if (quatMag > 1e-16f)
     {
         float quatMagInv = 1.0f/quatMag;
@@ -312,7 +257,7 @@ void  UpdateStrapdownEquationsNED()
     ConstrainStates();
 }
 
-void CovariancePrediction(float dt)
+void AttPosEKF::CovariancePrediction(float dt)
 {
     // scalars
     float windVelSigma;
@@ -953,7 +898,7 @@ void CovariancePrediction(float dt)
     ConstrainVariances();
 }
 
-void FuseVelposNED()
+void AttPosEKF::FuseVelposNED()
 {
 
 // declare variables used by fault isolation logic
@@ -999,8 +944,8 @@ void FuseVelposNED()
         observation[5] = -(hgtMea);
 
         // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
-        velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring
-        posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring
+        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
         R_OBS[0] = 0.04f + sq(velErr);
         R_OBS[1] = R_OBS[0];
         R_OBS[2] = 0.08f + sq(velErr);
@@ -1026,7 +971,7 @@ void FuseVelposNED()
                 varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
             }
             // apply a 5-sigma threshold
-            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
             current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
             if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
             {
@@ -1175,7 +1120,7 @@ void FuseVelposNED()
     //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
 }
 
-void FuseMagnetometer()
+void AttPosEKF::FuseMagnetometer()
 {
     uint8_t obsIndex;
     uint8_t indexLimit;
@@ -1482,7 +1427,7 @@ void FuseMagnetometer()
     ConstrainVariances();
 }
 
-void FuseAirspeed()
+void AttPosEKF::FuseAirspeed()
 {
     float vn;
     float ve;
@@ -1503,14 +1448,14 @@ void FuseAirspeed()
 
     // Need to check that it is flying before fusing airspeed data
     // Calculate the predicted airspeed
-    VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
     // Perform fusion of True Airspeed measurement
-    if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0))
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
     {
         // Calculate observation jacobians
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
-        SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
-        SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
+        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
         
         float H_TAS[21];
         for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
@@ -1611,7 +1556,7 @@ void FuseAirspeed()
     ConstrainVariances();
 }
 
-void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
@@ -1624,7 +1569,7 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
     }
 }
 
-void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
@@ -1637,13 +1582,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
     }
 }
 
-float sq(float valIn)
+float AttPosEKF::sq(float valIn)
 {
     return valIn*valIn;
 }
 
 // Store states in a history array along with time stamp
-void StoreStates(uint64_t timestamp_ms)
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
 {
     for (unsigned i=0; i<n_states; i++)
         storedStates[i][storeIndex] = states[i];
@@ -1653,7 +1598,7 @@ void StoreStates(uint64_t timestamp_ms)
         storeIndex = 0;
 }
 
-void ResetStoredStates()
+void AttPosEKF::ResetStoredStates()
 {
     // reset all stored states
     memset(&storedStates[0][0], 0, sizeof(storedStates));
@@ -1674,7 +1619,7 @@ void ResetStoredStates()
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
-int RecallStates(float statesForFusion[n_states], uint64_t msec)
+int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
     int ret = 0;
 
@@ -1723,7 +1668,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
     return ret;
 }
 
-void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
 {
     // Calculate the nav to body cosine matrix
     float q00 = sq(quat[0]);
@@ -1748,7 +1693,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
     Tnb.y.z = 2*(q23 + q01);
 }
 
-void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
 {
     // Calculate the body to nav cosine matrix
     float q00 = sq(quat[0]);
@@ -1773,7 +1718,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
     Tbn.z.y = 2*(q23 + q01);
 }
 
-void eul2quat(float (&quat)[4], const float (&eul)[3])
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
 {
     float u1 = cos(0.5f*eul[0]);
     float u2 = cos(0.5f*eul[1]);
@@ -1787,35 +1732,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3])
     quat[3] = u1*u2*u6-u4*u5*u3;
 }
 
-void quat2eul(float (&y)[3], const float (&u)[4])
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
 {
-    y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
-    y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2]));
-    y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
 }
 
-void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
 {
-    velNED[0] = gpsGndSpd*cos(gpsCourse);
-    velNED[1] = gpsGndSpd*sin(gpsCourse);
+    velNED[0] = gpsGndSpd*cosf(gpsCourse);
+    velNED[1] = gpsGndSpd*sinf(gpsCourse);
     velNED[2] = gpsVelD;
 }
 
-void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
 {
     posNED[0] = earthRadius * (lat - latRef);
     posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
     posNED[2] = -(hgt - hgtRef);
 }
 
-void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
 {
     lat = latRef + posNED[0] * earthRadiusInv;
     lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
     hgt = hgtRef - posNED[2];
 }
 
-void OnGroundCheck()
+void AttPosEKF::OnGroundCheck()
 {
     onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
     if (staticMode) {
@@ -1823,7 +1768,7 @@ void OnGroundCheck()
     }
 }
 
-void calcEarthRateNED(Vector3f &omega, float latitude)
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
 {
     //Define Earth rotation vector in the NED navigation frame
     omega.x  = earthRate*cosf(latitude);
@@ -1831,13 +1776,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
     omega.z  = -earthRate*sinf(latitude);
 }
 
-void CovarianceInit()
+void AttPosEKF::CovarianceInit()
 {
     // Calculate the initial covariance matrix P
-    P[0][0]   = 0.25f*sq(1.0f*deg2rad);
-    P[1][1]   = 0.25f*sq(1.0f*deg2rad);
-    P[2][2]   = 0.25f*sq(1.0f*deg2rad);
-    P[3][3]   = 0.25f*sq(10.0f*deg2rad);
+    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
+    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
+    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
+    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
     P[4][4]   = sq(0.7);
     P[5][5]   = P[4][4];
     P[6][6]   = sq(0.7);
@@ -1857,12 +1802,12 @@ void CovarianceInit()
     P[20][20] = P[18][18];
 }
 
-float ConstrainFloat(float val, float min, float max)
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
 {
     return (val > max) ? max : ((val < min) ? min : val);
 }
 
-void ConstrainVariances()
+void AttPosEKF::ConstrainVariances()
 {
     if (!numericalProtection) {
         return;
@@ -1914,7 +1859,7 @@ void ConstrainVariances()
 
 }
 
-void ConstrainStates()
+void AttPosEKF::ConstrainStates()
 {
     if (!numericalProtection) {
         return;
@@ -1971,7 +1916,7 @@ void ConstrainStates()
 
 }
 
-void ForceSymmetry()
+void AttPosEKF::ForceSymmetry()
 {
     if (!numericalProtection) {
         return;
@@ -1989,7 +1934,7 @@ void ForceSymmetry()
     }
 }
 
-bool FilterHealthy()
+bool AttPosEKF::FilterHealthy()
 {
     if (!statesInitialised) {
         return false;
@@ -2012,7 +1957,7 @@ bool FilterHealthy()
  * This resets the position to the last GPS measurement
  * or to zero in case of static position.
  */
-void ResetPosition(void)
+void AttPosEKF::ResetPosition(void)
 {
     if (staticMode) {
         states[7] = 0;
@@ -2030,7 +1975,7 @@ void ResetPosition(void)
  *
  * This resets the height state with the last altitude measurements
  */
-void ResetHeight(void)
+void AttPosEKF::ResetHeight(void)
 {
     // write to the state vector
     states[9]   = -hgtMea;
@@ -2039,7 +1984,7 @@ void ResetHeight(void)
 /**
  * Reset the velocity state.
  */
-void ResetVelocity(void)
+void AttPosEKF::ResetVelocity(void)
 {
     if (staticMode) {
         states[4] = 0.0f;
@@ -2054,7 +1999,7 @@ void ResetVelocity(void)
 }
 
 
-void FillErrorReport(struct ekf_status_report *err)
+void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
 {
     for (int i = 0; i < n_states; i++)
     {
@@ -2069,7 +2014,7 @@ void FillErrorReport(struct ekf_status_report *err)
     err->hgtTimeout = current_ekf_state.hgtTimeout;
 }
 
-bool StatesNaN(struct ekf_status_report *err_report) {
+bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
     bool err = false;
 
     // check all states and covariance matrices
@@ -2122,7 +2067,7 @@ bool StatesNaN(struct ekf_status_report *err_report) {
  * updated, but before any of the fusion steps are
  * executed.
  */
-int CheckAndBound()
+int AttPosEKF::CheckAndBound()
 {
 
     // Store the old filter state
@@ -2164,7 +2109,7 @@ int CheckAndBound()
     return 0;
 }
 
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
 {
     float initialRoll, initialPitch;
     float cosRoll, sinRoll, cosPitch, sinPitch;
@@ -2200,7 +2145,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
     initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
 }
 
-void InitializeDynamic(float (&initvelNED)[3])
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
 {
 
     // Clear the init flag
@@ -2254,7 +2199,7 @@ void InitializeDynamic(float (&initvelNED)[3])
     summedDelVel.z = 0.0f;
 }
 
-void InitialiseFilter(float (&initvelNED)[3])
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
 {
     //store initial lat,long and height
     latRef = gpsLat;
@@ -2266,7 +2211,7 @@ void InitialiseFilter(float (&initvelNED)[3])
     InitializeDynamic(initvelNED);
 }
 
-void ZeroVariables()
+void AttPosEKF::ZeroVariables()
 {
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
@@ -2292,12 +2237,12 @@ void ZeroVariables()
     memset(&current_ekf_state, 0, sizeof(current_ekf_state));
 }
 
-void GetFilterState(struct ekf_status_report *state)
+void AttPosEKF::GetFilterState(struct ekf_status_report *state)
 {
     memcpy(state, &current_ekf_state, sizeof(state));
 }
 
-void GetLastErrorState(struct ekf_status_report *last_error)
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
 {
     memcpy(last_error, &last_ekf_error, sizeof(last_error));
 }
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index c5a5e9d8d..e62f1a98a 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -48,85 +48,10 @@ void swap_var(float &d1, float &d2);
 const unsigned int n_states = 21;
 const unsigned int data_buffer_size = 50;
 
-extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
-extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
-extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
-extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
-extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
-extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
-extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
-extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
-extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
-extern Vector3f dVelIMU;
-extern Vector3f dAngIMU;
-
-extern float P[n_states][n_states]; // covariance matrix
-extern float Kfusion[n_states]; // Kalman gains
-extern float states[n_states]; // state matrix
-extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
-
-extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
-extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
-extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
-extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
-
-extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
-
-extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
-extern bool useAirspeed; // boolean true if airspeed data is being used
-extern bool useCompass; // boolean true if magnetometer data is being used
-extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
-extern float innovVelPos[6]; // innovation output
-extern float varInnovVelPos[6]; // innovation variance output
-
-extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
-extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
-extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
-extern bool fuseMagData; // boolean true when magnetometer data is to be fused
-
-extern float velNED[3]; // North, East, Down velocity obs (m/s)
-extern float posNE[2]; // North, East position obs (m)
-extern float hgtMea; //  measured height (m)
-extern float posNED[3]; // North, East Down position (m)
-
-extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
-
-extern float innovMag[3]; // innovation output
-extern float varInnovMag[3]; // innovation variance output
-extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
-extern float innovVtas; // innovation output
-extern float varInnovVtas; // innovation variance output
-extern bool fuseVtasData; // boolean true when airspeed data is to be fused
-extern float VtasMeas; // true airspeed measurement (m/s)
-extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
-extern float latRef; // WGS-84 latitude of reference point (rad)
-extern float lonRef; // WGS-84 longitude of reference point (rad)
-extern float hgtRef; // WGS-84 height of reference point (m)
-extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
-extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
-extern float EAS2TAS; // ratio f true to equivalent airspeed
-
-// GPS input data variables
-extern float gpsCourse;
-extern float gpsVelD;
-extern float gpsLat;
-extern float gpsLon;
-extern float gpsHgt;
-extern uint8_t GPSstatus;
-
-// Baro input
-extern float baroHgt;
-
-extern bool statesInitialised;
-extern bool numericalProtection;
-
 const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
 const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
 
-extern bool staticMode;
+// extern bool staticMode;
 
 enum GPS_FIX {
     GPS_FIX_NOFIX = 0,
@@ -150,6 +75,93 @@ struct ekf_status_report {
     bool kalmanGainsNaN;
 };
 
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float posNED[3]; // North, East Down position (m)
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float innovVtas; // innovation output
+    float varInnovVtas; // innovation variance output
+    float VtasMeas; // true airspeed measurement (m/s)
+    float latRef; // WGS-84 latitude of reference point (rad)
+    float lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    // GPS input data variables
+    float gpsCourse;
+    float gpsVelD;
+    float gpsLat;
+    float gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
 void  UpdateStrapdownEquationsNED();
 
 void CovariancePrediction(float dt);
@@ -164,8 +176,6 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
 void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
-float sq(float valIn);
-
 void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 
 // store staes along with system time stamp in msces
@@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
 
 void calcEarthRateNED(Vector3f &omega, float latitude);
 
-void eul2quat(float (&quat)[4], const float (&eul)[3]);
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
-void quat2eul(float (&eul)[3], const float (&quat)[4]);
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
 
-void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
 
-void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
 
 void OnGroundCheck();
 
@@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err);
 
 void InitializeDynamic(float (&initvelNED)[3]);
 
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
+
+};
+
 uint32_t millis();
 
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index c9d75bce4..840cd585e 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -124,6 +124,16 @@ public:
 	 */
 	int		start();
 
+	/**
+	 * Print the current status.
+	 */
+	void		print_status();
+
+	/**
+	 * Trip the filter by feeding it NaN values.
+	 */
+	int		trip_nan();
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -199,6 +209,7 @@ private:
 		param_t	tas_delay_ms;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
+	AttPosEKF					*_ekf;
 
 	/**
 	 * Update our local parameter cache.
@@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() :
 /* states */
 	_initialized(false),
 	_gps_initialized(false),
-	_mavlink_fd(-1)
+	_mavlink_fd(-1),
+	_ekf(nullptr)
 {
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -384,6 +396,12 @@ void
 FixedwingEstimator::task_main()
 {
 
+	_ekf = new AttPosEKF();
+
+	if (!_ekf) {
+		errx(1, "failed allocating EKF filter - out of RAM!");
+	}
+
 	/*
 	 * do subscriptions
 	 */
@@ -414,23 +432,23 @@ FixedwingEstimator::task_main()
 	parameters_update();
 
 	/* set initial filter state */
-	fuseVelData = false;
-	fusePosData = false;
-	fuseHgtData = false;
-	fuseMagData = false;
-	fuseVtasData = false;
-	statesInitialised = false;
+	_ekf->fuseVelData = false;
+	_ekf->fusePosData = false;
+	_ekf->fuseHgtData = false;
+	_ekf->fuseMagData = false;
+	_ekf->fuseVtasData = false;
+	_ekf->statesInitialised = false;
 
 	/* initialize measurement data */
-	VtasMeas = 0.0f;
+	_ekf->VtasMeas = 0.0f;
 	Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
 	Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
-	dVelIMU.x = 0.0f;
-	dVelIMU.y = 0.0f;
-	dVelIMU.z = 0.0f;
-	dAngIMU.x = 0.0f;
-	dAngIMU.y = 0.0f;
-	dAngIMU.z = 0.0f;
+	_ekf->dVelIMU.x = 0.0f;
+	_ekf->dVelIMU.y = 0.0f;
+	_ekf->dVelIMU.z = 0.0f;
+	_ekf->dAngIMU.x = 0.0f;
+	_ekf->dAngIMU.y = 0.0f;
+	_ekf->dAngIMU.z = 0.0f;
 
 	/* wakeup source(s) */
 	struct pollfd fds[2];
@@ -509,7 +527,7 @@ FixedwingEstimator::task_main()
 			}
 
 			last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3f;
+			_ekf.IMUmsec = _gyro.timestamp / 1e3f;
 
 			float deltaT = (_gyro.timestamp - last_run) / 1e6f;
 			last_run = _gyro.timestamp;
@@ -521,20 +539,20 @@ FixedwingEstimator::task_main()
 
 			// Always store data, independent of init status
 			/* fill in last data set */
-			dtIMU = deltaT;
+			_ekf->dtIMU = deltaT;
 
-			angRate.x = _gyro.x;
-			angRate.y = _gyro.y;
-			angRate.z = _gyro.z;
+			_ekf->angRate.x = _gyro.x;
+			_ekf->angRate.y = _gyro.y;
+			_ekf->angRate.z = _gyro.z;
 
-			accel.x = _accel.x;
-			accel.y = _accel.y;
-			accel.z = _accel.z;
+			_ekf->accel.x = _accel.x;
+			_ekf->accel.y = _accel.y;
+			_ekf->accel.z = _accel.z;
 
-			dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
-			lastAngRate = angRate;
-			dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
-			lastAccel = accel;
+			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+			_ekf->lastAngRate = angRate;
+			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+			_ekf->lastAccel = accel;
 
 
 #else
@@ -563,20 +581,20 @@ FixedwingEstimator::task_main()
 
 			// Always store data, independent of init status
 			/* fill in last data set */
-			dtIMU = deltaT;
+			_ekf->dtIMU = deltaT;
 
-			angRate.x = _sensor_combined.gyro_rad_s[0];
-			angRate.y = _sensor_combined.gyro_rad_s[1];
-			angRate.z = _sensor_combined.gyro_rad_s[2];
+			_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];
 
-			accel.x = _sensor_combined.accelerometer_m_s2[0];
-			accel.y = _sensor_combined.accelerometer_m_s2[1];
-			accel.z = _sensor_combined.accelerometer_m_s2[2];
+			_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];
 
-			dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
-			lastAngRate = angRate;
-			dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
-			lastAccel = accel;
+			_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;
@@ -597,7 +615,7 @@ FixedwingEstimator::task_main()
 				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 				perf_count(_perf_airspeed);
 
-				VtasMeas = _airspeed.true_airspeed_m_s;
+				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
 				newAdsData = true;
 
 			} else {
@@ -622,24 +640,24 @@ FixedwingEstimator::task_main()
 
 					/* check if we had a GPS outage for a long time */
 					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
-						ResetPosition();
-						ResetVelocity();
-						ResetStoredStates();
+						_ekf->ResetPosition();
+						_ekf->ResetVelocity();
+						_ekf->ResetStoredStates();
 					}
 
 					/* fuse GPS updates */
 
 					//_gps.timestamp / 1e3;
-					GPSstatus = _gps.fix_type;
-					velNED[0] = _gps.vel_n_m_s;
-					velNED[1] = _gps.vel_e_m_s;
-					velNED[2] = _gps.vel_d_m_s;
+					_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]);
 
-					gpsLat = math::radians(_gps.lat / (double)1e7);
-					gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
-					gpsHgt = _gps.alt / 1e3f;
+					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
+					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+					_ekf->gpsHgt = _gps.alt / 1e3f;
 					newDataGps = true;
 
 				}
@@ -652,10 +670,10 @@ FixedwingEstimator::task_main()
 			if (baro_updated) {
 				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
-				baroHgt = _baro.altitude - _baro_ref;
+				_ekf->baroHgt = _baro.altitude - _baro_ref;
 
 				// Could use a blend of GPS and baro alt data if desired
-				hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
+				_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
 			}
 
 #ifndef SENSOR_COMBINED_SUB
@@ -671,27 +689,27 @@ FixedwingEstimator::task_main()
 
 				// XXX we compensate the offsets upfront - should be close to zero.
 				// 0.001f
-				magData.x = _mag.x;
-				magBias.x = 0.000001f; // _mag_offsets.x_offset
+				_ekf->magData.x = _mag.x;
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-				magData.y = _mag.y;
-				magBias.y = 0.000001f; // _mag_offsets.y_offset
+				_ekf->magData.y = _mag.y;
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
 
-				magData.z = _mag.z;
-				magBias.z = 0.000001f; // _mag_offsets.y_offset
+				_ekf->magData.z = _mag.z;
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 
 #else
 
 				// XXX we compensate the offsets upfront - should be close to zero.
 				// 0.001f
-				magData.x = _sensor_combined.magnetometer_ga[0];
-				magBias.x = 0.000001f; // _mag_offsets.x_offset
+				_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-				magData.y = _sensor_combined.magnetometer_ga[1];
-				magBias.y = 0.000001f; // _mag_offsets.y_offset
+				_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
 
-				magData.z = _sensor_combined.magnetometer_ga[2];
-				magBias.z = 0.000001f; // _mag_offsets.y_offset
+				_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 
 #endif
 
@@ -705,7 +723,7 @@ FixedwingEstimator::task_main()
 			/**
 			 *    CHECK IF THE INPUT DATA IS SANE
 			 */
-			int check = CheckAndBound();
+			int check = _ekf->CheckAndBound();
 
 			switch (check) {
 				case 0:
@@ -739,7 +757,7 @@ FixedwingEstimator::task_main()
 
 				struct ekf_status_report ekf_report;
 
-				GetLastErrorState(&ekf_report);
+				_ekf->GetLastErrorState(&ekf_report);
 
 				struct estimator_status_report rep;
 				memset(&rep, 0, sizeof(rep));
@@ -779,16 +797,16 @@ FixedwingEstimator::task_main()
 
 			if (hrt_elapsed_time(&start_time) > 100000) {
 
-				if (!_gps_initialized && (GPSstatus == 3)) {
-					velNED[0] = _gps.vel_n_m_s;
-					velNED[1] = _gps.vel_e_m_s;
-					velNED[2] = _gps.vel_d_m_s;
+				if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
+					_ekf->velNED[0] = _gps.vel_n_m_s;
+					_ekf->velNED[1] = _gps.vel_e_m_s;
+					_ekf->velNED[2] = _gps.vel_d_m_s;
 
 					double lat = _gps.lat * 1e-7;
 					double lon = _gps.lon * 1e-7;
 					float alt = _gps.alt * 1e-3;
 
-					InitialiseFilter(velNED);
+					_ekf->InitialiseFilter(_ekf->velNED);
 
 					// Initialize projection
 					_local_pos.ref_lat = _gps.lat;
@@ -799,7 +817,7 @@ FixedwingEstimator::task_main()
 					// Store 
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 					_baro_ref = _baro.altitude;
-					baroHgt = _baro.altitude - _baro_ref;
+					_ekf->baroHgt = _baro.altitude - _baro_ref;
 					_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
 
 					// XXX this is not multithreading safe
@@ -808,24 +826,24 @@ FixedwingEstimator::task_main()
 
 					_gps_initialized = true;
 
-				} else if (!statesInitialised) {
-					velNED[0] = 0.0f;
-					velNED[1] = 0.0f;
-					velNED[2] = 0.0f;
-					posNED[0] = 0.0f;
-					posNED[1] = 0.0f;
-					posNED[2] = 0.0f;
-
-					posNE[0] = posNED[0];
-					posNE[1] = posNED[1];
-					InitialiseFilter(velNED);
+				} else if (!_ekf->statesInitialised) {
+					_ekf->velNED[0] = 0.0f;
+					_ekf->velNED[1] = 0.0f;
+					_ekf->velNED[2] = 0.0f;
+					_ekf->posNED[0] = 0.0f;
+					_ekf->posNED[1] = 0.0f;
+					_ekf->posNED[2] = 0.0f;
+
+					_ekf->posNE[0] = _ekf->posNED[0];
+					_ekf->posNE[1] = _ekf->posNED[1];
+					_ekf->InitialiseFilter(_ekf->velNED);
 				}
 			}
 
 			// If valid IMU data and states initialised, predict states and covariances
-			if (statesInitialised) {
+			if (_ekf->statesInitialised) {
 				// Run the strapdown INS equations every IMU update
-				UpdateStrapdownEquationsNED();
+				_ekf->UpdateStrapdownEquationsNED();
 #if 0
 				// debug code - could be tunred into a filter mnitoring/watchdog function
 				float tempQuat[4];
@@ -842,20 +860,20 @@ FixedwingEstimator::task_main()
 
 #endif
 				// store the predicted states for subsequent use by measurement fusion
-				StoreStates(IMUmsec);
+				_ekf->StoreStates(IMUmsec);
 				// Check if on ground - status is used by covariance prediction
-				OnGroundCheck();
+				_ekf->OnGroundCheck();
 				// sum delta angles and time used by covariance prediction
-				summedDelAng = summedDelAng + correctedDelAng;
-				summedDelVel = summedDelVel + dVelIMU;
-				dt += dtIMU;
+				_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 >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
-					CovariancePrediction(dt);
-					summedDelAng = summedDelAng.zero();
-					summedDelVel = summedDelVel.zero();
+				if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
+					_ekf->CovariancePrediction(dt);
+					_ekf->summedDelAng = _ekf->summedDelAng.zero();
+					_ekf->summedDelVel = _ekf->summedDelVel.zero();
 					dt = 0.0f;
 				}
 
@@ -865,79 +883,79 @@ FixedwingEstimator::task_main()
 			// Fuse GPS Measurements
 			if (newDataGps && _gps_initialized) {
 				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				velNED[0] = _gps.vel_n_m_s;
-				velNED[1] = _gps.vel_e_m_s;
-				velNED[2] = _gps.vel_d_m_s;
-				calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
+				_ekf->velNED[0] = _gps.vel_n_m_s;
+				_ekf->velNED[1] = _gps.vel_e_m_s;
+				_ekf->velNED[2] = _gps.vel_d_m_s;
+				_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
 
-				posNE[0] = posNED[0];
-				posNE[1] = posNED[1];
+				_ekf->posNE[0] = _ekf->posNED[0];
+				_ekf->posNE[1] = _ekf->posNED[1];
 				// set fusion flags
-				fuseVelData = true;
-				fusePosData = true;
+				_ekf->fuseVelData = true;
+				_ekf->fusePosData = true;
 				// recall states stored at time of measurement after adjusting for delays
-				RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
 				// run the fusion step
-				FuseVelposNED();
+				_ekf->FuseVelposNED();
 
-			} else if (statesInitialised) {
+			} else if (_ekf->statesInitialised) {
 				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				velNED[0] = 0.0f;
-				velNED[1] = 0.0f;
-				velNED[2] = 0.0f;
-				posNED[0] = 0.0f;
-				posNED[1] = 0.0f;
-				posNED[2] = 0.0f;
-
-				posNE[0] = posNED[0];
-				posNE[1] = posNED[1];
+				_ekf->velNED[0] = 0.0f;
+				_ekf->velNED[1] = 0.0f;
+				_ekf->velNED[2] = 0.0f;
+				_ekf->posNED[0] = 0.0f;
+				_ekf->posNED[1] = 0.0f;
+				_ekf->posNED[2] = 0.0f;
+
+				_ekf->posNE[0] = _ekf->posNED[0];
+				_ekf->posNE[1] = _ekf->posNED[1];
 				// set fusion flags
-				fuseVelData = true;
-				fusePosData = true;
+				_ekf->fuseVelData = true;
+				_ekf->fusePosData = true;
 				// recall states stored at time of measurement after adjusting for delays
-				RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
 				// run the fusion step
-				FuseVelposNED();
+				_ekf->FuseVelposNED();
 
 			} else {
-				fuseVelData = false;
-				fusePosData = false;
+				_ekf->fuseVelData = false;
+				_ekf->fusePosData = false;
 			}
 
-			if (newAdsData && statesInitialised) {
+			if (newAdsData && _ekf->statesInitialised) {
 				// Could use a blend of GPS and baro alt data if desired
-				hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
-				fuseHgtData = true;
+				_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+				_ekf->fuseHgtData = true;
 				// recall states stored at time of measurement after adjusting for delays
-				RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
 				// run the fusion step
-				FuseVelposNED();
+				_ekf->FuseVelposNED();
 
 			} else {
-				fuseHgtData = false;
+				_ekf->fuseHgtData = false;
 			}
 
 			// Fuse Magnetometer Measurements
-			if (newDataMag && statesInitialised) {
-				fuseMagData = true;
-				RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+			if (newDataMag && _ekf->statesInitialised) {
+				_ekf->fuseMagData = true;
+				_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
 			} else {
-				fuseMagData = false;
+				_ekf->fuseMagData = false;
 			}
 
-			if (statesInitialised) FuseMagnetometer();
+			if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
 
 			// Fuse Airspeed Measurements
-			if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
-				fuseVtasData = true;
-				RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-				FuseAirspeed();
+			if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+				_ekf->fuseVtasData = true;
+				_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+				_ekf->FuseAirspeed();
 
 			} else {
-				fuseVtasData = false;
+				_ekf->fuseVtasData = false;
 			}
 
 			// Publish results
@@ -954,7 +972,7 @@ FixedwingEstimator::task_main()
 				// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
 				// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
 
-				math::Quaternion q(states[0], states[1], states[2], states[3]);
+				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();
 
@@ -962,10 +980,10 @@ FixedwingEstimator::task_main()
 						_att.R[i][j] = R(i, j);
 
 				_att.timestamp = last_sensor_timestamp;
-				_att.q[0] = states[0];
-				_att.q[1] = states[1];
-				_att.q[2] = states[2];
-				_att.q[3] = states[3];
+				_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;
 
@@ -974,13 +992,13 @@ FixedwingEstimator::task_main()
 				_att.pitch = euler(1);
 				_att.yaw = euler(2);
 
-				_att.rollspeed = angRate.x - states[10];
-				_att.pitchspeed = angRate.y - states[11];
-				_att.yawspeed = angRate.z - states[12];
+				_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] = states[10];
-				_att.rate_offsets[1] = states[11];
-				_att.rate_offsets[2] = states[12];
+				_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) {
@@ -993,20 +1011,15 @@ FixedwingEstimator::task_main()
 				}
 			}
 
-			if (!isfinite(states[0])) {
-				print_status();
-				_exit(0);
-			}
-
 			if (_gps_initialized) {
 				_local_pos.timestamp = last_sensor_timestamp;
-				_local_pos.x = states[7];
-				_local_pos.y = states[8];
-				_local_pos.z = states[9];
+				_local_pos.x = _ekf->states[7];
+				_local_pos.y = _ekf->states[8];
+				_local_pos.z = _ekf->states[9];
 
-				_local_pos.vx = states[4];
-				_local_pos.vy = states[5];
-				_local_pos.vz = states[6];
+				_local_pos.vx = _ekf->states[4];
+				_local_pos.vy = _ekf->states[5];
+				_local_pos.vz = _ekf->states[6];
 
 				_local_pos.xy_valid = _gps_initialized;
 				_local_pos.z_valid = true;
@@ -1107,9 +1120,10 @@ FixedwingEstimator::start()
 	return OK;
 }
 
-void print_status()
+void
+FixedwingEstimator::print_status()
 {
-	math::Quaternion q(states[0], states[1], states[2], states[3]);
+	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();
 
@@ -1125,30 +1139,30 @@ void print_status()
 	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-	printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec);
-	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z);
-	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
-	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
-	printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
-	printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
-	printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]);
-	printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
-	printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
-	printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
+	printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
+	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
+	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
+	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
+	printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
+	printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+	printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
+	printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
+	printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
-	       (statesInitialised) ? "INITIALIZED" : "NON_INIT",
-	       (onGround) ? "ON_GROUND" : "AIRBORNE",
-	       (fuseVelData) ? "FUSE_VEL" : "INH_VEL",
-	       (fusePosData) ? "FUSE_POS" : "INH_POS",
-	       (fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
-	       (fuseMagData) ? "FUSE_MAG" : "INH_MAG",
-	       (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
-	       (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
-	       (useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
-	       (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+	       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
-int trip_nan() {
+int FixedwingEstimator::trip_nan() {
 
 	int ret = 0;
 
@@ -1166,7 +1180,7 @@ int trip_nan() {
 		float nan_val = 0.0f / 0.0f;
 
 		warnx("system not armed, tripping state vector with NaN values");
-		states[5] = nan_val;
+		_ekf->states[5] = nan_val;
 		usleep(100000);
 
 		// warnx("tripping covariance #1 with NaN values");
@@ -1178,15 +1192,15 @@ int trip_nan() {
 		// usleep(100000);
 
 		warnx("tripping covariance #3 with NaN values");
-		P[3][3] = nan_val; // covariance matrix
+		_ekf->P[3][3] = nan_val; // covariance matrix
 		usleep(100000);
 
 		warnx("tripping Kalman gains with NaN values");
-		Kfusion[0] = nan_val; // Kalman gains
+		_ekf->Kfusion[0] = nan_val; // Kalman gains
 		usleep(100000);
 
 		warnx("tripping stored states[0] with NaN values");
-		storedStates[0][0] = nan_val;
+		_ekf->storedStates[0][0] = nan_val;
 		usleep(100000);
 
 		warnx("\nDONE - FILTER STATE:");
@@ -1234,7 +1248,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
 		if (estimator::g_estimator) {
 			warnx("running");
 
-			print_status();
+			estimator::g_estimator->print_status();
 
 			exit(0);
 
@@ -1245,7 +1259,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "trip")) {
 		if (estimator::g_estimator) {
-			int ret = trip_nan();
+			int ret = estimator::g_estimator->trip_nan();
 
 			exit(ret);
 
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 4de722832..d432edd2b 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 		if (_updated) {
 			orb_copy(_topic, _fd, _data);
+			_published = true;
 			return true;
 		}
 	}
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c45cafc1b..5a94b6671 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
-			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..56c5aa005 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -134,8 +134,6 @@ controls_tick() {
 
 	perf_begin(c_gather_sbus);
 
-	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
-
 	bool sbus_failsafe, sbus_frame_drop;
 	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
 
@@ -201,94 +199,104 @@ controls_tick() {
 		/* update RC-received timestamp */
 		system_state.rc_channels_timestamp_received = hrt_absolute_time();
 
-		/* do not command anything in failsafe, kick in the RC loss counter */
-		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
-
-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
-
-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
-					uint16_t raw = r_raw_rc_values[i];
-
-					int16_t scaled;
-
-					/*
-					 * 1) Constrain to min/max values, as later processing depends on bounds.
-					 */
-					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-						raw = conf[PX4IO_P_RC_CONFIG_MIN];
-					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-						raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
-					/*
-					 * 2) Scale around the mid point differently for lower and upper range.
-					 *
-					 * This is necessary as they don't share the same endpoints and slope.
-					 *
-					 * First normalize to 0..1 range with correct sign (below or above center),
-					 * then scale to 20000 range (if center is an actual center, -10000..10000,
-					 * if parameters only support half range, scale to 10000 range, e.g. if
-					 * center == min 0..10000, if center == max -10000..0).
-					 *
-					 * As the min and max bounds were enforced in step 1), division by zero
-					 * cannot occur, as for the case of center == min or center == max the if
-					 * statement is mutually exclusive with the arithmetic NaN case.
-					 *
-					 * DO NOT REMOVE OR ALTER STEP 1!
-					 */
-					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
-					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
-					} else {
-						/* in the configured dead zone, output zero */
-						scaled = 0;
-					}
-
-					/* invert channel if requested */
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-						scaled = -scaled;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+				uint16_t raw = r_raw_rc_values[i];
+
+				int16_t scaled;
+
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
 
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					if (mapped < PX4IO_CONTROL_CHANNELS) {
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+					scaled = -scaled;
+				}
 
-						/* invert channel if pitch - pulling the lever down means pitching up by convention */
-						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-							scaled = -scaled;
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
+					}
 
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+						} else {
+							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+						}
 					}
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
 				}
 			}
+		}
 
-			/* set un-assigned controls to zero */
-			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-				if (!(assigned_channels & (1 << i)))
-					r_rc_values[i] = 0;
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i))) {
+				r_rc_values[i] = 0;
 			}
+		}
 
-			/* set RC OK flag, as we got an update */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		/* set RC OK flag, as we got an update */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
 
-			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
-			if (assigned_channels > 4) {
-				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
-			} else {
-				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
-			}
+		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+		if (assigned_channels > 4) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
 
 		/*
@@ -316,8 +324,13 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
-	/* this kicks in if the receiver is gone or the system went to failsafe */
-	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	/* if we are in failsafe, clear the override flag */
+	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+	}
+
+	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;
 
+		/* Set raw channel count to zero */
+		r_raw_rc_count = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}
 
-	/* this kicks in if the receiver is completely gone */
-	if (rc_input_lost) {
-
-		/* Set channel count to zero */
-		r_raw_rc_count = 0;
-	}
-
 	/*
 	 * Check for manual override.
 	 *
 	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input.
+	 * must have R/C input (NO FAILSAFE!).
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -369,10 +379,10 @@ controls_tick() {
 				mixer_tick();
 
 		} else {
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
 	} else {
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..a978d483a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -164,10 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
-#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
-#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum {							/* DSM bind states */
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
- 					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+						/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
-#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
-#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
 
-#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+						/* 12 occupied by CRC */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum {							/* DSM bind states */
 #define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 
 #define PX4IO_P_CONTROLS_GROUP_VALID		64
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD			52
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 54c5663a5..4db948484 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -108,6 +108,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 #endif
+#define r_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..6752e7b4b 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -169,6 +169,7 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3f07eea53..e62b0fafc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -84,6 +84,8 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
 		struct estimator_status_report estimator_status;
+		struct system_power_s system_power;
+		struct servorail_status_s servorail_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
+			struct log_PWR_s log_PWR;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int telemetry_sub;
 		int range_finder_sub;
 		int estimator_status_sub;
+		int system_power_sub;
+		int servorail_status_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 
 	thread_running = true;
 
@@ -1184,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
+			log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
 			LOGBUFFER_WRITE_AND_COUNT(AIRS);
 		}
 
@@ -1226,6 +1236,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(BATT);
 		}
 
+		/* --- SYSTEM POWER RAILS --- */
+		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+			log_msg.msg_type = LOG_PWR_MSG;
+			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+			log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+			log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+			log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+			log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+			/* copy servo rail status topic here too */
+			orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
+			log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
+			log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
+
+			LOGBUFFER_WRITE_AND_COUNT(PWR);
+		}
+
 		/* --- TELEMETRY --- */
 		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
 			log_msg.msg_type = LOG_TELE_MSG;
@@ -1258,7 +1286,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
 			log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
 			log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
-			LOGBUFFER_WRITE_AND_COUNT(DIST);
+			LOGBUFFER_WRITE_AND_COUNT(ESTM);
 		}
 
 		/* signal the other thread new data, but not yet unlock */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 2b41755de..a1a5856bc 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -174,6 +174,7 @@ struct log_OUT0_s {
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
+	float air_temperature_celsius;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -277,6 +278,29 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };
 
+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 23
+struct log_ESTM_s {
+	float s[10];
+	uint8_t n_states;
+	uint8_t states_nan;
+	uint8_t covariance_nan;
+	uint8_t kalman_gain_nan;
+};
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+	float peripherals_5v;
+	float servo_rail_5v;
+	float servo_rssi;
+	uint8_t usb_ok;
+	uint8_t brick_ok;
+	uint8_t servo_ok;
+	uint8_t low_power_rail_overcurrent;
+	uint8_t high_power_rail_overcurrent;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -299,16 +323,6 @@ struct log_PARM_s {
 	float value;
 };
 
-/* --- ESTM - ESTIMATOR STATUS --- */
-#define LOG_ESTM_MSG 132
-struct log_ESTM_s {
-	float s[32];
-	uint8_t n_states;
-	uint8_t states_nan;
-	uint8_t covariance_nan;
-	uint8_t kalman_gain_nan;
-};
-
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -325,7 +339,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
+	LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
@@ -335,15 +349,16 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
 
 	/* system-level messages, ID >= 0x80 */
-	// FMT: don't write format of format message, it's useless
+	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
-	LOG_FORMAT(PARM, "Nf", "Name,Value"),
-	LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value")
 };
 
-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
 
 #endif /* SDLOG2_MESSAGES_H_ */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 288c6e00a..14f7ac812 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
  */
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
 
-
-/**
- * Failsafe channel mapping.
- *
- * @min 0
- * @max 18
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_CH, 0);
-
-/**
- * Failsafe channel mode.
- *
- * 0 = too low means signal loss,
- * 1 = too high means signal loss
- *
- * @min 0
- * @max 1
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);
-
 /**
  * Failsafe channel PWM threshold.
  *
- * @min 0
- * @max 1
+ * @min 800
+ * @max 2200
  * @group Radio Calibration
  */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f890c4c7f..4083b8b4f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,9 +263,7 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
-		int rc_fs_ch;
-		int rc_fs_mode;
-		float rc_fs_thr;
+		int32_t rc_fs_thr;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -313,8 +311,6 @@ private:
 		param_t rc_scale_yaw;
 		param_t rc_scale_flaps;
 
-		param_t rc_fs_ch;
-		param_t rc_fs_mode;
 		param_t rc_fs_thr;
 
 		param_t battery_voltage_scaling;
@@ -531,9 +527,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
 	/* RC failsafe */
-	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
-	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
-	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -689,8 +683,6 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
 	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
-	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
 	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
@@ -1033,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
-		float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1312,19 +1305,15 @@ Sensors::rc_poll()
 		manual_control.aux5 = NAN;
 
 		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
+		if (rc_input.channel_count < 4) {
 			return;
+		}
 
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
-
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
-			}
+		/* check for failsafe */
+		if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
+			/* do not publish manual control setpoints when there are none */
+			return;
 		}
 
 		unsigned channel_limit = rc_input.channel_count;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4b31cc8a4..c8a589bb7 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
 #include "topics/servorail_status.h"
 ORB_DEFINE(servorail_status, struct servorail_status_s);
 
+#include "topics/system_power.h"
+ORB_DEFINE(system_power, struct system_power_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index a3da3758f..d2ee754cd 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -52,9 +52,10 @@
  * Airspeed
  */
 struct airspeed_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		air_temperature_celsius;	/**< air temperature in degrees celsius, -1000 if unknown */
 };
 
 /**
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index ff88b04c6..01e14cda9 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,13 +52,14 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
-	uint64_t	error_count;
+	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
+	uint64_t	error_count;			/**< Number of errors detected by driver */
 	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
-	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
-	float		temperature;			/**< Temperature provided by sensor, -1000.0f if unknown */
+	float	voltage;				/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float	temperature;				/**< Temperature provided by sensor, -1000.0f if unknown */
 
 };
 
diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h
new file mode 100644
index 000000000..7763b6004
--- /dev/null
+++ b/src/modules/uORB/topics/system_power.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_power.h
+ *
+ * Definition of the system_power voltage and power status uORB topic.
+ */
+
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage5V_v;		/**< peripheral 5V rail voltage */
+	uint8_t         usb_connected:1;        /**< USB is connected when 1 */
+	uint8_t         brick_valid:1;          /**< brick power is good when 1 */
+	uint8_t         servo_valid:1;          /**< servo power is good when 1 */
+	uint8_t		periph_5V_OC:1;         /**< peripheral overcurrent when 1 */
+	uint8_t		hipower_5V_OC:1;        /**< hi power peripheral overcurrent when 1 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
+
+#endif