Merge remote-tracking branch 'upstream/master' into ROS_shared_lib_base_class

129 files changed, 5265 insertions, 3389 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 293690d27..6db6713c4 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -147,7 +147,7 @@ Airspeed::init()
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
 		if (_airspeed_pub < 0)
-			warnx("failed to create airspeed sensor object. uORB started?");
+			warnx("uORB started?");
 	}
 
 	ret = OK;
@@ -159,13 +159,15 @@ out:
 int
 Airspeed::probe()
 {
-	/* on initial power up the device needs more than one retry
-	   for detection. Once it is running then retries aren't
-	   needed 
+	/* on initial power up the device may need more than one retry
+	   for detection. Once it is running the number of retries can
+	   be reduced
 	*/
 	_retries = 4;
 	int ret = measure();
-	_retries = 0;
+
+        // drop back to 2 retries once initialised
+	_retries = 2;
 	return ret;
 }
 
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index e5bb772b3..9d2c1441d 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -89,8 +89,8 @@ static void
 usage(const char *reason)
 {
 	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+		warnx("%s\n", reason);
+	warnx("usage: {start|stop|status} [-d <UART>]\n\n");
 	exit(1);
 }
 
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("ardrone_interface already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tardrone_interface is running\n");
+			warnx("running");
 		} else {
-			printf("\tardrone_interface not started\n");
+			warnx("not started");
 		}
 		exit(0);
 	}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: tcsetattr: %s", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	char *device = "/dev/ttyS1";
 
-	/* welcome user */
-	printf("[ardrone_interface] Control started, taking over motors\n");
-
 	/* File descriptors */
 	int gpios;
 
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	struct termios uart_config_original;
 
 	if (motor_test_mode) {
-		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+		warnx("setting 10 %% thrust.\n");
 	}
 
 	/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	printf("[ardrone_interface] Motors initialized - ready.\n");
-	fflush(stdout);
-
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("No UART, exiting.");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("write fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int termios_state;
 
 	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+		warnx("ERR: tcsetattr");
 	}
 
-	printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
 	/* close uarts */
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index fc017dd58..4fa24275f 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
 
 	if (errcounter != 0) {
-		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+		warnx("Failed %d times", -errcounter);
 		fflush(stdout);
 	}
 	return errcounter;
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
index b53fe0a29..0a2d91009 100644
--- a/src/drivers/boards/aerocore/module.mk
+++ b/src/drivers/boards/aerocore/module.mk
@@ -6,3 +6,5 @@ SRCS		 = aerocore_init.c \
 		   aerocore_pwm_servo.c \
 		   aerocore_spi.c \
 		   aerocore_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..5e1a27d5a 100644
--- a/src/drivers/boards/px4fmu-v1/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
index 99d37eeca..103232b0c 100644
--- a/src/drivers/boards/px4fmu-v2/module.mk
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu2_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..a7a14dd07 100644
--- a/src/drivers/boards/px4io-v1/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
@@ -4,3 +4,5 @@
 
 SRCS	= px4io_init.c \
 	  px4io_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
index 85f94e8be..3f0e9a0b3 100644
--- a/src/drivers/boards/px4io-v2/module.mk
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -4,3 +4,5 @@
 
 SRCS		= px4iov2_init.c \
 		  px4iov2_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 9d684e394..67aaa0aff 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -130,7 +130,8 @@ public:
 	enum DeviceBusType {
 		DeviceBusType_UNKNOWN = 0,
 		DeviceBusType_I2C     = 1,
-		DeviceBusType_SPI     = 2
+		DeviceBusType_SPI     = 2,
+		DeviceBusType_UAVCAN  = 3,
 	};
 
 	/*
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 6873f24b6..b10c3e18a 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -117,6 +117,23 @@ struct pwm_output_values {
 	unsigned			channel_count;
 };
 
+
+/**
+ * RC config values for a channel
+ *
+ * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
+ * param_get() dependency
+ */
+struct pwm_output_rc_config {
+	uint8_t channel;
+	uint16_t rc_min;
+	uint16_t rc_trim;
+	uint16_t rc_max;
+	uint16_t rc_dz;
+	uint16_t rc_assignment;
+	bool     rc_reverse;
+};
+
 /*
  * ORB tag for PWM outputs.
  */
@@ -214,7 +231,19 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+#define PWM_SERVO_SET_FORCE_SAFETY_ON		_IOC(_PWM_SERVO_BASE, 26)
+
+/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
+#define PWM_SERVO_SET_RC_CONFIG			_IOC(_PWM_SERVO_BASE, 27)
+
+/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_SET_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 28)
+
+/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_CLEAR_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 29)
+
+/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
+#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_IOC(_PWM_SERVO_BASE, 30)
 
 /*
  *
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 6e0839043..bccdf1190 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	static const speed_t speed = B9600;
 
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: %s (tcsetattr)\n", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	/* Print welcome text */
-	warnx("FrSky telemetry interface starting...");
-
 	/* Open UART */
 	struct termios uart_config_original;
 	const int uart = frsky_open_uart(device_name, &uart_config_original);
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5fb4b9ff8..47c907bd3 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
 void
 GPS::task_main()
 {
-	log("starting");
 
 	/* open the serial port */
 	_serial_fd = ::open(_port, O_RDWR);
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index f0dc0c651..9b5c8133b 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -442,8 +442,6 @@ HIL::task_main()
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
 
-	log("stopping");
-
 	/* note - someone else is responsible for restoring the GPIO config */
 
 	/* tell the dtor that we are exiting */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 81f767965..d4dbf3778 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1349,7 +1349,7 @@ HMC5883	*g_dev_ext = nullptr;
 void	start(int bus, enum Rotation rotation);
 void	test(int bus);
 void	reset(int bus);
-void	info(int bus);
+int	info(int bus);
 int	calibrate(int bus);
 void	usage();
 
@@ -1595,17 +1595,23 @@ reset(int bus)
 /**
  * Print a little info about the driver.
  */
-void
+int
 info(int bus)
 {
-	HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
-	if (g_dev == nullptr)
-		errx(1, "driver not running");
+	int ret = 1;
+
+	HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
+	if (g_dev == nullptr) {
+		warnx("not running on bus %d", bus);
+	} else {
 
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
+		warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
 
-	exit(0);
+		g_dev->print_info();
+		ret = 0;
+	}
+
+	return ret;
 }
 
 void
@@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
-		hmc5883::info(bus);
+	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+		if (bus == -1) {
+			int ret = 0;
+			if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
+				ret = 1;
+			}
+
+			if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
+				ret = 1;
+			}
+			exit(ret);
+		} else {
+			exit(hmc5883::info(bus));
+		}
+	}
 
 	/*
 	 * Autocalibrate the scaling
diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
index 5daa01dc5..be2ee7276 100644
--- a/src/drivers/hmc5883/module.mk
+++ b/src/drivers/hmc5883/module.mk
@@ -42,3 +42,5 @@ SRCS		= hmc5883.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
index cb8bbba37..60a49b559 100644
--- a/src/drivers/hott/comms.cpp
+++ b/src/drivers/hott/comms.cpp
@@ -55,7 +55,7 @@ open_uart(const char *device)
 	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		err(1, "Error opening port: %s", device);
+		err(1, "ERR: opening %s", device);
 	}
 	
 	/* Back up the original uart configuration to restore it after exit */	
@@ -63,7 +63,7 @@ open_uart(const char *device)
 	struct termios uart_config_original;
 	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
 		close(uart);
-		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+		err(1, "ERR: %s: %d", device, termios_state);
 	}
 
 	/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+		err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
 			 device, termios_state);
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+		err(1, "ERR: %s (tcsetattr)", device);
 	}
 
 	/* Activate single wire mode */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index a3d3a3933..8ab9d8d55 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index d293f9954..edbb14172 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
index 5630e7aec..3d64d62be 100644
--- a/src/drivers/l3gd20/module.mk
+++ b/src/drivers/l3gd20/module.mk
@@ -8,3 +8,5 @@ SRCS		 = l3gd20.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index b4f3974f4..0421eb113 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -8,3 +8,5 @@ SRCS		 = lsm303d.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1d9a463ad..ba46de379 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -519,7 +519,7 @@ test()
 		ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			errx(1, "timed out");
 		}
 
 		/* now go get it */
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
index 5b4893b12..da9fcc0fc 100644
--- a/src/drivers/mpu6000/module.mk
+++ b/src/drivers/mpu6000/module.mk
@@ -42,3 +42,5 @@ SRCS		= mpu6000.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 6c68636c6..09ec4bf96 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.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
@@ -73,15 +73,13 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
- 
-/* PX4FLOW Registers addresses */
-#define PX4FLOW_REG	0x00		/* Measure Register */
 
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
+/* PX4FLOW Registers addresses */
+#define PX4FLOW_REG		0x16	/* Measure Register 22*/
 
+#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -92,28 +90,42 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-//struct i2c_frame
-//{
-//    uint16_t frame_count;
-//    int16_t pixel_flow_x_sum;
-//    int16_t pixel_flow_y_sum;
-//    int16_t flow_comp_m_x;
-//    int16_t flow_comp_m_y;
-//    int16_t qual;
-//    int16_t gyro_x_rate;
-//    int16_t gyro_y_rate;
-//    int16_t gyro_z_rate;
-//    uint8_t gyro_range;
-//    uint8_t sonar_timestamp;
-//    int16_t ground_distance;
-//};
-//
-//struct i2c_frame f;
-
-class PX4FLOW : public device::I2C
+struct i2c_frame {
+	uint16_t frame_count;
+	int16_t pixel_flow_x_sum;
+	int16_t pixel_flow_y_sum;
+	int16_t flow_comp_m_x;
+	int16_t flow_comp_m_y;
+	int16_t qual;
+	int16_t gyro_x_rate;
+	int16_t gyro_y_rate;
+	int16_t gyro_z_rate;
+	uint8_t gyro_range;
+	uint8_t sonar_timestamp;
+	int16_t ground_distance;
+};
+struct i2c_frame f;
+
+typedef struct i2c_integral_frame {
+	uint16_t frame_count_since_last_readout;
+	int16_t pixel_flow_x_integral;
+	int16_t pixel_flow_y_integral;
+	int16_t gyro_x_rate_integral;
+	int16_t gyro_y_rate_integral;
+	int16_t gyro_z_rate_integral;
+	uint32_t integration_timespan;
+	uint32_t time_since_last_sonar_update;
+	uint16_t ground_distance;
+	int16_t gyro_temperature;
+	uint8_t qual;
+} __attribute__((packed));
+struct i2c_integral_frame f_integral;
+
+
+class PX4FLOW: public device::I2C
 {
 public:
-	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
+	PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
 	virtual ~PX4FLOW();
 
 	virtual int 		init();
@@ -122,8 +134,8 @@ public:
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 	/**
-	* Diagnostics - print some basic information about the driver.
-	*/
+	 * Diagnostics - print some basic information about the driver.
+	 */
 	void				print_info();
 
 protected:
@@ -144,42 +156,41 @@ private:
 	perf_counter_t		_buffer_overflows;
 
 	/**
-	* Test whether the device supported by the driver is present at a
-	* specific address.
-	*
-	* @param address	The I2C bus address to probe.
-	* @return		True if the device is present.
-	*/
+	 * Test whether the device supported by the driver is present at a
+	 * specific address.
+	 *
+	 * @param address	The I2C bus address to probe.
+	 * @return		True if the device is present.
+	 */
 	int					probe_address(uint8_t address);
 
 	/**
-	* Initialise the automatic measurement state machine and start it.
-	*
-	* @note This function is called at open and error time.  It might make sense
-	*       to make it more aggressive about resetting the bus in case of errors.
-	*/
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
 	void				start();
 
 	/**
-	* Stop the automatic measurement state machine.
-	*/
+	 * Stop the automatic measurement state machine.
+	 */
 	void				stop();
 
 	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 */
 	void				cycle();
 	int					measure();
 	int					collect();
 	/**
-	* Static trampoline from the workq context; because we don't have a
-	* generic workq wrapper yet.
-	*
-	* @param arg		Instance pointer for the driver that is polling.
-	*/
-	static void		cycle_trampoline(void *arg);
-
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void			cycle_trampoline(void *arg);
 
 };
 
@@ -189,7 +200,7 @@ private:
 extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
 
 PX4FLOW::PX4FLOW(int bus, int address) :
-	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -200,7 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -228,21 +239,12 @@ PX4FLOW::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
+	_reports = new RingBuffer(2, sizeof(optical_flow_s));
 
 	if (_reports == nullptr) {
 		goto out;
 	}
 
-	/* get a publish handle on the px4flow topic */
-	struct optical_flow_s zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
-
-	if (_px4flow_topic < 0) {
-		debug("failed to create px4flow object. Did you start uOrb?");
-	}
-
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
@@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		/* wait for it to complete */
-		usleep(PX4FLOW_CONVERSION_INTERVAL);
-
 		/* run the collection phase */
 		if (OK != collect()) {
 			ret = -EIO;
@@ -442,8 +441,7 @@ PX4FLOW::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		printf("i2c::transfer flow returned %d");
+		debug("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -455,52 +453,96 @@ PX4FLOW::measure()
 int
 PX4FLOW::collect()
 {
-	int	ret = -EIO;
+	int ret = -EIO;
 
 	/* read from the sensor */
-	uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	uint8_t val[47] = { 0 };
 
 	perf_begin(_sample_perf);
 
-	ret = transfer(nullptr, 0, &val[0], 22);
+	if (PX4FLOW_REG == 0x00) {
+		ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
+	}
+
+	if (PX4FLOW_REG == 0x16) {
+		ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
+	}
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		debug("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
 
-//	f.frame_count = val[1] << 8 | val[0];
-//	f.pixel_flow_x_sum= val[3] << 8 | val[2];
-//	f.pixel_flow_y_sum= val[5] << 8 | val[4];
-//	f.flow_comp_m_x= val[7] << 8 | val[6];
-//	f.flow_comp_m_y= val[9] << 8 | val[8];
-//	f.qual= val[11] << 8 | val[10];
-//	f.gyro_x_rate= val[13] << 8 | val[12];
-//	f.gyro_y_rate= val[15] << 8 | val[14];
-//	f.gyro_z_rate= val[17] << 8 | val[16];
-//	f.gyro_range= val[18];
-//	f.sonar_timestamp= val[19];
-//	f.ground_distance= val[21] << 8 | val[20];
+	if (PX4FLOW_REG == 0) {
+		f.frame_count = val[1] << 8 | val[0];
+		f.pixel_flow_x_sum = val[3] << 8 | val[2];
+		f.pixel_flow_y_sum = val[5] << 8 | val[4];
+		f.flow_comp_m_x = val[7] << 8 | val[6];
+		f.flow_comp_m_y = val[9] << 8 | val[8];
+		f.qual = val[11] << 8 | val[10];
+		f.gyro_x_rate = val[13] << 8 | val[12];
+		f.gyro_y_rate = val[15] << 8 | val[14];
+		f.gyro_z_rate = val[17] << 8 | val[16];
+		f.gyro_range = val[18];
+		f.sonar_timestamp = val[19];
+		f.ground_distance = val[21] << 8 | val[20];
+
+		f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
+		f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
+		f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
+		f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
+		f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
+		f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
+		f_integral.integration_timespan = val[37] << 24 | val[36] << 16
+						  | val[35] << 8 | val[34];
+		f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
+				| val[39] << 8 | val[38];
+		f_integral.ground_distance = val[43] << 8 | val[42];
+		f_integral.gyro_temperature = val[45] << 8 | val[44];
+		f_integral.qual = val[46];
+	}
+
+	if (PX4FLOW_REG == 0x16) {
+		f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
+		f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
+		f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
+		f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
+		f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
+		f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
+		f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
+		f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
+		f_integral.ground_distance = val[21] << 8 | val[20];
+		f_integral.gyro_temperature = val[23] << 8 | val[22];
+		f_integral.qual = val[24];
+	}
 
-	int16_t flowcx = val[7] << 8 | val[6];
-	int16_t flowcy = val[9] << 8 | val[8];
-	int16_t gdist = val[21] << 8 | val[20];
 
 	struct optical_flow_s report;
-	report.flow_comp_x_m = float(flowcx) / 1000.0f;
-	report.flow_comp_y_m = float(flowcy) / 1000.0f;
-	report.flow_raw_x = val[3] << 8 | val[2];
-	report.flow_raw_y = val[5] << 8 | val[4];
-	report.ground_distance_m = float(gdist) / 1000.0f;
-	report.quality =  val[10];
-	report.sensor_id = 0;
+
 	report.timestamp = hrt_absolute_time();
+	report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
+	report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
+	report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
+	report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
+	report.quality = f_integral.qual; //0:bad ; 255 max quality
+	report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
+	report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
+	report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
+	report.integration_timespan = f_integral.integration_timespan; //microseconds
+	report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
 
+	report.sensor_id = 0;
 
-	/* publish it */
-	orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
+	if (_px4flow_topic < 0) {
+		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
+
+	} else {
+		/* publish it */
+		orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
+	}
 
 	/* post a report to the ring */
 	if (_reports->force(&report)) {
@@ -560,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
 void
 PX4FLOW::cycle()
 {
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		if (OK != collect()) {
-			log("collection error");
-			/* restart the measurement state machine */
-			start();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&PX4FLOW::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
-
-			return;
-		}
+	if (OK != measure()) {
+		debug("measure error");
 	}
 
-	/* measurement phase */
-	if (OK != measure()) {
-		log("measure error");
+	/* perform collection */
+	if (OK != collect()) {
+		debug("collection error");
+		/* restart the measurement state machine */
+		start();
+		return;
 	}
 
-	/* next phase is collection */
-	_collect_phase = true;
+	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
+		   _measure_ticks);
 
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&PX4FLOW::cycle_trampoline,
-		   this,
-		   USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
 }
 
 void
@@ -649,14 +662,41 @@ start()
 	}
 
 	/* create the driver */
-	g_dev = new PX4FLOW(PX4FLOW_BUS);
+	g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
 
 	if (g_dev == nullptr) {
 		goto fail;
 	}
 
 	if (OK != g_dev->init()) {
-		goto fail;
+
+		#ifdef PX4_I2C_BUS_ESC
+		delete g_dev;
+		/* try 2nd bus */
+		g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
+
+		if (g_dev == nullptr) {
+			goto fail;
+		}
+
+		if (OK != g_dev->init()) {
+		#endif
+
+			delete g_dev;
+			/* try 3rd bus */
+			g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
+
+			if (g_dev == nullptr) {
+				goto fail;
+			}
+
+			if (OK != g_dev->init()) {
+				goto fail;
+			}
+
+		#ifdef PX4_I2C_BUS_ESC
+		}
+		#endif
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
@@ -685,7 +725,8 @@ fail:
 /**
  * Stop the driver
  */
-void stop()
+void
+stop()
 {
 	if (g_dev != nullptr) {
 		delete g_dev;
@@ -716,6 +757,7 @@ test()
 		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
 	}
 
+
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
@@ -725,18 +767,18 @@ test()
 	}
 
 	warnx("single read");
-	warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
-	warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
-	warnx("time:        %lld", report.timestamp);
-
+	warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
+	warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
+	warnx("framecount_integral: %u",
+	      f_integral.frame_count_since_last_readout);
 
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		errx(1, "failed to set 2Hz poll rate");
+	/* start the sensor polling at 10Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
+		errx(1, "failed to set 10Hz poll rate");
 	}
 
 	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
+	for (unsigned i = 0; i < 10; i++) {
 		struct pollfd fds;
 
 		/* wait for data to be ready */
@@ -756,9 +798,22 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
-		warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
-		warnx("time:        %lld", report.timestamp);
+
+		warnx("framecount_total: %u", f.frame_count);
+		warnx("framecount_integral: %u",
+		      f_integral.frame_count_since_last_readout);
+		warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
+		warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
+		warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
+		warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
+		warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
+		warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
+		warnx("ground_distance: %0.2f m",
+		      (double) f_integral.ground_distance / 1000);
+		warnx("time since last sonar update [us]: %i",
+		      f_integral.time_since_last_sonar_update);
+		warnx("quality integration average : %i", f_integral.qual);
+		warnx("quality : %i", f.qual);
 
 
 	}
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index a60f1a434..a06323a52 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -8,3 +8,5 @@ SRCS		 = fmu.cpp
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 5b838fb75..924283356 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -48,3 +48,5 @@ INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6d68d9f60..58390ba4c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -817,6 +817,11 @@ PX4IO::init()
 
 	}
 
+	/* set safety to off if circuit breaker enabled */
+	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
+		(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
+	}
+
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
 
@@ -1155,52 +1160,54 @@ PX4IO::io_set_arming_state()
 	actuator_armed_s	armed;		///< system armed state
 	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
 
-	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
-	orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
+	int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+	int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed) {
-		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
-
-	} else {
-		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
-	}
-
-	if (armed.lockdown && !_lockdown_override) {
-		set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
-	} else {
-		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
-	}
+        if (have_armed == OK) {
+		if (armed.armed) {
+			set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+		} else {
+			clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+		}
 
-	/* Do not set failsafe if circuit breaker is enabled */
-	if (armed.force_failsafe && !_cb_flighttermination) {
-		set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
-	} else {
-		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
-	}
+		if (armed.lockdown && !_lockdown_override) {
+			set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+		} else {
+			clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+		}
 
-	// XXX this is for future support in the commander
-	// but can be removed if unneeded
-	// if (armed.termination_failsafe) {
-	// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
-	// } else {
-	// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
-	// }
+		/* Do not set failsafe if circuit breaker is enabled */
+		if (armed.force_failsafe && !_cb_flighttermination) {
+			set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+		} else {
+			clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+		}
 
-	if (armed.ready_to_arm) {
-		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+		// XXX this is for future support in the commander
+		// but can be removed if unneeded
+		// if (armed.termination_failsafe) {
+		// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+		// } else {
+		// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+		// }
 
-	} else {
-		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+		if (armed.ready_to_arm) {
+			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+			
+		} else {
+			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+		}
 	}
 
-	if (control_mode.flag_external_manual_override_ok) {
-		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
-
-	} else {
-		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+	if (have_control_mode == OK) {
+		if (control_mode.flag_external_manual_override_ok) {
+			set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+		} else {
+			clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+		}
 	}
 
 	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
@@ -1245,41 +1252,42 @@ PX4IO::io_set_rc_config()
 	 *       for compatibility reasons with existing
 	 *       autopilots / GCS'.
 	 */
-	param_get(param_find("RC_MAP_ROLL"), &ichan);
-
-	/* subtract one from 1-based index - this might be
-	 * a negative number now
-	 */
-	ichan -= 1;
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 0;
+	/* ROLL */
+	param_get(param_find("RC_MAP_ROLL"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 0;
+	}
 
+	/* PITCH */
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 1;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 1;
-
+	/* YAW */
 	param_get(param_find("RC_MAP_YAW"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 2;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 2;
-
+	/* THROTTLE */
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 3;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 3;
-
+	/* FLAPS */
 	param_get(param_find("RC_MAP_FLAPS"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 4;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 4;
-
+	/* MAIN MODE SWITCH */
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
-		input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
 
 	/*
@@ -1656,10 +1664,6 @@ PX4IO::io_publish_raw_rc()
 int
 PX4IO::io_publish_pwm_outputs()
 {
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
 	/* data we are going to fetch */
 	actuator_outputs_s outputs;
 	outputs.timestamp = hrt_absolute_time();
@@ -2060,7 +2064,7 @@ PX4IO::print_status(bool extended_status)
 		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
 	       arming,
 	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
 	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2070,7 +2074,8 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2195,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
 		if (pwm->channel_count > _max_actuators)
 			/* fail with error */
-			return E2BIG;
+			return -E2BIG;
 
 		/* copy values to registers in IO */
 		ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
@@ -2214,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
 		if (pwm->channel_count > _max_actuators)
 			/* fail with error */
-			return E2BIG;
+			return -E2BIG;
 
 		/* copy values to registers in IO */
 		ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
@@ -2233,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
 		if (pwm->channel_count > _max_actuators)
 			/* fail with error */
-			return E2BIG;
+			return -E2BIG;
 
 		/* copy values to registers in IO */
 		ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
@@ -2252,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
 		if (pwm->channel_count > _max_actuators)
 			/* fail with error */
-			return E2BIG;
+			return -E2BIG;
 
 		/* copy values to registers in IO */
 		ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
@@ -2310,6 +2315,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
+		/* control whether override on FMU failure is
+                   immediate or waits for override threshold on mode
+                   switch */
+		if (arg == 0) {
+			/* clear override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
+		} else {
+			/* set override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2571,6 +2589,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_RC_CONFIG: {
+		/* enable setting of RC configuration without relying
+		   on param_get() 
+		*/
+		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
+		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
+			/* fail with error */
+			return -E2BIG;
+		}
+
+		/* copy values to registers in IO */
+		uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+		uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
+		regs[PX4IO_P_RC_CONFIG_MIN]        = config->rc_min;
+		regs[PX4IO_P_RC_CONFIG_CENTER]     = config->rc_trim;
+		regs[PX4IO_P_RC_CONFIG_MAX]        = config->rc_max;
+		regs[PX4IO_P_RC_CONFIG_DEADZONE]   = config->rc_dz;
+		regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
+		regs[PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+		if (config->rc_reverse) {
+			regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+		}
+		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+		break;
+	}
+
+	case PWM_SERVO_SET_OVERRIDE_OK:
+		/* set the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_OVERRIDE_OK:
+		/* clear the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		break;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filep, cmd, arg);
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 13cbfdfa8..d35722244 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -121,7 +121,7 @@ private:
 /* for now, we only support one RGBLED */
 namespace
 {
-RGBLED *g_rgbled;
+RGBLED *g_rgbled = nullptr;
 }
 
 void rgbled_usage();
@@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[])
 
 		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
 		close(fd);
+		/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
+		if (!strcmp(verb, "stop")) {
+			delete g_rgbled;
+			g_rgbled = nullptr;
+			exit(0);
+		}
 		exit(ret);
 	}
 
-	if (!strcmp(verb, "stop")) {
-		delete g_rgbled;
-		g_rgbled = nullptr;
-		exit(0);
-	}
-
 	if (!strcmp(verb, "rgb")) {
 		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
index 48620feea..38ea490a0 100644
--- a/src/drivers/stm32/adc/module.mk
+++ b/src/drivers/stm32/adc/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= adc
 SRCS		= adc.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
index bb751c7f6..54428e270 100644
--- a/src/drivers/stm32/module.mk
+++ b/src/drivers/stm32/module.mk
@@ -41,3 +41,5 @@ SRCS		= drv_hrt.c \
 		  drv_pwm_servo.c
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
index 827cf30b2..25a194ef6 100644
--- a/src/drivers/stm32/tone_alarm/module.mk
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= tone_alarm
 SRCS		= tone_alarm.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/trone/module.mk b/src/drivers/trone/module.mk
new file mode 100644
index 000000000..38499c6c3
--- /dev/null
+++ b/src/drivers/trone/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 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
+# 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.
+#
+############################################################################
+
+#
+# Makefile to build the TeraRanger One range finder driver
+#
+
+MODULE_COMMAND	= trone
+
+SRCS		= trone.cpp
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
new file mode 100644
index 000000000..2f2f692a1
--- /dev/null
+++ b/src/drivers/trone/trone.cpp
@@ -0,0 +1,913 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 trone.cpp
+ * @author Luis Rodrigues
+ *
+ * Driver for the TeraRanger One range finders connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define TRONE_BUS 		PX4_I2C_BUS_EXPANSION
+#define TRONE_BASEADDR 		0x30 /* 7-bit address */
+#define TRONE_DEVICE_PATH 	"/dev/trone"
+
+/* TRONE Registers addresses */
+
+#define TRONE_MEASURE_REG	0x00		/* Measure range register */
+
+/* Device limits */
+#define TRONE_MIN_DISTANCE (0.20f)
+#define TRONE_MAX_DISTANCE (14.00f)
+
+#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class TRONE : public device::I2C
+{
+public:
+	TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR);
+	virtual ~TRONE();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int					_class_instance;
+
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE
+	* and TRONE_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+
+
+};
+
+static const uint8_t crc_table[] = {
+    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
+    0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
+    0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
+    0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
+    0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
+    0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
+    0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
+    0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
+    0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
+    0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
+    0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
+    0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
+    0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
+    0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
+    0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
+    0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
+    0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
+    0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
+    0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
+    0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
+    0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
+    0xfa, 0xfd, 0xf4, 0xf3
+};
+
+uint8_t crc8(uint8_t *p, uint8_t len){
+        uint16_t i;
+        uint16_t crc = 0x0;
+
+        while (len--) {
+                i = (crc ^ *p++) & 0xFF;
+                crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
+        }
+
+        return crc & 0xFF;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int trone_main(int argc, char *argv[]);
+
+TRONE::TRONE(int bus, int address) :
+	I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000),
+	_min_distance(TRONE_MIN_DISTANCE),
+	_max_distance(TRONE_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
+{
+	// up the retries since the device misses the first measure attempts
+	I2C::_retries = 3;
+
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+TRONE::~TRONE()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+	
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
+	}
+	
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+}
+
+int
+TRONE::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		goto out;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {	
+		/* get a publish handle on the range finder topic */
+		struct range_finder_report rf_report;
+		measure();
+		_reports->get(&rf_report);
+		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+
+		if (_range_finder_topic < 0) {
+			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+TRONE::probe()
+{
+	return measure();
+}
+
+void
+TRONE::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+TRONE::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+TRONE::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+TRONE::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+TRONE::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(TRONE_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+TRONE::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	const uint8_t cmd = TRONE_MEASURE_REG;
+	ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+TRONE::collect()
+{
+	int	ret = -EIO;
+
+        /* read from the sensor */
+	uint8_t val[3] = {0, 0, 0};
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 3);
+
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance = (val[0] << 8) | val[1];
+	float si_units = distance *  0.001f; /* mm to m */
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	
+
+	/* publish it, if we are the primary */
+	if (_range_finder_topic >= 0) {
+		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	}
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+TRONE::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+TRONE::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+TRONE::cycle_trampoline(void *arg)
+{
+	TRONE *dev = (TRONE *)arg;
+
+	dev->cycle();
+}
+
+void
+TRONE::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&TRONE::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&TRONE::cycle_trampoline,
+		   this,
+		   USEC2TICK(TRONE_CONVERSION_INTERVAL));
+}
+
+void
+TRONE::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace trone
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+TRONE	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new TRONE(TRONE_BUS);
+
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(TRONE_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		errx(1, "failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 50x and report each value */
+	for (unsigned i = 0; i < 50; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			errx(1, "timed out waiting for sensor data");
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			err(1, "periodic read failed");
+		}
+
+		warnx("periodic read %u", i);
+		warnx("valid %u", report.valid);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+trone_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		trone::start();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		trone::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		trone::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		trone::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		trone::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index c775428ef..0b8c01f79 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					if (vehicle_liftoff || params.debug)
 					{
 						/* copy flow */
-						flow_speed[0] = flow.flow_comp_x_m;
-						flow_speed[1] = flow.flow_comp_y_m;
+						flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
+						flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
 						flow_speed[2] = 0.0f;
 
 						/* convert to bodyframe velocity */
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
 		}
 
-		// Calculate PD + FF throttle
+		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
 		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
 		// Calculate the throttle increment from the specified slew time
 		if (fabsf(_throttle_slewrate) > 0.01f) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
 			_throttle_dem = constrain(_throttle_dem,
-						  _last_throttle_dem - thrRateIncr,
-						  _last_throttle_dem + thrRateIncr);
+						_last_throttle_dem - thrRateIncr,
+						_last_throttle_dem + thrRateIncr);
 		}
 
 		// Ensure _last_throttle_dem is always initialized properly
-		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
 		_last_throttle_dem = _throttle_dem;
 
-
 		// Calculate integrator state upper and lower limits
-		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
 		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
 		float integ_min = (_THRminf - _throttle_dem - 0.1f);
 
 		// Calculate integrator state, constraining state
-		// Set integrator to a max throttle value dduring climbout
+		// Set integrator to a max throttle value during climbout
 		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
 
 		if (_climbOutDem) {
diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m
new file mode 100644
index 000000000..fea1a773e
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m
@@ -0,0 +1,298 @@
+function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+    = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+
+
+%LQG Postion Estimator and Controller
+% Observer:
+%        x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
+%        x[n+1|n] = Ax[n|n] + Bu[n]
+%
+% $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $
+%
+%
+% Arguments:
+% approx_prediction: if 1 then the exponential map is approximated with a
+% first order taylor approximation. has at the moment not a big influence
+% (just 1st or 2nd order approximation) we should change it to rodriquez
+% approximation.
+% use_inertia_matrix: set to true if you have the inertia matrix J for your
+% quadrotor
+% xa_apo_k: old state vectotr
+% zFlag: if sensor measurement is available [gyro, acc, mag]
+% dt: dt in s
+% z: measurements [gyro, acc, mag]
+% q_rotSpeed: process noise gyro
+% q_rotAcc: process noise gyro acceleration
+% q_acc: process noise acceleration
+% q_mag: process noise magnetometer
+% r_gyro: measurement noise gyro
+% r_accel: measurement noise accel
+% r_mag: measurement noise mag
+% J: moment of inertia matrix
+
+
+% Output:
+% xa_apo: updated state vectotr
+% Pa_apo: updated state covariance matrix
+% Rot_matrix: rotation matrix
+% eulerAngles: euler angles
+% debugOutput: not used
+
+
+%% model specific parameters
+
+
+
+% compute once the inverse of the Inertia
+persistent Ji;
+if isempty(Ji)
+    Ji=single(inv(J));
+end
+
+%% init
+persistent x_apo
+if(isempty(x_apo))
+    gyro_init=single([0;0;0]);
+    gyro_acc_init=single([0;0;0]);
+    acc_init=single([0;0;-9.81]);
+    mag_init=single([1;0;0]);
+    x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
+    
+end
+
+persistent P_apo
+if(isempty(P_apo))
+    %     P_apo = single(eye(NSTATES) * 1000);
+    P_apo = single(200*ones(12));
+end
+
+debugOutput = single(zeros(4,1));
+
+%% copy the states
+wx=  x_apo(1);   % x  body angular rate
+wy=  x_apo(2);   % y  body angular rate
+wz=  x_apo(3);   % z  body angular rate
+
+wax=  x_apo(4);  % x  body angular acceleration
+way=  x_apo(5);  % y  body angular acceleration
+waz=  x_apo(6);  % z  body angular acceleration
+
+zex=  x_apo(7);  % x  component gravity vector
+zey=  x_apo(8);  % y  component gravity vector
+zez=  x_apo(9);  % z  component gravity vector
+
+mux=  x_apo(10); % x  component magnetic field vector
+muy=  x_apo(11); % y  component magnetic field vector
+muz=  x_apo(12); % z  component magnetic field vector
+
+
+
+
+%% prediction section
+% compute the apriori state estimate from the previous aposteriori estimate
+%body angular accelerations
+if (use_inertia_matrix==1)
+    wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
+else
+    wak =[wax;way;waz];
+end
+
+%body angular rates
+wk =[wx;  wy; wz] + dt*wak;
+
+%derivative of the prediction rotation matrix
+O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
+
+%prediction of the earth z vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    zek =(O*dt+single(eye(3)))*[zex;zey;zez];
+    
+else
+    zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
+    %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
+    %precision
+end
+
+
+
+%prediction of the magnetic vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    muk =(O*dt+single(eye(3)))*[mux;muy;muz];
+else
+     muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
+    %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
+    %precision
+end
+
+x_apr=[wk;wak;zek;muk];
+
+% compute the apriori error covariance estimate from the previous
+%aposteriori estimate
+
+EZ=[0,zez,-zey;
+    -zez,0,zex;
+    zey,-zex,0]';
+MA=[0,muz,-muy;
+    -muz,0,mux;
+    muy,-mux,0]';
+
+E=single(eye(3));
+Z=single(zeros(3));
+
+A_lin=[ Z,  E,  Z,  Z
+    Z,  Z,  Z,  Z
+    EZ, Z,  O,  Z
+    MA, Z,  Z,  O];
+
+A_lin=eye(12)+A_lin*dt;
+
+%process covariance matrix
+
+persistent Q
+if (isempty(Q))
+    Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
+        q_rotAcc,q_rotAcc,q_rotAcc,...
+        q_acc,q_acc,q_acc,...
+        q_mag,q_mag,q_mag]);
+end
+
+P_apr=A_lin*P_apo*A_lin'+Q;
+
+
+%% update
+if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
+    
+%     R=[r_gyro,0,0,0,0,0,0,0,0;
+%         0,r_gyro,0,0,0,0,0,0,0;
+%         0,0,r_gyro,0,0,0,0,0,0;
+%         0,0,0,r_accel,0,0,0,0,0;
+%         0,0,0,0,r_accel,0,0,0,0;
+%         0,0,0,0,0,r_accel,0,0,0;
+%         0,0,0,0,0,0,r_mag,0,0;
+%         0,0,0,0,0,0,0,r_mag,0;
+%         0,0,0,0,0,0,0,0,r_mag];
+     R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
+    %observation matrix
+    %[zw;ze;zmk];
+    H_k=[  E,     Z,      Z,    Z;
+        Z,     Z,      E,    Z;
+        Z,     Z,      Z,    E];
+    
+    y_k=z(1:9)-H_k*x_apr;
+    
+    
+    %S_k=H_k*P_apr*H_k'+R;
+     S_k=H_k*P_apr*H_k';
+     S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
+    K_k=(P_apr*H_k'/(S_k));
+    
+    
+    x_apo=x_apr+K_k*y_k;
+    P_apo=(eye(12)-K_k*H_k)*P_apr;
+else
+    if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
+        
+        R=[r_gyro,0,0;
+            0,r_gyro,0;
+            0,0,r_gyro];
+        R_v=[r_gyro,r_gyro,r_gyro];
+        %observation matrix
+        
+        H_k=[  E,     Z,      Z,    Z];
+        
+        y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
+        
+       % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
+        S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
+        S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
+        K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
+        
+        
+        x_apo=x_apr+K_k*y_k;
+        P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
+    else
+        if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
+            
+%             R=[r_gyro,0,0,0,0,0;
+%                 0,r_gyro,0,0,0,0;
+%                 0,0,r_gyro,0,0,0;
+%                 0,0,0,r_accel,0,0;
+%                 0,0,0,0,r_accel,0;
+%                 0,0,0,0,0,r_accel];
+            
+            R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
+            %observation matrix
+            
+            H_k=[  E,     Z,      Z,    Z;
+                Z,     Z,      E,    Z];
+            
+            y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
+            
+           % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+            S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+            S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+            K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+            
+            
+            x_apo=x_apr+K_k*y_k;
+            P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+        else
+            if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
+%                 R=[r_gyro,0,0,0,0,0;
+%                     0,r_gyro,0,0,0,0;
+%                     0,0,r_gyro,0,0,0;
+%                     0,0,0,r_mag,0,0;
+%                     0,0,0,0,r_mag,0;
+%                     0,0,0,0,0,r_mag];
+                  R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
+                %observation matrix
+                
+                H_k=[  E,     Z,      Z,    Z;
+                    Z,     Z,      Z,    E];
+                
+                y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
+                
+                %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+                S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+                S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+                K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+                
+                
+                x_apo=x_apr+K_k*y_k;
+                P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+            else
+                x_apo=x_apr;
+                P_apo=P_apr;
+            end
+        end
+    end
+end
+
+
+
+%% euler anglels extraction
+z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
+m_n_b = x_apo(10:12)./norm(x_apo(10:12));
+
+y_n_b=cross(z_n_b,m_n_b);
+y_n_b=y_n_b./norm(y_n_b);
+
+x_n_b=(cross(y_n_b,z_n_b));
+x_n_b=x_n_b./norm(x_n_b);
+
+
+xa_apo=x_apo;
+Pa_apo=P_apo;
+% rotation matrix from earth to body system
+Rot_matrix=[x_n_b,y_n_b,z_n_b];
+
+
+phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
+theta=-asin(Rot_matrix(1,3));
+psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
+eulerAngles=[phi;theta;psi];
+
diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
new file mode 100644
index 000000000..9ea520346
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
@@ -0,0 +1,502 @@
+<?xml version="1.0" encoding="UTF-8"?>

+<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">

+  <configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">

+    <profile key="profile.mex">

+      <param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>

+      <param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>

+      <param.RanInstrumentedMex>false</param.RanInstrumentedMex>

+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>

+      <param.SpecifiedWorkingFolder />

+      <param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>

+      <param.SpecifiedBuildFolder />

+      <param.SearchPaths />

+      <param.ResponsivenessChecks>true</param.ResponsivenessChecks>

+      <param.ExtrinsicCalls>true</param.ExtrinsicCalls>

+      <param.IntegrityChecks>true</param.IntegrityChecks>

+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>

+      <param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>

+      <param.EnableVariableSizing>true</param.EnableVariableSizing>

+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>

+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>

+      <param.StackUsageMax>200000</param.StackUsageMax>

+      <param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>

+      <param.GenerateComments>true</param.GenerateComments>

+      <param.MATLABSourceComments>false</param.MATLABSourceComments>

+      <param.ReservedNameArray />

+      <param.EnableScreener>true</param.EnableScreener>

+      <param.EnableDebugging>false</param.EnableDebugging>

+      <param.GenerateReport>true</param.GenerateReport>

+      <param.LaunchReport>false</param.LaunchReport>

+      <param.CustomSourceCode />

+      <param.CustomHeaderCode />

+      <param.CustomInitializer />

+      <param.CustomTerminator />

+      <param.CustomInclude />

+      <param.CustomSource />

+      <param.CustomLibrary />

+      <param.PostCodeGenCommand />

+      <param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>

+      <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>

+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>

+      <param.RecursionLimit>100</param.RecursionLimit>

+      <param.TargetLang>option.TargetLang.C</param.TargetLang>

+      <param.EchoExpressions>true</param.EchoExpressions>

+      <param.InlineThreshold>10</param.InlineThreshold>

+      <param.InlineThresholdMax>200</param.InlineThresholdMax>

+      <param.InlineStackLimit>4000</param.InlineStackLimit>

+      <param.EnableMemcpy>true</param.EnableMemcpy>

+      <param.MemcpyThreshold>64</param.MemcpyThreshold>

+      <param.EnableOpenMP>true</param.EnableOpenMP>

+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>

+      <param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>

+      <unset>

+        <param.MergeInstrumentationResults />

+        <param.BuiltInstrumentedMex />

+        <param.RanInstrumentedMex />

+        <param.WorkingFolder />

+        <param.SpecifiedWorkingFolder />

+        <param.BuildFolder />

+        <param.SpecifiedBuildFolder />

+        <param.SearchPaths />

+        <param.ResponsivenessChecks />

+        <param.ExtrinsicCalls />

+        <param.IntegrityChecks />

+        <param.SaturateOnIntegerOverflow />

+        <param.GlobalDataSyncMethod />

+        <param.EnableVariableSizing />

+        <param.DynamicMemoryAllocation />

+        <param.DynamicMemoryAllocationThreshold />

+        <param.StackUsageMax />

+        <param.FilePartitionMethod />

+        <param.GenerateComments />

+        <param.MATLABSourceComments />

+        <param.ReservedNameArray />

+        <param.EnableScreener />

+        <param.EnableDebugging />

+        <param.GenerateReport />

+        <param.LaunchReport />

+        <param.CustomSourceCode />

+        <param.CustomHeaderCode />

+        <param.CustomInitializer />

+        <param.CustomTerminator />

+        <param.CustomInclude />

+        <param.CustomSource />

+        <param.CustomLibrary />

+        <param.PostCodeGenCommand />

+        <param.ProposeFixedPointDataTypes />

+        <param.mex.GenCodeOnly />

+        <param.ConstantFoldingTimeout />

+        <param.RecursionLimit />

+        <param.TargetLang />

+        <param.EchoExpressions />

+        <param.InlineThreshold />

+        <param.InlineThresholdMax />

+        <param.InlineStackLimit />

+        <param.EnableMemcpy />

+        <param.MemcpyThreshold />

+        <param.EnableOpenMP />

+        <param.InitFltsAndDblsToZero />

+        <param.ConstantInputs />

+      </unset>

+    </profile>

+    <profile key="profile.c">

+      <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>

+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>

+      <param.SpecifiedWorkingFolder />

+      <param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>

+      <param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>

+      <param.SearchPaths />

+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>

+      <param.PurelyIntegerCode>false</param.PurelyIntegerCode>

+      <param.SupportNonFinite>false</param.SupportNonFinite>

+      <param.EnableVariableSizing>false</param.EnableVariableSizing>

+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>

+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>

+      <param.StackUsageMax>4000</param.StackUsageMax>

+      <param.MultiInstanceCode>false</param.MultiInstanceCode>

+      <param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>

+      <param.GenerateComments>true</param.GenerateComments>

+      <param.MATLABSourceComments>true</param.MATLABSourceComments>

+      <param.MATLABFcnDesc>false</param.MATLABFcnDesc>

+      <param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>

+      <param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>

+      <param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>

+      <param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>

+      <param.MaxIdLength>31</param.MaxIdLength>

+      <param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>

+      <param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>

+      <param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>

+      <param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>

+      <param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>

+      <param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>

+      <param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>

+      <param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>

+      <param.ReservedNameArray />

+      <param.EnableScreener>true</param.EnableScreener>

+      <param.Verbose>false</param.Verbose>

+      <param.GenerateReport>true</param.GenerateReport>

+      <param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>

+      <param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>

+      <param.LaunchReport>true</param.LaunchReport>

+      <param.CustomSourceCode />

+      <param.CustomHeaderCode />

+      <param.CustomInitializer />

+      <param.CustomTerminator />

+      <param.CustomInclude />

+      <param.CustomSource />

+      <param.CustomLibrary />

+      <param.PostCodeGenCommand />

+      <param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>

+      <param.SameHardware>true</param.SameHardware>

+      <param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>

+      <param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>

+      <var.instance.enabled.Production>true</var.instance.enabled.Production>

+      <param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>

+      <param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>

+      <param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>

+      <param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>

+      <param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>

+      <param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>

+      <param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>

+      <param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>

+      <param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>

+      <param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>

+      <param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>

+      <param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>

+      <param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>

+      <param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>

+      <param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>

+      <param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>

+      <param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>

+      <var.instance.enabled.Target>false</var.instance.enabled.Target>

+      <param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>

+      <param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>

+      <param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>

+      <param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>

+      <param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>

+      <param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>

+      <param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>

+      <param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>

+      <param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>

+      <param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>

+      <param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>

+      <param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>

+      <param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>

+      <param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>

+      <param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>

+      <param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>

+      <param.BuildConfiguration>Faster Builds</param.BuildConfiguration>

+      <param.CustomToolchainOptions />

+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>

+      <param.RecursionLimit>100</param.RecursionLimit>

+      <param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>

+      <param.TargetLang>option.TargetLang.C</param.TargetLang>

+      <param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>

+      <param.CCompilerCustomOptimizations />

+      <param.GenerateMakefile>true</param.GenerateMakefile>

+      <param.BuildToolEnable>false</param.BuildToolEnable>

+      <param.MakeCommand>make_rtw</param.MakeCommand>

+      <param.TemplateMakefile>default_tmf</param.TemplateMakefile>

+      <param.BuildToolConfiguration />

+      <param.InlineThreshold>10</param.InlineThreshold>

+      <param.InlineThresholdMax>200</param.InlineThresholdMax>

+      <param.InlineStackLimit>4000</param.InlineStackLimit>

+      <param.EnableMemcpy>true</param.EnableMemcpy>

+      <param.MemcpyThreshold>64</param.MemcpyThreshold>

+      <param.EnableOpenMP>true</param.EnableOpenMP>

+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>

+      <param.PassStructByReference>false</param.PassStructByReference>

+      <param.UseECoderFeatures>true</param.UseECoderFeatures>

+      <unset>

+        <param.WorkingFolder />

+        <param.SpecifiedWorkingFolder />

+        <param.SearchPaths />

+        <param.SaturateOnIntegerOverflow />

+        <param.PurelyIntegerCode />

+        <param.DynamicMemoryAllocation />

+        <param.DynamicMemoryAllocationThreshold />

+        <param.MultiInstanceCode />

+        <param.GenerateComments />

+        <param.MATLABFcnDesc />

+        <param.DataTypeReplacement />

+        <param.ConvertIfToSwitch />

+        <param.PreserveExternInFcnDecls />

+        <param.ParenthesesLevel />

+        <param.MaxIdLength />

+        <param.CustomSymbolStrGlobalVar />

+        <param.CustomSymbolStrType />

+        <param.CustomSymbolStrField />

+        <param.CustomSymbolStrFcn />

+        <param.CustomSymbolStrTmpVar />

+        <param.CustomSymbolStrMacro />

+        <param.CustomSymbolStrEMXArray />

+        <param.CustomSymbolStrEMXArrayFcn />

+        <param.ReservedNameArray />

+        <param.EnableScreener />

+        <param.Verbose />

+        <param.GenerateReport />

+        <param.GenerateCodeMetricsReport />

+        <param.GenerateCodeReplacementReport />

+        <param.CustomInclude />

+        <param.CustomSource />

+        <param.CustomLibrary />

+        <param.SameHardware />

+        <var.instance.enabled.Production />

+        <param.HardwareSizeChar.Production />

+        <param.HardwareSizeShort.Production />

+        <param.HardwareSizeInt.Production />

+        <param.HardwareSizeLong.Production />

+        <param.HardwareSizeLongLong.Production />

+        <param.HardwareSizeFloat.Production />

+        <param.HardwareSizeDouble.Production />

+        <param.HardwareSizeWord.Production />

+        <param.HardwareSizePointer.Production />

+        <param.HardwareEndianness.Production />

+        <param.HardwareLongLongMode.Production />

+        <param.HardwareDivisionRounding.Production />

+        <var.instance.enabled.Target />

+        <param.HardwareSizeChar.Target />

+        <param.HardwareSizeShort.Target />

+        <param.HardwareSizeInt.Target />

+        <param.HardwareSizeLongLong.Target />

+        <param.HardwareSizeFloat.Target />

+        <param.HardwareSizeDouble.Target />

+        <param.HardwareEndianness.Target />

+        <param.HardwareAtomicFloatSize.Target />

+        <param.CustomToolchainOptions />

+        <param.ConstantFoldingTimeout />

+        <param.RecursionLimit />

+        <param.IncludeTerminateFcn />

+        <param.TargetLang />

+        <param.CCompilerCustomOptimizations />

+        <param.GenerateMakefile />

+        <param.BuildToolEnable />

+        <param.MakeCommand />

+        <param.TemplateMakefile />

+        <param.BuildToolConfiguration />

+        <param.InlineThreshold />

+        <param.InlineThresholdMax />

+        <param.InlineStackLimit />

+        <param.EnableMemcpy />

+        <param.MemcpyThreshold />

+        <param.EnableOpenMP />

+        <param.InitFltsAndDblsToZero />

+        <param.UseECoderFeatures />

+      </unset>

+    </profile>

+    <param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>

+    <param.version>R2012a</param.version>

+    <param.HasECoderFeatures>true</param.HasECoderFeatures>

+    <param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>

+    <param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>

+    <param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>

+    <param.VerificationMode>option.VerificationMode.None</param.VerificationMode>

+    <param.SILDebugging>false</param.SILDebugging>

+    <param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>

+    <param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>

+    <param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>

+    <param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>

+    <param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>

+    <param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>

+    <param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>

+    <param.grt.outputfile>AttitudeEKF</param.grt.outputfile>

+    <param.artifact>option.target.artifact.lib</param.artifact>

+    <param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>

+    <param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>

+    <param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>

+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>

+    <param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>

+    <param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>

+    <param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>

+    <param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>

+    <param.UsePreconditions>false</param.UsePreconditions>

+    <param.FeatureFlags />

+    <param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>

+    <param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>

+    <param.ComputedFixedPointData />

+    <param.UserFixedPointData />

+    <param.DefaultWordLength>16</param.DefaultWordLength>

+    <param.DefaultFractionLength>4</param.DefaultFractionLength>

+    <param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>

+    <param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>

+    <param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>

+    <param.StaticAnalysisTimeout />

+    <param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>

+    <param.LogAllIOValues>false</param.LogAllIOValues>

+    <param.LogHistogram>false</param.LogHistogram>

+    <param.ShowCoverage>true</param.ShowCoverage>

+    <param.ExcludedFixedPointVerificationFiles />

+    <param.ExcludedFixedPointSimulationFiles />

+    <param.InstrumentedBuildChecksum />

+    <param.FixedPointStaticAnalysisChecksum />

+    <param.InstrumentedMexFile />

+    <param.FixedPointValidationChecksum />

+    <param.FixedPointSourceCodeChecksum />

+    <param.FixedPointFunctionReplacements />

+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>

+    <param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>

+    <param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>

+    <unset>

+      <param.outputfile />

+      <param.version />

+      <param.HasECoderFeatures />

+      <param.CallGeneratedCodeFromTest />

+      <param.VerificationMode />

+      <param.SILDebugging />

+      <param.AutoInferUseVariableSize />

+      <param.AutoInferUseUnboundedSize />

+      <param.AutoInferVariableSizeThreshold />

+      <param.AutoInferUnboundedSizeThreshold />

+      <param.mex.outputfile />

+      <param.grt.outputfile />

+      <param.FixedPointTypeProposalMode />

+      <param.DefaultProposedFixedPointType />

+      <param.MinMaxSafetyMargin />

+      <param.OptimizeWholeNumbers />

+      <param.LaunchInstrumentationReport />

+      <param.OpenInstrumentationReportInBrowser />

+      <param.CreatePrintableInstrumentationReport />

+      <param.EnableAutoExtrinsicCalls />

+      <param.UsePreconditions />

+      <param.FeatureFlags />

+      <param.FixedPointMode />

+      <param.AutoScaleLoopIndexVariables />

+      <param.ComputedFixedPointData />

+      <param.UserFixedPointData />

+      <param.DefaultWordLength />

+      <param.DefaultFractionLength />

+      <param.FixedPointSafetyMargin />

+      <param.FixedPointFimath />

+      <param.FixedPointTypeSource />

+      <param.StaticAnalysisTimeout />

+      <param.StaticAnalysisGlobalRangesOnly />

+      <param.LogAllIOValues />

+      <param.LogHistogram />

+      <param.ShowCoverage />

+      <param.ExcludedFixedPointVerificationFiles />

+      <param.ExcludedFixedPointSimulationFiles />

+      <param.InstrumentedBuildChecksum />

+      <param.FixedPointStaticAnalysisChecksum />

+      <param.InstrumentedMexFile />

+      <param.FixedPointValidationChecksum />

+      <param.FixedPointSourceCodeChecksum />

+      <param.FixedPointFunctionReplacements />

+      <param.GeneratedFixedPointFileSuffix />

+      <param.DefaultFixedPointSignedness />

+    </unset>

+    <fileset.entrypoints>

+      <file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">

+        <Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">

+          <Input Name="approx_prediction">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="use_inertia_matrix">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="zFlag">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>3 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="dt">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="z">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>9 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_rotSpeed">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_rotAcc">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_acc">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_mag">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_gyro">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_accel">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_mag">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="J">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>3 x 3</Size>

+            <Complex>false</Complex>

+          </Input>

+        </Inputs>

+      </file>

+    </fileset.entrypoints>

+    <fileset.testbench>

+      <file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>

+    </fileset.testbench>

+    <fileset.package />

+    <build-deliverables>

+      <file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>

+    </build-deliverables>

+    <workflow />

+    <matlab>

+      <root>/opt/matlab/r2013b</root>

+      <toolboxes>

+        <toolbox name="fixedpoint" />

+      </toolboxes>

+    </matlab>

+    <platform>

+      <unix>true</unix>

+      <mac>false</mac>

+      <windows>false</windows>

+      <win2k>false</win2k>

+      <winxp>false</winxp>

+      <vista>false</vista>

+      <linux>true</linux>

+      <solaris>false</solaris>

+      <osver>3.16.1-1-ARCH</osver>

+      <os32>false</os32>

+      <os64>true</os64>

+      <arch>glnxa64</arch>

+      <matlab>true</matlab>

+    </platform>

+  </configuration>

+</deployment-project>

+

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 35dc39ec6..c1f032b49 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -38,6 +38,7 @@
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -74,8 +75,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "codegen/attitudeKalmanfilter_initialize.h"
-#include "codegen/attitudeKalmanfilter.h"
+#include "codegen/AttitudeEKF.h"
 #include "attitude_estimator_ekf_params.h"
 #ifdef __cplusplus
 }
@@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      14000,
+					      7200,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -206,14 +206,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
+	float debugOutput[4] = { 0.0f };
 
 	int overloadcounter = 19;
 
 	/* Initialize filter */
-	attitudeKalmanfilter_initialize();
+	AttitudeEKF_initialize();
 
 	/* store start time to guard against too slow update rates */
 	uint64_t last_run = hrt_absolute_time();
@@ -277,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_ekf_params ekf_params;
+	struct attitude_estimator_ekf_params ekf_params = { 0 };
 
-	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
 
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
@@ -508,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
+					/* Call the estimator */
+					AttitudeEKF(false, // approx_prediction
+							(unsigned char)ekf_params.use_moment_inertia,
+							update_vect,
+							dt,
+							z_k,
+							ekf_params.q[0], // q_rotSpeed,
+							ekf_params.q[1], // q_rotAcc
+							ekf_params.q[2], // q_acc
+							ekf_params.q[3], // q_mag
+							ekf_params.r[0], // r_gyro
+							ekf_params.r[1], // r_accel
+							ekf_params.r[2], // r_mag
+							ekf_params.moment_inertia_J,
+							x_aposteriori,
+							P_aposteriori,
+							Rot_matrix,
+							euler,
+							debugOutput);
 
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index bc0e3b93a..5637ec102 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,28 +44,96 @@
 
 /* Extended Kalman Filter covariances */
 
-/* gyro process noise */
+
+/**
+ * Body angular rate process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+
+/**
+ * Body angular acceleration process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+
+/**
+ * Acceleration process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
-/* gyro offsets process noise */
+
+/**
+ * Magnet field vector process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
-/* gyro measurement noise */
+/**
+ * Gyro measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
-/* accel measurement noise */
+
+/**
+ * Accel measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
-/* mag measurement noise */
+
+/**
+ * Mag measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-/* offset estimation - UNUSED */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
 PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
+/**
+ * Moment of inertia matrix diagonal entry (1, 1)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (2, 2)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (3, 3)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
+
+/**
+ * Moment of inertia enabled in estimator
+ *
+ * If set to != 0 the moment of inertia will be used in the estimator
+ *
+ * @group attitude_ekf
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_J_EN, 0);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q1 	=	param_find("EKF_ATT_V3_Q1");
 	h->q2 	=	param_find("EKF_ATT_V3_Q2");
 	h->q3 	=	param_find("EKF_ATT_V3_Q3");
-	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
 	h->r0 	=	param_find("EKF_ATT_V4_R0");
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
-	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
+	h->moment_inertia_J[0]  =   param_find("ATT_J11");
+	h->moment_inertia_J[1]  =   param_find("ATT_J22");
+	h->moment_inertia_J[2]  =   param_find("ATT_J33");
+	h->use_moment_inertia	=   param_find("ATT_J_EN");
+
 	return OK;
 }
 
@@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q1, &(p->q[1]));
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));
 
 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));
 
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI_F / 180.0f;
 
 	param_get(h->acc_comp, &(p->acc_comp));
 
+	for (int i = 0; i < 3; i++) {
+		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
+	}
+	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
+
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 5985541ca..5d3b6b244 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -42,8 +42,10 @@
 #include <systemlib/param/param.h>
 
 struct attitude_estimator_ekf_params {
-	float r[9];
-	float q[12];
+	float r[3];
+	float q[4];
+	float moment_inertia_J[9];
+	int32_t use_moment_inertia;
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
@@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
 };
 
 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3;
-	param_t q0, q1, q2, q3, q4;
+	param_t r0, r1, r2;
+	param_t q0, q1, q2, q3;
+	param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
+	param_t use_moment_inertia;
 	param_t mag_decl;
 	param_t acc_comp;
 };
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
new file mode 100644
index 000000000..68db382cf
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
@@ -0,0 +1,1456 @@
+/*
+ * AttitudeEKF.c
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+/* Include files */
+#include "AttitudeEKF.h"
+
+/* Variable Definitions */
+static float Ji[9];
+static boolean_T Ji_not_empty;
+static float x_apo[12];
+static float P_apo[144];
+static float Q[144];
+static boolean_T Q_not_empty;
+
+/* Function Declarations */
+static void AttitudeEKF_init(void);
+static void b_mrdivide(const float A[72], const float B[36], float y[72]);
+static void inv(const float x[9], float y[9]);
+static void mrdivide(const float A[108], const float B[81], float y[108]);
+static float norm(const float x[3]);
+
+/* Function Definitions */
+static void AttitudeEKF_init(void)
+{
+  int i;
+  static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+    -9.81F, 1.0F, 0.0F, 0.0F };
+
+  for (i = 0; i < 12; i++) {
+    x_apo[i] = fv5[i];
+  }
+
+  for (i = 0; i < 144; i++) {
+    P_apo[i] = 200.0F;
+  }
+}
+
+/*
+ *
+ */
+static void b_mrdivide(const float A[72], const float B[36], float y[72])
+{
+  float b_A[36];
+  signed char ipiv[6];
+  int i1;
+  int iy;
+  int j;
+  int c;
+  int ix;
+  float temp;
+  int k;
+  float s;
+  int jy;
+  int ijA;
+  float Y[72];
+  for (i1 = 0; i1 < 6; i1++) {
+    for (iy = 0; iy < 6; iy++) {
+      b_A[iy + 6 * i1] = B[i1 + 6 * iy];
+    }
+
+    ipiv[i1] = (signed char)(1 + i1);
+  }
+
+  for (j = 0; j < 5; j++) {
+    c = j * 7;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 6 - j; k++) {
+      ix++;
+      s = (real32_T)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (signed char)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 6; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 6;
+          iy += 6;
+        }
+      }
+
+      i1 = (c - j) + 6;
+      for (jy = c + 1; jy + 1 <= i1; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    iy = c;
+    jy = c + 6;
+    for (k = 1; k <= 5 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i1 = (iy - j) + 12;
+        for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
+          ix++;
+        }
+      }
+
+      jy += 6;
+      iy += 6;
+    }
+  }
+
+  for (i1 = 0; i1 < 12; i1++) {
+    for (iy = 0; iy < 6; iy++) {
+      Y[iy + 6 * i1] = A[i1 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 6; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 6 * j];
+        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+        Y[(ipiv[jy] + 6 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 0; k < 6; k++) {
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 7; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 5; k > -1; k += -1) {
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+        }
+      }
+    }
+  }
+
+  for (i1 = 0; i1 < 6; i1++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i1] = Y[i1 + 6 * iy];
+    }
+  }
+}
+
+/*
+ *
+ */
+static void inv(const float x[9], float y[9])
+{
+  float b_x[9];
+  int p1;
+  int p2;
+  int p3;
+  float absx11;
+  float absx21;
+  float absx31;
+  int itmp;
+  for (p1 = 0; p1 < 9; p1++) {
+    b_x[p1] = x[p1];
+  }
+
+  p1 = 0;
+  p2 = 3;
+  p3 = 6;
+  absx11 = (real32_T)fabs(x[0]);
+  absx21 = (real32_T)fabs(x[1]);
+  absx31 = (real32_T)fabs(x[2]);
+  if ((absx21 > absx11) && (absx21 > absx31)) {
+    p1 = 3;
+    p2 = 0;
+    b_x[0] = x[1];
+    b_x[1] = x[0];
+    b_x[3] = x[4];
+    b_x[4] = x[3];
+    b_x[6] = x[7];
+    b_x[7] = x[6];
+  } else {
+    if (absx31 > absx11) {
+      p1 = 6;
+      p3 = 0;
+      b_x[0] = x[2];
+      b_x[2] = x[0];
+      b_x[3] = x[5];
+      b_x[5] = x[3];
+      b_x[6] = x[8];
+      b_x[8] = x[6];
+    }
+  }
+
+  absx11 = b_x[1] / b_x[0];
+  b_x[1] /= b_x[0];
+  absx21 = b_x[2] / b_x[0];
+  b_x[2] /= b_x[0];
+  b_x[4] -= absx11 * b_x[3];
+  b_x[5] -= absx21 * b_x[3];
+  b_x[7] -= absx11 * b_x[6];
+  b_x[8] -= absx21 * b_x[6];
+  if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) {
+    itmp = p2;
+    p2 = p3;
+    p3 = itmp;
+    b_x[1] = absx21;
+    b_x[2] = absx11;
+    absx11 = b_x[4];
+    b_x[4] = b_x[5];
+    b_x[5] = absx11;
+    absx11 = b_x[7];
+    b_x[7] = b_x[8];
+    b_x[8] = absx11;
+  }
+
+  absx11 = b_x[5] / b_x[4];
+  b_x[5] /= b_x[4];
+  b_x[8] -= absx11 * b_x[7];
+  absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8];
+  absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4];
+  y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
+  y[p1 + 1] = absx21;
+  y[p1 + 2] = absx11;
+  absx11 = -b_x[5] / b_x[8];
+  absx21 = (1.0F - b_x[7] * absx11) / b_x[4];
+  y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
+  y[p2 + 1] = absx21;
+  y[p2 + 2] = absx11;
+  absx11 = 1.0F / b_x[8];
+  absx21 = -b_x[7] * absx11 / b_x[4];
+  y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
+  y[p3 + 1] = absx21;
+  y[p3 + 2] = absx11;
+}
+
+/*
+ *
+ */
+static void mrdivide(const float A[108], const float B[81], float y[108])
+{
+  float b_A[81];
+  signed char ipiv[9];
+  int i0;
+  int iy;
+  int j;
+  int c;
+  int ix;
+  float temp;
+  int k;
+  float s;
+  int jy;
+  int ijA;
+  float Y[108];
+  for (i0 = 0; i0 < 9; i0++) {
+    for (iy = 0; iy < 9; iy++) {
+      b_A[iy + 9 * i0] = B[i0 + 9 * iy];
+    }
+
+    ipiv[i0] = (signed char)(1 + i0);
+  }
+
+  for (j = 0; j < 8; j++) {
+    c = j * 10;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 9 - j; k++) {
+      ix++;
+      s = (real32_T)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (signed char)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 9; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 9;
+          iy += 9;
+        }
+      }
+
+      i0 = (c - j) + 9;
+      for (jy = c + 1; jy + 1 <= i0; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    iy = c;
+    jy = c + 9;
+    for (k = 1; k <= 8 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i0 = (iy - j) + 18;
+        for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
+          ix++;
+        }
+      }
+
+      jy += 9;
+      iy += 9;
+    }
+  }
+
+  for (i0 = 0; i0 < 12; i0++) {
+    for (iy = 0; iy < 9; iy++) {
+      Y[iy + 9 * i0] = A[i0 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 9; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 9 * j];
+        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+        Y[(ipiv[jy] + 9 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 0; k < 9; k++) {
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 10; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 8; k > -1; k += -1) {
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+        }
+      }
+    }
+  }
+
+  for (i0 = 0; i0 < 9; i0++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i0] = Y[i0 + 9 * iy];
+    }
+  }
+}
+
+/*
+ *
+ */
+static float norm(const float x[3])
+{
+  float y;
+  float scale;
+  int k;
+  float absxk;
+  float t;
+  y = 0.0F;
+  scale = 1.17549435E-38F;
+  for (k = 0; k < 3; k++) {
+    absxk = (real32_T)fabs(x[k]);
+    if (absxk > scale) {
+      t = scale / absxk;
+      y = 1.0F + y * t * t;
+      scale = absxk;
+    } else {
+      t = absxk / scale;
+      y += t * t;
+    }
+  }
+
+  return scale * (real32_T)sqrt(y);
+}
+
+/*
+ * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+ *     = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+ */
+void AttitudeEKF(unsigned char approx_prediction, unsigned char
+                 use_inertia_matrix, const unsigned char zFlag[3], float dt,
+                 const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc,
+                 float q_mag, float r_gyro, float r_accel, float r_mag, const
+                 float J[9], float xa_apo[12], float Pa_apo[144], float
+                 Rot_matrix[9], float eulerAngles[3], float debugOutput[4])
+{
+  int i;
+  float fv0[3];
+  int r2;
+  float zek[3];
+  float muk[3];
+  float b_muk[3];
+  float c_muk[3];
+  float fv1[3];
+  float wak[3];
+  float O[9];
+  float b_O[9];
+  static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
+
+  float fv2[3];
+  float maxval;
+  int r1;
+  float fv3[9];
+  float fv4[3];
+  float x_apr[12];
+  signed char I[144];
+  static float A_lin[144];
+  static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1,
+    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  static float b_A_lin[144];
+  float v[12];
+  static float P_apr[144];
+  float a[108];
+  static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+  float S_k[81];
+  static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+  float b_r_gyro[9];
+  float K_k[108];
+  float b_S_k[36];
+  static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  float c_r_gyro[3];
+  float B[36];
+  int r3;
+  float a21;
+  float Y[36];
+  float d_a[72];
+  static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0 };
+
+  static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0 };
+
+  float d_r_gyro[6];
+  float c_S_k[6];
+  float b_K_k[72];
+  static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1 };
+
+  static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1 };
+
+  float b_z[6];
+
+  /* LQG Postion Estimator and Controller */
+  /*  Observer: */
+  /*         x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */
+  /*         x[n+1|n] = Ax[n|n] + Bu[n] */
+  /*  */
+  /*  $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $ */
+  /*  */
+  /*  */
+  /*  Arguments: */
+  /*  approx_prediction: if 1 then the exponential map is approximated with a */
+  /*  first order taylor approximation. has at the moment not a big influence */
+  /*  (just 1st or 2nd order approximation) we should change it to rodriquez */
+  /*  approximation. */
+  /*  use_inertia_matrix: set to true if you have the inertia matrix J for your */
+  /*  quadrotor */
+  /*  xa_apo_k: old state vectotr */
+  /*  zFlag: if sensor measurement is available [gyro, acc, mag] */
+  /*  dt: dt in s */
+  /*  z: measurements [gyro, acc, mag] */
+  /*  q_rotSpeed: process noise gyro */
+  /*  q_rotAcc: process noise gyro acceleration */
+  /*  q_acc: process noise acceleration */
+  /*  q_mag: process noise magnetometer */
+  /*  r_gyro: measurement noise gyro */
+  /*  r_accel: measurement noise accel */
+  /*  r_mag: measurement noise mag */
+  /*  J: moment of inertia matrix */
+  /*  Output: */
+  /*  xa_apo: updated state vectotr */
+  /*  Pa_apo: updated state covariance matrix */
+  /*  Rot_matrix: rotation matrix */
+  /*  eulerAngles: euler angles */
+  /*  debugOutput: not used */
+  /* % model specific parameters */
+  /*  compute once the inverse of the Inertia */
+  /* 'AttitudeEKF:48' if isempty(Ji) */
+  if (!Ji_not_empty) {
+    /* 'AttitudeEKF:49' Ji=single(inv(J)); */
+    inv(J, Ji);
+    Ji_not_empty = TRUE;
+  }
+
+  /* % init */
+  /* 'AttitudeEKF:54' if(isempty(x_apo)) */
+  /* 'AttitudeEKF:64' if(isempty(P_apo)) */
+  /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */
+  for (i = 0; i < 4; i++) {
+    debugOutput[i] = 0.0F;
+  }
+
+  /* % copy the states */
+  /* 'AttitudeEKF:72' wx=  x_apo(1); */
+  /*  x  body angular rate */
+  /* 'AttitudeEKF:73' wy=  x_apo(2); */
+  /*  y  body angular rate */
+  /* 'AttitudeEKF:74' wz=  x_apo(3); */
+  /*  z  body angular rate */
+  /* 'AttitudeEKF:76' wax=  x_apo(4); */
+  /*  x  body angular acceleration */
+  /* 'AttitudeEKF:77' way=  x_apo(5); */
+  /*  y  body angular acceleration */
+  /* 'AttitudeEKF:78' waz=  x_apo(6); */
+  /*  z  body angular acceleration */
+  /* 'AttitudeEKF:80' zex=  x_apo(7); */
+  /*  x  component gravity vector */
+  /* 'AttitudeEKF:81' zey=  x_apo(8); */
+  /*  y  component gravity vector */
+  /* 'AttitudeEKF:82' zez=  x_apo(9); */
+  /*  z  component gravity vector */
+  /* 'AttitudeEKF:84' mux=  x_apo(10); */
+  /*  x  component magnetic field vector */
+  /* 'AttitudeEKF:85' muy=  x_apo(11); */
+  /*  y  component magnetic field vector */
+  /* 'AttitudeEKF:86' muz=  x_apo(12); */
+  /*  z  component magnetic field vector */
+  /* % prediction section */
+  /*  compute the apriori state estimate from the previous aposteriori estimate */
+  /* body angular accelerations */
+  /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */
+  if (use_inertia_matrix == 1) {
+    /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */
+    fv0[0] = x_apo[3];
+    fv0[1] = x_apo[4];
+    fv0[2] = x_apo[5];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += J[r2 + 3 * i] * fv0[i];
+      }
+    }
+
+    muk[0] = x_apo[3];
+    muk[1] = x_apo[4];
+    muk[2] = x_apo[5];
+    b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1];
+    b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2];
+    b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0];
+    for (r2 = 0; r2 < 3; r2++) {
+      c_muk[r2] = -b_muk[r2];
+    }
+
+    fv1[0] = x_apo[3];
+    fv1[1] = x_apo[4];
+    fv1[2] = x_apo[5];
+    for (r2 = 0; r2 < 3; r2++) {
+      fv0[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        fv0[r2] += Ji[r2 + 3 * i] * c_muk[i];
+      }
+
+      wak[r2] = fv1[r2] + fv0[r2] * dt;
+    }
+  } else {
+    /* 'AttitudeEKF:96' else */
+    /* 'AttitudeEKF:97' wak =[wax;way;waz]; */
+    wak[0] = x_apo[3];
+    wak[1] = x_apo[4];
+    wak[2] = x_apo[5];
+  }
+
+  /* body angular rates */
+  /* 'AttitudeEKF:101' wk =[wx;  wy; wz] + dt*wak; */
+  /* derivative of the prediction rotation matrix */
+  /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+  O[0] = 0.0F;
+  O[1] = -x_apo[2];
+  O[2] = x_apo[1];
+  O[3] = x_apo[2];
+  O[4] = 0.0F;
+  O[5] = -x_apo[0];
+  O[6] = -x_apo[1];
+  O[7] = x_apo[0];
+  O[8] = 0.0F;
+
+  /* prediction of the earth z vector */
+  /* 'AttitudeEKF:107' if (approx_prediction==1) */
+  if (approx_prediction == 1) {
+    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
+    /*  so we do a first order approximation of the exponential map */
+    /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
+      }
+    }
+
+    fv2[0] = x_apo[6];
+    fv2[1] = x_apo[7];
+    fv2[2] = x_apo[8];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += b_O[r2 + 3 * i] * fv2[i];
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:112' else */
+    /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */
+    maxval = dt * dt / 2.0F;
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[r2 + 3 * i] = 0.0F;
+        for (r1 = 0; r1 < 3; r1++) {
+          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
+        }
+      }
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
+          * b_O[i + 3 * r2];
+      }
+    }
+
+    fv2[0] = x_apo[6];
+    fv2[1] = x_apo[7];
+    fv2[2] = x_apo[8];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += fv3[r2 + 3 * i] * fv2[i];
+      }
+    }
+
+    /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */
+    /* precision */
+  }
+
+  /* prediction of the magnetic vector */
+  /* 'AttitudeEKF:121' if (approx_prediction==1) */
+  if (approx_prediction == 1) {
+    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
+    /*  so we do a first order approximation of the exponential map */
+    /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
+      }
+    }
+
+    fv4[0] = x_apo[9];
+    fv4[1] = x_apo[10];
+    fv4[2] = x_apo[11];
+    for (r2 = 0; r2 < 3; r2++) {
+      muk[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        muk[r2] += b_O[r2 + 3 * i] * fv4[i];
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:125' else */
+    /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */
+    maxval = dt * dt / 2.0F;
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[r2 + 3 * i] = 0.0F;
+        for (r1 = 0; r1 < 3; r1++) {
+          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
+        }
+      }
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
+          * b_O[i + 3 * r2];
+      }
+    }
+
+    fv4[0] = x_apo[9];
+    fv4[1] = x_apo[10];
+    fv4[2] = x_apo[11];
+    for (r2 = 0; r2 < 3; r2++) {
+      muk[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        muk[r2] += fv3[r2 + 3 * i] * fv4[i];
+      }
+    }
+
+    /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */
+    /* precision */
+  }
+
+  /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */
+  x_apr[0] = x_apo[0] + dt * wak[0];
+  x_apr[1] = x_apo[1] + dt * wak[1];
+  x_apr[2] = x_apo[2] + dt * wak[2];
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 3] = wak[i];
+  }
+
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 6] = zek[i];
+  }
+
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 9] = muk[i];
+  }
+
+  /*  compute the apriori error covariance estimate from the previous */
+  /* aposteriori estimate */
+  /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */
+  /* 'AttitudeEKF:137'     -zez,0,zex; */
+  /* 'AttitudeEKF:138'     zey,-zex,0]'; */
+  /* 'AttitudeEKF:139' MA=[0,muz,-muy; */
+  /* 'AttitudeEKF:140'     -muz,0,mux; */
+  /* 'AttitudeEKF:141'     muy,-mux,0]'; */
+  /* 'AttitudeEKF:143' E=single(eye(3)); */
+  /* 'AttitudeEKF:144' Z=single(zeros(3)); */
+  /* 'AttitudeEKF:146' A_lin=[ Z,  E,  Z,  Z */
+  /* 'AttitudeEKF:147'     Z,  Z,  Z,  Z */
+  /* 'AttitudeEKF:148'     EZ, Z,  O,  Z */
+  /* 'AttitudeEKF:149'     MA, Z,  Z,  O]; */
+  /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */
+  memset(&I[0], 0, 144U * sizeof(signed char));
+  for (i = 0; i < 12; i++) {
+    I[i + 12 * i] = 1;
+    for (r2 = 0; r2 < 3; r2++) {
+      A_lin[r2 + 12 * i] = iv1[r2 + 3 * i];
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      A_lin[(r2 + 12 * i) + 3] = 0.0F;
+    }
+  }
+
+  A_lin[6] = 0.0F;
+  A_lin[7] = x_apo[8];
+  A_lin[8] = -x_apo[7];
+  A_lin[18] = -x_apo[8];
+  A_lin[19] = 0.0F;
+  A_lin[20] = x_apo[6];
+  A_lin[30] = x_apo[7];
+  A_lin[31] = -x_apo[6];
+  A_lin[32] = 0.0F;
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2];
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F;
+    }
+  }
+
+  A_lin[9] = 0.0F;
+  A_lin[10] = x_apo[11];
+  A_lin[11] = -x_apo[10];
+  A_lin[21] = -x_apo[11];
+  A_lin[22] = 0.0F;
+  A_lin[23] = x_apo[9];
+  A_lin[33] = x_apo[10];
+  A_lin[34] = -x_apo[9];
+  A_lin[35] = 0.0F;
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2];
+    }
+  }
+
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt;
+    }
+  }
+
+  /* process covariance matrix */
+  /* 'AttitudeEKF:156' if (isempty(Q)) */
+  if (!Q_not_empty) {
+    /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */
+    /* 'AttitudeEKF:158'         q_rotAcc,q_rotAcc,q_rotAcc,... */
+    /* 'AttitudeEKF:159'         q_acc,q_acc,q_acc,... */
+    /* 'AttitudeEKF:160'         q_mag,q_mag,q_mag]); */
+    v[0] = q_rotSpeed;
+    v[1] = q_rotSpeed;
+    v[2] = q_rotSpeed;
+    v[3] = q_rotAcc;
+    v[4] = q_rotAcc;
+    v[5] = q_rotAcc;
+    v[6] = q_acc;
+    v[7] = q_acc;
+    v[8] = q_acc;
+    v[9] = q_mag;
+    v[10] = q_mag;
+    v[11] = q_mag;
+    memset(&Q[0], 0, 144U * sizeof(float));
+    for (i = 0; i < 12; i++) {
+      Q[i + 12 * i] = v[i];
+    }
+
+    Q_not_empty = TRUE;
+  }
+
+  /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      A_lin[r2 + 12 * i] = 0.0F;
+      for (r1 = 0; r1 < 12; r1++) {
+        A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i];
+      }
+    }
+  }
+
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      maxval = 0.0F;
+      for (r1 = 0; r1 < 12; r1++) {
+        maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1];
+      }
+
+      P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i];
+    }
+  }
+
+  /* % update */
+  /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */
+  if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) {
+    /*      R=[r_gyro,0,0,0,0,0,0,0,0; */
+    /*          0,r_gyro,0,0,0,0,0,0,0; */
+    /*          0,0,r_gyro,0,0,0,0,0,0; */
+    /*          0,0,0,r_accel,0,0,0,0,0; */
+    /*          0,0,0,0,r_accel,0,0,0,0; */
+    /*          0,0,0,0,0,r_accel,0,0,0; */
+    /*          0,0,0,0,0,0,r_mag,0,0; */
+    /*          0,0,0,0,0,0,0,r_mag,0; */
+    /*          0,0,0,0,0,0,0,0,r_mag]; */
+    /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */
+    /* observation matrix */
+    /* [zw;ze;zmk]; */
+    /* 'AttitudeEKF:181' H_k=[  E,     Z,      Z,    Z; */
+    /* 'AttitudeEKF:182'         Z,     Z,      E,    Z; */
+    /* 'AttitudeEKF:183'         Z,     Z,      Z,    E]; */
+    /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */
+    /* S_k=H_k*P_apr*H_k'+R; */
+    /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */
+    for (r2 = 0; r2 < 9; r2++) {
+      for (i = 0; i < 12; i++) {
+        a[r2 + 9 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i];
+        }
+      }
+
+      for (i = 0; i < 9; i++) {
+        S_k[r2 + 9 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i];
+        }
+      }
+    }
+
+    /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */
+    b_r_gyro[0] = r_gyro;
+    b_r_gyro[1] = r_gyro;
+    b_r_gyro[2] = r_gyro;
+    b_r_gyro[3] = r_accel;
+    b_r_gyro[4] = r_accel;
+    b_r_gyro[5] = r_accel;
+    b_r_gyro[6] = r_mag;
+    b_r_gyro[7] = r_mag;
+    b_r_gyro[8] = r_mag;
+    for (r2 = 0; r2 < 9; r2++) {
+      b_O[r2] = S_k[10 * r2] + b_r_gyro[r2];
+    }
+
+    for (r2 = 0; r2 < 9; r2++) {
+      S_k[10 * r2] = b_O[r2];
+    }
+
+    /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 9; i++) {
+        a[r2 + 12 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i];
+        }
+      }
+    }
+
+    mrdivide(a, S_k, K_k);
+
+    /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */
+    for (r2 = 0; r2 < 9; r2++) {
+      maxval = 0.0F;
+      for (i = 0; i < 12; i++) {
+        maxval += (float)b_a[r2 + 9 * i] * x_apr[i];
+      }
+
+      b_O[r2] = z[r2] - maxval;
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      maxval = 0.0F;
+      for (i = 0; i < 9; i++) {
+        maxval += K_k[r2 + 12 * i] * b_O[i];
+      }
+
+      x_apo[r2] = x_apr[r2] + maxval;
+    }
+
+    /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */
+    memset(&I[0], 0, 144U * sizeof(signed char));
+    for (i = 0; i < 12; i++) {
+      I[i + 12 * i] = 1;
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 12; i++) {
+        maxval = 0.0F;
+        for (r1 = 0; r1 < 9; r1++) {
+          maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i];
+        }
+
+        A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+      }
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 12; i++) {
+        P_apo[r2 + 12 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+        }
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:196' else */
+    /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */
+    if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) {
+      /* 'AttitudeEKF:199' R=[r_gyro,0,0; */
+      /* 'AttitudeEKF:200'             0,r_gyro,0; */
+      /* 'AttitudeEKF:201'             0,0,r_gyro]; */
+      /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */
+      /* observation matrix */
+      /* 'AttitudeEKF:205' H_k=[  E,     Z,      Z,    Z]; */
+      /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */
+      /*  S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */
+      /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 12; i++) {
+          b_S_k[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i];
+          }
+        }
+
+        for (i = 0; i < 3; i++) {
+          O[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i];
+          }
+        }
+      }
+
+      /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */
+      c_r_gyro[0] = r_gyro;
+      c_r_gyro[1] = r_gyro;
+      c_r_gyro[2] = r_gyro;
+      for (r2 = 0; r2 < 3; r2++) {
+        b_muk[r2] = O[r2 << 2] + c_r_gyro[r2];
+      }
+
+      for (r2 = 0; r2 < 3; r2++) {
+        O[r2 << 2] = b_muk[r2];
+      }
+
+      /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 3; i++) {
+          b_O[i + 3 * r2] = O[r2 + 3 * i];
+        }
+
+        for (i = 0; i < 12; i++) {
+          B[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2];
+          }
+        }
+      }
+
+      r1 = 0;
+      r2 = 1;
+      r3 = 2;
+      maxval = (real32_T)fabs(b_O[0]);
+      a21 = (real32_T)fabs(b_O[1]);
+      if (a21 > maxval) {
+        maxval = a21;
+        r1 = 1;
+        r2 = 0;
+      }
+
+      if ((real32_T)fabs(b_O[2]) > maxval) {
+        r1 = 2;
+        r2 = 1;
+        r3 = 0;
+      }
+
+      b_O[r2] /= b_O[r1];
+      b_O[r3] /= b_O[r1];
+      b_O[3 + r2] -= b_O[r2] * b_O[3 + r1];
+      b_O[3 + r3] -= b_O[r3] * b_O[3 + r1];
+      b_O[6 + r2] -= b_O[r2] * b_O[6 + r1];
+      b_O[6 + r3] -= b_O[r3] * b_O[6 + r1];
+      if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) {
+        i = r2;
+        r2 = r3;
+        r3 = i;
+      }
+
+      b_O[3 + r3] /= b_O[3 + r2];
+      b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2];
+      for (i = 0; i < 12; i++) {
+        Y[3 * i] = B[r1 + 3 * i];
+        Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2];
+        Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] *
+          b_O[3 + r3];
+        Y[2 + 3 * i] /= b_O[6 + r3];
+        Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1];
+        Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2];
+        Y[1 + 3 * i] /= b_O[3 + r2];
+        Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1];
+        Y[3 * i] /= b_O[r1];
+      }
+
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 12; i++) {
+          b_S_k[i + 12 * r2] = Y[r2 + 3 * i];
+        }
+      }
+
+      /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */
+      for (r2 = 0; r2 < 3; r2++) {
+        maxval = 0.0F;
+        for (i = 0; i < 12; i++) {
+          maxval += (float)c_a[r2 + 3 * i] * x_apr[i];
+        }
+
+        b_muk[r2] = z[r2] - maxval;
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        maxval = 0.0F;
+        for (i = 0; i < 3; i++) {
+          maxval += b_S_k[r2 + 12 * i] * b_muk[i];
+        }
+
+        x_apo[r2] = x_apr[r2] + maxval;
+      }
+
+      /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */
+      memset(&I[0], 0, 144U * sizeof(signed char));
+      for (i = 0; i < 12; i++) {
+        I[i + 12 * i] = 1;
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        for (i = 0; i < 12; i++) {
+          maxval = 0.0F;
+          for (r1 = 0; r1 < 3; r1++) {
+            maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i];
+          }
+
+          A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+        }
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        for (i = 0; i < 12; i++) {
+          P_apo[r2 + 12 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+          }
+        }
+      }
+    } else {
+      /* 'AttitudeEKF:217' else */
+      /* 'AttitudeEKF:218' if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */
+      if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) {
+        /*              R=[r_gyro,0,0,0,0,0; */
+        /*                  0,r_gyro,0,0,0,0; */
+        /*                  0,0,r_gyro,0,0,0; */
+        /*                  0,0,0,r_accel,0,0; */
+        /*                  0,0,0,0,r_accel,0; */
+        /*                  0,0,0,0,0,r_accel]; */
+        /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */
+        /* observation matrix */
+        /* 'AttitudeEKF:230' H_k=[  E,     Z,      Z,    Z; */
+        /* 'AttitudeEKF:231'                 Z,     Z,      E,    Z]; */
+        /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */
+        /*  S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
+        /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
+        for (r2 = 0; r2 < 6; r2++) {
+          for (i = 0; i < 12; i++) {
+            d_a[r2 + 6 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
+            }
+          }
+
+          for (i = 0; i < 6; i++) {
+            b_S_k[r2 + 6 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i];
+            }
+          }
+        }
+
+        /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
+        d_r_gyro[0] = r_gyro;
+        d_r_gyro[1] = r_gyro;
+        d_r_gyro[2] = r_gyro;
+        d_r_gyro[3] = r_accel;
+        d_r_gyro[4] = r_accel;
+        d_r_gyro[5] = r_accel;
+        for (r2 = 0; r2 < 6; r2++) {
+          c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
+        }
+
+        for (r2 = 0; r2 < 6; r2++) {
+          b_S_k[7 * r2] = c_S_k[r2];
+        }
+
+        /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 6; i++) {
+            d_a[r2 + 12 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i];
+            }
+          }
+        }
+
+        b_mrdivide(d_a, b_S_k, b_K_k);
+
+        /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */
+        for (r2 = 0; r2 < 6; r2++) {
+          maxval = 0.0F;
+          for (i = 0; i < 12; i++) {
+            maxval += (float)e_a[r2 + 6 * i] * x_apr[i];
+          }
+
+          d_r_gyro[r2] = z[r2] - maxval;
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          maxval = 0.0F;
+          for (i = 0; i < 6; i++) {
+            maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i];
+          }
+
+          x_apo[r2] = x_apr[r2] + maxval;
+        }
+
+        /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
+        memset(&I[0], 0, 144U * sizeof(signed char));
+        for (i = 0; i < 12; i++) {
+          I[i + 12 * i] = 1;
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 12; i++) {
+            maxval = 0.0F;
+            for (r1 = 0; r1 < 6; r1++) {
+              maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i];
+            }
+
+            A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+          }
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 12; i++) {
+            P_apo[r2 + 12 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+            }
+          }
+        }
+      } else {
+        /* 'AttitudeEKF:243' else */
+        /* 'AttitudeEKF:244' if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */
+        if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) {
+          /*                  R=[r_gyro,0,0,0,0,0; */
+          /*                      0,r_gyro,0,0,0,0; */
+          /*                      0,0,r_gyro,0,0,0; */
+          /*                      0,0,0,r_mag,0,0; */
+          /*                      0,0,0,0,r_mag,0; */
+          /*                      0,0,0,0,0,r_mag]; */
+          /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */
+          /* observation matrix */
+          /* 'AttitudeEKF:254' H_k=[  E,     Z,      Z,    Z; */
+          /* 'AttitudeEKF:255'                     Z,     Z,      Z,    E]; */
+          /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */
+          /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
+          /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
+          for (r2 = 0; r2 < 6; r2++) {
+            for (i = 0; i < 12; i++) {
+              d_a[r2 + 6 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
+              }
+            }
+
+            for (i = 0; i < 6; i++) {
+              b_S_k[r2 + 6 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i];
+              }
+            }
+          }
+
+          /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
+          d_r_gyro[0] = r_gyro;
+          d_r_gyro[1] = r_gyro;
+          d_r_gyro[2] = r_gyro;
+          d_r_gyro[3] = r_mag;
+          d_r_gyro[4] = r_mag;
+          d_r_gyro[5] = r_mag;
+          for (r2 = 0; r2 < 6; r2++) {
+            c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
+          }
+
+          for (r2 = 0; r2 < 6; r2++) {
+            b_S_k[7 * r2] = c_S_k[r2];
+          }
+
+          /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 6; i++) {
+              d_a[r2 + 12 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i];
+              }
+            }
+          }
+
+          b_mrdivide(d_a, b_S_k, b_K_k);
+
+          /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */
+          for (r2 = 0; r2 < 3; r2++) {
+            d_r_gyro[r2] = z[r2];
+          }
+
+          for (r2 = 0; r2 < 3; r2++) {
+            d_r_gyro[r2 + 3] = z[6 + r2];
+          }
+
+          for (r2 = 0; r2 < 6; r2++) {
+            c_S_k[r2] = 0.0F;
+            for (i = 0; i < 12; i++) {
+              c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i];
+            }
+
+            b_z[r2] = d_r_gyro[r2] - c_S_k[r2];
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            maxval = 0.0F;
+            for (i = 0; i < 6; i++) {
+              maxval += b_K_k[r2 + 12 * i] * b_z[i];
+            }
+
+            x_apo[r2] = x_apr[r2] + maxval;
+          }
+
+          /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
+          memset(&I[0], 0, 144U * sizeof(signed char));
+          for (i = 0; i < 12; i++) {
+            I[i + 12 * i] = 1;
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 12; i++) {
+              maxval = 0.0F;
+              for (r1 = 0; r1 < 6; r1++) {
+                maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i];
+              }
+
+              A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+            }
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 12; i++) {
+              P_apo[r2 + 12 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+              }
+            }
+          }
+        } else {
+          /* 'AttitudeEKF:267' else */
+          /* 'AttitudeEKF:268' x_apo=x_apr; */
+          for (i = 0; i < 12; i++) {
+            x_apo[i] = x_apr[i];
+          }
+
+          /* 'AttitudeEKF:269' P_apo=P_apr; */
+          memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float));
+        }
+      }
+    }
+  }
+
+  /* % euler anglels extraction */
+  /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */
+  maxval = norm(*(float (*)[3])&x_apo[6]);
+  a21 = norm(*(float (*)[3])&x_apo[9]);
+  for (i = 0; i < 3; i++) {
+    /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */
+    muk[i] = -x_apo[i + 6] / maxval;
+    zek[i] = x_apo[i + 9] / a21;
+  }
+
+  /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */
+  wak[0] = muk[1] * zek[2] - muk[2] * zek[1];
+  wak[1] = muk[2] * zek[0] - muk[0] * zek[2];
+  wak[2] = muk[0] * zek[1] - muk[1] * zek[0];
+
+  /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */
+  maxval = norm(wak);
+  for (r2 = 0; r2 < 3; r2++) {
+    wak[r2] /= maxval;
+  }
+
+  /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */
+  zek[0] = wak[1] * muk[2] - wak[2] * muk[1];
+  zek[1] = wak[2] * muk[0] - wak[0] * muk[2];
+  zek[2] = wak[0] * muk[1] - wak[1] * muk[0];
+
+  /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */
+  maxval = norm(zek);
+  for (r2 = 0; r2 < 3; r2++) {
+    zek[r2] /= maxval;
+  }
+
+  /* 'AttitudeEKF:288' xa_apo=x_apo; */
+  for (i = 0; i < 12; i++) {
+    xa_apo[i] = x_apo[i];
+  }
+
+  /* 'AttitudeEKF:289' Pa_apo=P_apo; */
+  memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float));
+
+  /*  rotation matrix from earth to body system */
+  /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+  for (r2 = 0; r2 < 3; r2++) {
+    Rot_matrix[r2] = zek[r2];
+    Rot_matrix[3 + r2] = wak[r2];
+    Rot_matrix[6 + r2] = muk[r2];
+  }
+
+  /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+  /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */
+  /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+  /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */
+  eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]);
+  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+  eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]);
+}
+
+void AttitudeEKF_initialize(void)
+{
+  Q_not_empty = FALSE;
+  Ji_not_empty = FALSE;
+  AttitudeEKF_init();
+}
+
+void AttitudeEKF_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (AttitudeEKF.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
new file mode 100644
index 000000000..7094da1da
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
@@ -0,0 +1,26 @@
+/*
+ * AttitudeEKF.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_H__
+#define __ATTITUDEEKF_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "AttitudeEKF_types.h"
+
+/* Function Declarations */
+extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
+extern void AttitudeEKF_initialize(void);
+extern void AttitudeEKF_terminate(void);
+#endif
+/* End of code generation (AttitudeEKF.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
new file mode 100644
index 000000000..3f7522ffa
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
@@ -0,0 +1,17 @@
+/*
+ * AttitudeEKF_types.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_TYPES_H__
+#define __ATTITUDEEKF_TYPES_H__
+
+/* Include files */
+#include "rtwtypes.h"
+
+#endif
+/* End of code generation (AttitudeEKF_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
deleted file mode 100755
index 9e97ad11a..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/*

- * attitudeKalmanfilter.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-#include "norm.h"

-#include "cross.h"

-#include "eye.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  int32_T b_u0;

-  int32_T b_u1;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {

-    if (u0 > 0.0F) {

-      b_u0 = 1;

-    } else {

-      b_u0 = -1;

-    }

-

-    if (u1 > 0.0F) {

-      b_u1 = 1;

-    } else {

-      b_u1 = -1;

-    }

-

-    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

-  } else if (u1 == 0.0F) {

-    if (u0 > 0.0F) {

-      y = RT_PIF / 2.0F;

-    } else if (u0 < 0.0F) {

-      y = -(RT_PIF / 2.0F);

-    } else {

-      y = 0.0F;

-    }

-  } else {

-    y = (real32_T)atan2(u0, u1);

-  }

-

-  return y;

-}

-

-/*

- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)

- */

-void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

-  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

-  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T

-  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

-  P_aposteriori[144])

-{

-  real32_T wak[3];

-  real32_T O[9];

-  real_T dv0[9];

-  real32_T a[9];

-  int32_T i;

-  real32_T b_a[9];

-  real32_T x_n_b[3];

-  real32_T b_x_aposteriori_k[3];

-  real32_T z_n_b[3];

-  real32_T c_a[3];

-  real32_T d_a[3];

-  int32_T i0;

-  real32_T x_apriori[12];

-  real_T dv1[144];

-  real32_T A_lin[144];

-  static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T b_A_lin[144];

-  real32_T b_q[144];

-  real32_T c_A_lin[144];

-  real32_T d_A_lin[144];

-  real32_T e_A_lin[144];

-  int32_T i1;

-  real32_T P_apriori[144];

-  real32_T b_P_apriori[108];

-  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T K_k[108];

-  real32_T fv0[81];

-  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T b_r[81];

-  real32_T fv1[81];

-  real32_T f0;

-  real32_T c_P_apriori[36];

-  static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T fv2[36];

-  static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T c_r[9];

-  real32_T b_K_k[36];

-  real32_T d_P_apriori[72];

-  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0 };

-

-  real32_T c_K_k[72];

-  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0 };

-

-  real32_T b_z[6];

-  static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  real32_T fv3[6];

-  real32_T c_z[6];

-

-  /*  Extended Attitude Kalmanfilter */

-  /*  */

-  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

-  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

-  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

-  /*  */

-  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

-  /*  */

-  /*  Example.... */

-  /*  */

-  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */

-  /* coder.varsize('udpIndVect', [9,1], [1,0]) */

-  /* udpIndVect=find(updVect); */

-  /* process and measurement noise covariance matrix */

-  /* Q = diag(q.^2*dt); */

-  /* observation matrix */

-  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */

-  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */

-  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */

-  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */

-  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */

-  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */

-  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */

-  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */

-  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */

-  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */

-  /* % prediction section */

-  /* body angular accelerations */

-  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */

-  wak[0] = x_aposteriori_k[3];

-  wak[1] = x_aposteriori_k[4];

-  wak[2] = x_aposteriori_k[5];

-

-  /* body angular rates */

-  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */

-  /* derivative of the prediction rotation matrix */

-  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

-  O[0] = 0.0F;

-  O[1] = -x_aposteriori_k[2];

-  O[2] = x_aposteriori_k[1];

-  O[3] = x_aposteriori_k[2];

-  O[4] = 0.0F;

-  O[5] = -x_aposteriori_k[0];

-  O[6] = -x_aposteriori_k[1];

-  O[7] = x_aposteriori_k[0];

-  O[8] = 0.0F;

-

-  /* prediction of the earth z vector */

-  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* prediction of the magnetic vector */

-  /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    b_a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */

-  /* 'attitudeKalmanfilter:66'     -zez,0,zex; */

-  /* 'attitudeKalmanfilter:67'     zey,-zex,0]'; */

-  /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */

-  /* 'attitudeKalmanfilter:69'     -muz,0,mux; */

-  /* 'attitudeKalmanfilter:70'     zey,-mux,0]'; */

-  /* 'attitudeKalmanfilter:74' E=eye(3); */

-  /* 'attitudeKalmanfilter:76' Z=zeros(3); */

-  /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */

-  x_n_b[0] = x_aposteriori_k[0];

-  x_n_b[1] = x_aposteriori_k[1];

-  x_n_b[2] = x_aposteriori_k[2];

-  b_x_aposteriori_k[0] = x_aposteriori_k[6];

-  b_x_aposteriori_k[1] = x_aposteriori_k[7];

-  b_x_aposteriori_k[2] = x_aposteriori_k[8];

-  z_n_b[0] = x_aposteriori_k[9];

-  z_n_b[1] = x_aposteriori_k[10];

-  z_n_b[2] = x_aposteriori_k[11];

-  for (i = 0; i < 3; i++) {

-    c_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];

-    }

-

-    d_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];

-    }

-

-    x_apriori[i] = x_n_b[i] + dt * wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 3] = wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 6] = c_a[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 9] = d_a[i];

-  }

-

-  /* 'attitudeKalmanfilter:81' A_lin=[ Z,  E,  Z,  Z */

-  /* 'attitudeKalmanfilter:82'     Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:83'     EZ, Z,  O,  Z */

-  /* 'attitudeKalmanfilter:84'     MA, Z,  Z,  O]; */

-  /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */

-  b_eye(dv1);

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];

-    }

-

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * i) + 3] = 0.0F;

-    }

-  }

-

-  A_lin[6] = 0.0F;

-  A_lin[7] = x_aposteriori_k[8];

-  A_lin[8] = -x_aposteriori_k[7];

-  A_lin[18] = -x_aposteriori_k[8];

-  A_lin[19] = 0.0F;

-  A_lin[20] = x_aposteriori_k[6];

-  A_lin[30] = x_aposteriori_k[7];

-  A_lin[31] = -x_aposteriori_k[6];

-  A_lin[32] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;

-    }

-  }

-

-  A_lin[9] = 0.0F;

-  A_lin[10] = x_aposteriori_k[11];

-  A_lin[11] = -x_aposteriori_k[10];

-  A_lin[21] = -x_aposteriori_k[11];

-  A_lin[22] = 0.0F;

-  A_lin[23] = x_aposteriori_k[9];

-  A_lin[33] = x_aposteriori_k[7];

-  A_lin[34] = -x_aposteriori_k[9];

-  A_lin[35] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *

-        dt;

-    }

-  }

-

-  /* 'attitudeKalmanfilter:88' Qtemp=[ q(1),     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:89'         0,     q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:90'         0,     0,      q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:91'         0,     0,      0,      q(2),   0,      0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:92'         0,     0,      0,      0,      q(2),   0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:93'         0,     0,      0,      0,      0,      q(2),   0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:94'         0,     0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:95'         0,     0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:96'         0,     0,      0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0; */

-  /* 'attitudeKalmanfilter:97'         0,     0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0,      0; */

-  /* 'attitudeKalmanfilter:98'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0; */

-  /* 'attitudeKalmanfilter:99'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4)]; */

-  /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */

-  /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

-  b_q[0] = q[0];

-  b_q[12] = 0.0F;

-  b_q[24] = 0.0F;

-  b_q[36] = 0.0F;

-  b_q[48] = 0.0F;

-  b_q[60] = 0.0F;

-  b_q[72] = 0.0F;

-  b_q[84] = 0.0F;

-  b_q[96] = 0.0F;

-  b_q[108] = 0.0F;

-  b_q[120] = 0.0F;

-  b_q[132] = 0.0F;

-  b_q[1] = 0.0F;

-  b_q[13] = q[0];

-  b_q[25] = 0.0F;

-  b_q[37] = 0.0F;

-  b_q[49] = 0.0F;

-  b_q[61] = 0.0F;

-  b_q[73] = 0.0F;

-  b_q[85] = 0.0F;

-  b_q[97] = 0.0F;

-  b_q[109] = 0.0F;

-  b_q[121] = 0.0F;

-  b_q[133] = 0.0F;

-  b_q[2] = 0.0F;

-  b_q[14] = 0.0F;

-  b_q[26] = q[0];

-  b_q[38] = 0.0F;

-  b_q[50] = 0.0F;

-  b_q[62] = 0.0F;

-  b_q[74] = 0.0F;

-  b_q[86] = 0.0F;

-  b_q[98] = 0.0F;

-  b_q[110] = 0.0F;

-  b_q[122] = 0.0F;

-  b_q[134] = 0.0F;

-  b_q[3] = 0.0F;

-  b_q[15] = 0.0F;

-  b_q[27] = 0.0F;

-  b_q[39] = q[1];

-  b_q[51] = 0.0F;

-  b_q[63] = 0.0F;

-  b_q[75] = 0.0F;

-  b_q[87] = 0.0F;

-  b_q[99] = 0.0F;

-  b_q[111] = 0.0F;

-  b_q[123] = 0.0F;

-  b_q[135] = 0.0F;

-  b_q[4] = 0.0F;

-  b_q[16] = 0.0F;

-  b_q[28] = 0.0F;

-  b_q[40] = 0.0F;

-  b_q[52] = q[1];

-  b_q[64] = 0.0F;

-  b_q[76] = 0.0F;

-  b_q[88] = 0.0F;

-  b_q[100] = 0.0F;

-  b_q[112] = 0.0F;

-  b_q[124] = 0.0F;

-  b_q[136] = 0.0F;

-  b_q[5] = 0.0F;

-  b_q[17] = 0.0F;

-  b_q[29] = 0.0F;

-  b_q[41] = 0.0F;

-  b_q[53] = 0.0F;

-  b_q[65] = q[1];

-  b_q[77] = 0.0F;

-  b_q[89] = 0.0F;

-  b_q[101] = 0.0F;

-  b_q[113] = 0.0F;

-  b_q[125] = 0.0F;

-  b_q[137] = 0.0F;

-  b_q[6] = 0.0F;

-  b_q[18] = 0.0F;

-  b_q[30] = 0.0F;

-  b_q[42] = 0.0F;

-  b_q[54] = 0.0F;

-  b_q[66] = 0.0F;

-  b_q[78] = q[2];

-  b_q[90] = 0.0F;

-  b_q[102] = 0.0F;

-  b_q[114] = 0.0F;

-  b_q[126] = 0.0F;

-  b_q[138] = 0.0F;

-  b_q[7] = 0.0F;

-  b_q[19] = 0.0F;

-  b_q[31] = 0.0F;

-  b_q[43] = 0.0F;

-  b_q[55] = 0.0F;

-  b_q[67] = 0.0F;

-  b_q[79] = 0.0F;

-  b_q[91] = q[2];

-  b_q[103] = 0.0F;

-  b_q[115] = 0.0F;

-  b_q[127] = 0.0F;

-  b_q[139] = 0.0F;

-  b_q[8] = 0.0F;

-  b_q[20] = 0.0F;

-  b_q[32] = 0.0F;

-  b_q[44] = 0.0F;

-  b_q[56] = 0.0F;

-  b_q[68] = 0.0F;

-  b_q[80] = 0.0F;

-  b_q[92] = 0.0F;

-  b_q[104] = q[2];

-  b_q[116] = 0.0F;

-  b_q[128] = 0.0F;

-  b_q[140] = 0.0F;

-  b_q[9] = 0.0F;

-  b_q[21] = 0.0F;

-  b_q[33] = 0.0F;

-  b_q[45] = 0.0F;

-  b_q[57] = 0.0F;

-  b_q[69] = 0.0F;

-  b_q[81] = 0.0F;

-  b_q[93] = 0.0F;

-  b_q[105] = 0.0F;

-  b_q[117] = q[3];

-  b_q[129] = 0.0F;

-  b_q[141] = 0.0F;

-  b_q[10] = 0.0F;

-  b_q[22] = 0.0F;

-  b_q[34] = 0.0F;

-  b_q[46] = 0.0F;

-  b_q[58] = 0.0F;

-  b_q[70] = 0.0F;

-  b_q[82] = 0.0F;

-  b_q[94] = 0.0F;

-  b_q[106] = 0.0F;

-  b_q[118] = 0.0F;

-  b_q[130] = q[3];

-  b_q[142] = 0.0F;

-  b_q[11] = 0.0F;

-  b_q[23] = 0.0F;

-  b_q[35] = 0.0F;

-  b_q[47] = 0.0F;

-  b_q[59] = 0.0F;

-  b_q[71] = 0.0F;

-  b_q[83] = 0.0F;

-  b_q[95] = 0.0F;

-  b_q[107] = 0.0F;

-  b_q[119] = 0.0F;

-  b_q[131] = 0.0F;

-  b_q[143] = q[3];

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

-          i0];

-      }

-

-      c_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];

-      }

-    }

-

-    for (i0 = 0; i0 < 12; i0++) {

-      d_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-

-      e_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];

-    }

-  }

-

-  /* % update */

-  /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

-  if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

-    /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */

-    if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-      /* 'attitudeKalmanfilter:112' r(2)=10000; */

-      r[1] = 10000.0F;

-    }

-

-    /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:115'         0,r(1),0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:116'         0,0,r(1),0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:117'         0,0,0,r(2),0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:118'         0,0,0,0,r(2),0,0,0,0; */

-    /* 'attitudeKalmanfilter:119'         0,0,0,0,0,r(2),0,0,0; */

-    /* 'attitudeKalmanfilter:120'         0,0,0,0,0,0,r(3),0,0; */

-    /* 'attitudeKalmanfilter:121'         0,0,0,0,0,0,0,r(3),0; */

-    /* 'attitudeKalmanfilter:122'         0,0,0,0,0,0,0,0,r(3)]; */

-    /* observation matrix */

-    /* [zw;ze;zmk]; */

-    /* 'attitudeKalmanfilter:125' H_k=[  E,     Z,      Z,    Z; */

-    /* 'attitudeKalmanfilter:126'         Z,     Z,      E,    Z; */

-    /* 'attitudeKalmanfilter:127'         Z,     Z,      Z,    E]; */

-    /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */

-    /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */

-    /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        b_P_apriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1

-            + 12 * i0];

-        }

-      }

-    }

-

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        K_k[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];

-        }

-      }

-

-      for (i0 = 0; i0 < 9; i0++) {

-        fv0[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];

-        }

-      }

-    }

-

-    b_r[0] = r[0];

-    b_r[9] = 0.0F;

-    b_r[18] = 0.0F;

-    b_r[27] = 0.0F;

-    b_r[36] = 0.0F;

-    b_r[45] = 0.0F;

-    b_r[54] = 0.0F;

-    b_r[63] = 0.0F;

-    b_r[72] = 0.0F;

-    b_r[1] = 0.0F;

-    b_r[10] = r[0];

-    b_r[19] = 0.0F;

-    b_r[28] = 0.0F;

-    b_r[37] = 0.0F;

-    b_r[46] = 0.0F;

-    b_r[55] = 0.0F;

-    b_r[64] = 0.0F;

-    b_r[73] = 0.0F;

-    b_r[2] = 0.0F;

-    b_r[11] = 0.0F;

-    b_r[20] = r[0];

-    b_r[29] = 0.0F;

-    b_r[38] = 0.0F;

-    b_r[47] = 0.0F;

-    b_r[56] = 0.0F;

-    b_r[65] = 0.0F;

-    b_r[74] = 0.0F;

-    b_r[3] = 0.0F;

-    b_r[12] = 0.0F;

-    b_r[21] = 0.0F;

-    b_r[30] = r[1];

-    b_r[39] = 0.0F;

-    b_r[48] = 0.0F;

-    b_r[57] = 0.0F;

-    b_r[66] = 0.0F;

-    b_r[75] = 0.0F;

-    b_r[4] = 0.0F;

-    b_r[13] = 0.0F;

-    b_r[22] = 0.0F;

-    b_r[31] = 0.0F;

-    b_r[40] = r[1];

-    b_r[49] = 0.0F;

-    b_r[58] = 0.0F;

-    b_r[67] = 0.0F;

-    b_r[76] = 0.0F;

-    b_r[5] = 0.0F;

-    b_r[14] = 0.0F;

-    b_r[23] = 0.0F;

-    b_r[32] = 0.0F;

-    b_r[41] = 0.0F;

-    b_r[50] = r[1];

-    b_r[59] = 0.0F;

-    b_r[68] = 0.0F;

-    b_r[77] = 0.0F;

-    b_r[6] = 0.0F;

-    b_r[15] = 0.0F;

-    b_r[24] = 0.0F;

-    b_r[33] = 0.0F;

-    b_r[42] = 0.0F;

-    b_r[51] = 0.0F;

-    b_r[60] = r[2];

-    b_r[69] = 0.0F;

-    b_r[78] = 0.0F;

-    b_r[7] = 0.0F;

-    b_r[16] = 0.0F;

-    b_r[25] = 0.0F;

-    b_r[34] = 0.0F;

-    b_r[43] = 0.0F;

-    b_r[52] = 0.0F;

-    b_r[61] = 0.0F;

-    b_r[70] = r[2];

-    b_r[79] = 0.0F;

-    b_r[8] = 0.0F;

-    b_r[17] = 0.0F;

-    b_r[26] = 0.0F;

-    b_r[35] = 0.0F;

-    b_r[44] = 0.0F;

-    b_r[53] = 0.0F;

-    b_r[62] = 0.0F;

-    b_r[71] = 0.0F;

-    b_r[80] = r[2];

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];

-      }

-    }

-

-    mrdivide(b_P_apriori, fv1, K_k);

-

-    /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */

-    for (i = 0; i < 9; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];

-      }

-

-      O[i] = z[i] - f0;

-    }

-

-    for (i = 0; i < 12; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 9; i0++) {

-        f0 += K_k[i + 12 * i0] * O[i0];

-      }

-

-      x_aposteriori[i] = x_apriori[i] + f0;

-    }

-

-    /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

-    b_eye(dv1);

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];

-        }

-

-        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-      }

-    }

-

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        P_aposteriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12

-            * i0];

-        }

-      }

-    }

-  } else {

-    /* 'attitudeKalmanfilter:138' else */

-    /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

-    if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

-      /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */

-      /* 'attitudeKalmanfilter:142'             0,r(1),0; */

-      /* 'attitudeKalmanfilter:143'             0,0,r(1)]; */

-      /* observation matrix */

-      /* 'attitudeKalmanfilter:146' H_k=[  E,     Z,      Z,    Z]; */

-      /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

-      /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

-      /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          c_P_apriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-              iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          fv2[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *

-              i0];

-          }

-        }

-

-        for (i0 = 0; i0 < 3; i0++) {

-          O[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      c_r[0] = r[0];

-      c_r[3] = 0.0F;

-      c_r[6] = 0.0F;

-      c_r[1] = 0.0F;

-      c_r[4] = r[0];

-      c_r[7] = 0.0F;

-      c_r[2] = 0.0F;

-      c_r[5] = 0.0F;

-      c_r[8] = r[0];

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];

-        }

-      }

-

-      b_mrdivide(c_P_apriori, a, b_K_k);

-

-      /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */

-      for (i = 0; i < 3; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];

-        }

-

-        x_n_b[i] = z[i] - f0;

-      }

-

-      for (i = 0; i < 12; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 3; i0++) {

-          f0 += b_K_k[i + 12 * i0] * x_n_b[i0];

-        }

-

-        x_aposteriori[i] = x_apriori[i] + f0;

-      }

-

-      /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

-      b_eye(dv1);

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 = 0.0F;

-          for (i1 = 0; i1 < 3; i1++) {

-            f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];

-          }

-

-          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-        }

-      }

-

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          P_aposteriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +

-              12 * i0];

-          }

-        }

-      }

-    } else {

-      /* 'attitudeKalmanfilter:156' else */

-      /* 'attitudeKalmanfilter:157' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

-      if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

-      {

-        /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */

-        if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-          /* 'attitudeKalmanfilter:159' r(2)=10000; */

-          r[1] = 10000.0F;

-        }

-

-        /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */

-        /* 'attitudeKalmanfilter:163'                 0,r(1),0,0,0,0; */

-        /* 'attitudeKalmanfilter:164'                 0,0,r(1),0,0,0; */

-        /* 'attitudeKalmanfilter:165'                 0,0,0,r(2),0,0; */

-        /* 'attitudeKalmanfilter:166'                 0,0,0,0,r(2),0; */

-        /* 'attitudeKalmanfilter:167'                 0,0,0,0,0,r(2)]; */

-        /* observation matrix */

-        /* 'attitudeKalmanfilter:170' H_k=[  E,     Z,      Z,    Z; */

-        /* 'attitudeKalmanfilter:171'                 Z,     Z,      E,    Z]; */

-        /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

-        /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-        /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            d_P_apriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            c_K_k[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12

-                * i0];

-            }

-          }

-

-          for (i0 = 0; i0 < 6; i0++) {

-            fv2[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        b_K_k[0] = r[0];

-        b_K_k[6] = 0.0F;

-        b_K_k[12] = 0.0F;

-        b_K_k[18] = 0.0F;

-        b_K_k[24] = 0.0F;

-        b_K_k[30] = 0.0F;

-        b_K_k[1] = 0.0F;

-        b_K_k[7] = r[0];

-        b_K_k[13] = 0.0F;

-        b_K_k[19] = 0.0F;

-        b_K_k[25] = 0.0F;

-        b_K_k[31] = 0.0F;

-        b_K_k[2] = 0.0F;

-        b_K_k[8] = 0.0F;

-        b_K_k[14] = r[0];

-        b_K_k[20] = 0.0F;

-        b_K_k[26] = 0.0F;

-        b_K_k[32] = 0.0F;

-        b_K_k[3] = 0.0F;

-        b_K_k[9] = 0.0F;

-        b_K_k[15] = 0.0F;

-        b_K_k[21] = r[1];

-        b_K_k[27] = 0.0F;

-        b_K_k[33] = 0.0F;

-        b_K_k[4] = 0.0F;

-        b_K_k[10] = 0.0F;

-        b_K_k[16] = 0.0F;

-        b_K_k[22] = 0.0F;

-        b_K_k[28] = r[1];

-        b_K_k[34] = 0.0F;

-        b_K_k[5] = 0.0F;

-        b_K_k[11] = 0.0F;

-        b_K_k[17] = 0.0F;

-        b_K_k[23] = 0.0F;

-        b_K_k[29] = 0.0F;

-        b_K_k[35] = r[1];

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-          }

-        }

-

-        c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-        /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */

-        for (i = 0; i < 6; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

-          }

-

-          b_z[i] = z[i] - f0;

-        }

-

-        for (i = 0; i < 12; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 6; i0++) {

-            f0 += c_K_k[i + 12 * i0] * b_z[i0];

-          }

-

-          x_aposteriori[i] = x_apriori[i] + f0;

-        }

-

-        /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-        b_eye(dv1);

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 = 0.0F;

-            for (i1 = 0; i1 < 6; i1++) {

-              f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

-            }

-

-            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-          }

-        }

-

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            P_aposteriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1

-                + 12 * i0];

-            }

-          }

-        }

-      } else {

-        /* 'attitudeKalmanfilter:181' else */

-        /* 'attitudeKalmanfilter:182' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

-        if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

-        {

-          /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */

-          /* 'attitudeKalmanfilter:184'                     0,r(1),0,0,0,0; */

-          /* 'attitudeKalmanfilter:185'                     0,0,r(1),0,0,0; */

-          /* 'attitudeKalmanfilter:186'                     0,0,0,r(3),0,0; */

-          /* 'attitudeKalmanfilter:187'                     0,0,0,0,r(3),0; */

-          /* 'attitudeKalmanfilter:188'                     0,0,0,0,0,r(3)]; */

-          /* observation matrix */

-          /* 'attitudeKalmanfilter:191' H_k=[  E,     Z,      Z,    Z; */

-          /* 'attitudeKalmanfilter:192'                     Z,     Z,      Z,    E]; */

-          /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

-          /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-          /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              d_P_apriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                  iv7[i1 + 12 * i0];

-              }

-            }

-          }

-

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              c_K_k[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +

-                  12 * i0];

-              }

-            }

-

-            for (i0 = 0; i0 < 6; i0++) {

-              fv2[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *

-                  i0];

-              }

-            }

-          }

-

-          b_K_k[0] = r[0];

-          b_K_k[6] = 0.0F;

-          b_K_k[12] = 0.0F;

-          b_K_k[18] = 0.0F;

-          b_K_k[24] = 0.0F;

-          b_K_k[30] = 0.0F;

-          b_K_k[1] = 0.0F;

-          b_K_k[7] = r[0];

-          b_K_k[13] = 0.0F;

-          b_K_k[19] = 0.0F;

-          b_K_k[25] = 0.0F;

-          b_K_k[31] = 0.0F;

-          b_K_k[2] = 0.0F;

-          b_K_k[8] = 0.0F;

-          b_K_k[14] = r[0];

-          b_K_k[20] = 0.0F;

-          b_K_k[26] = 0.0F;

-          b_K_k[32] = 0.0F;

-          b_K_k[3] = 0.0F;

-          b_K_k[9] = 0.0F;

-          b_K_k[15] = 0.0F;

-          b_K_k[21] = r[2];

-          b_K_k[27] = 0.0F;

-          b_K_k[33] = 0.0F;

-          b_K_k[4] = 0.0F;

-          b_K_k[10] = 0.0F;

-          b_K_k[16] = 0.0F;

-          b_K_k[22] = 0.0F;

-          b_K_k[28] = r[2];

-          b_K_k[34] = 0.0F;

-          b_K_k[5] = 0.0F;

-          b_K_k[11] = 0.0F;

-          b_K_k[17] = 0.0F;

-          b_K_k[23] = 0.0F;

-          b_K_k[29] = 0.0F;

-          b_K_k[35] = r[2];

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-            }

-          }

-

-          c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-          /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */

-          for (i = 0; i < 3; i++) {

-            b_z[i] = z[i];

-          }

-

-          for (i = 0; i < 3; i++) {

-            b_z[i + 3] = z[i + 6];

-          }

-

-          for (i = 0; i < 6; i++) {

-            fv3[i] = 0.0F;

-            for (i0 = 0; i0 < 12; i0++) {

-              fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];

-            }

-

-            c_z[i] = b_z[i] - fv3[i];

-          }

-

-          for (i = 0; i < 12; i++) {

-            f0 = 0.0F;

-            for (i0 = 0; i0 < 6; i0++) {

-              f0 += c_K_k[i + 12 * i0] * c_z[i0];

-            }

-

-            x_aposteriori[i] = x_apriori[i] + f0;

-          }

-

-          /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-          b_eye(dv1);

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              f0 = 0.0F;

-              for (i1 = 0; i1 < 6; i1++) {

-                f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];

-              }

-

-              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-            }

-          }

-

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              P_aposteriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *

-                  P_apriori[i1 + 12 * i0];

-              }

-            }

-          }

-        } else {

-          /* 'attitudeKalmanfilter:202' else */

-          /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */

-          for (i = 0; i < 12; i++) {

-            x_aposteriori[i] = x_apriori[i];

-          }

-

-          /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */

-          memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));

-        }

-      }

-    }

-  }

-

-  /* % euler anglels extraction */

-  /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = -x_aposteriori[i + 6];

-  }

-

-  rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);

-

-  /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

-  rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&

-           x_aposteriori[9]), wak);

-

-  /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  cross(z_n_b, x_n_b, wak);

-

-  /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  rdivide(x_n_b, norm(wak), wak);

-

-  /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */

-  cross(wak, z_n_b, x_n_b);

-

-  /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */

-  for (i = 0; i < 3; i++) {

-    b_x_aposteriori_k[i] = x_n_b[i];

-  }

-

-  rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);

-

-  /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

-  for (i = 0; i < 3; i++) {

-    Rot_matrix[i] = x_n_b[i];

-    Rot_matrix[3 + i] = wak[i];

-    Rot_matrix[6 + i] = z_n_b[i];

-  }

-

-  /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */

-  eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

-  eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

-}

-

-/* End of code generation (attitudeKalmanfilter.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
deleted file mode 100755
index afa63c1a9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_H__

-#define __ATTITUDEKALMANFILTER_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

-#endif

-/* End of code generation (attitudeKalmanfilter.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
deleted file mode 100755
index 7d620d7fa..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.c

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_initialize.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_initialize(void)

-{

-  rt_InitInfAndNaN(8U);

-}

-

-/* End of code generation (attitudeKalmanfilter_initialize.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
deleted file mode 100755
index 8b599be66..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.h

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__

-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_initialize(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_initialize.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
deleted file mode 100755
index 7f9727419..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.c

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (attitudeKalmanfilter_terminate.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
deleted file mode 100755
index da84a5024..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.h

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__

-#define __ATTITUDEKALMANFILTER_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_terminate(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_terminate.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
deleted file mode 100755
index 30fd1e75e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * attitudeKalmanfilter_types.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__

-#define __ATTITUDEKALMANFILTER_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (attitudeKalmanfilter_types.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c
deleted file mode 100755
index 84ada9002..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/*

- * cross.c

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "cross.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])

-{

-  c[0] = a[1] * b[2] - a[2] * b[1];

-  c[1] = a[2] * b[0] - a[0] * b[2];

-  c[2] = a[0] * b[1] - a[1] * b[0];

-}

-

-/* End of code generation (cross.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h
deleted file mode 100755
index e727f0684..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * cross.h

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __CROSS_H__

-#define __CROSS_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);

-#endif

-/* End of code generation (cross.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c
deleted file mode 100755
index b89ab58ef..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*

- * eye.c

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "eye.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_eye(real_T I[144])

-{

-  int32_T i;

-  memset(&I[0], 0, 144U * sizeof(real_T));

-  for (i = 0; i < 12; i++) {

-    I[i + 12 * i] = 1.0;

-  }

-}

-

-/*

- *

- */

-void eye(real_T I[9])

-{

-  int32_T i;

-  memset(&I[0], 0, 9U * sizeof(real_T));

-  for (i = 0; i < 3; i++) {

-    I[i + 3 * i] = 1.0;

-  }

-}

-

-/* End of code generation (eye.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h
deleted file mode 100755
index 94fbe7671..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*

- * eye.h

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __EYE_H__

-#define __EYE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_eye(real_T I[144]);

-extern void eye(real_T I[9]);

-#endif

-/* End of code generation (eye.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
deleted file mode 100755
index a810f22e4..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
+++ /dev/null
@@ -1,357 +0,0 @@
-/*

- * mrdivide.c

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])

-{

-  real32_T b_A[9];

-  int32_T rtemp;

-  int32_T k;

-  real32_T b_B[36];

-  int32_T r1;

-  int32_T r2;

-  int32_T r3;

-  real32_T maxval;

-  real32_T a21;

-  real32_T Y[36];

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];

-    }

-  }

-

-  for (rtemp = 0; rtemp < 12; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];

-    }

-  }

-

-  r1 = 0;

-  r2 = 1;

-  r3 = 2;

-  maxval = (real32_T)fabs(b_A[0]);

-  a21 = (real32_T)fabs(b_A[1]);

-  if (a21 > maxval) {

-    maxval = a21;

-    r1 = 1;

-    r2 = 0;

-  }

-

-  if ((real32_T)fabs(b_A[2]) > maxval) {

-    r1 = 2;

-    r2 = 1;

-    r3 = 0;

-  }

-

-  b_A[r2] /= b_A[r1];

-  b_A[r3] /= b_A[r1];

-  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];

-  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];

-  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];

-  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];

-  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {

-    rtemp = r2;

-    r2 = r3;

-    r3 = rtemp;

-  }

-

-  b_A[3 + r3] /= b_A[3 + r2];

-  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];

-  for (k = 0; k < 12; k++) {

-    Y[3 * k] = b_B[r1 + 3 * k];

-    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];

-    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3

-      + r3];

-    Y[2 + 3 * k] /= b_A[6 + r3];

-    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];

-    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];

-    Y[1 + 3 * k] /= b_A[3 + r2];

-    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];

-    Y[3 * k] /= b_A[r1];

-  }

-

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 12; k++) {

-      y[k + 12 * rtemp] = Y[rtemp + 3 * k];

-    }

-  }

-}

-

-/*

- *

- */

-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])

-{

-  real32_T b_A[36];

-  int8_T ipiv[6];

-  int32_T i3;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[72];

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      b_A[iy + 6 * i3] = B[i3 + 6 * iy];

-    }

-

-    ipiv[i3] = (int8_T)(1 + i3);

-  }

-

-  for (j = 0; j < 5; j++) {

-    c = j * 7;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 6 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 6; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 6;

-          iy += 6;

-        }

-      }

-

-      i3 = (c - j) + 6;

-      for (jy = c + 1; jy + 1 <= i3; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 6;

-    for (k = 1; k <= 5 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i3 = (iy - j) + 12;

-        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 6;

-      iy += 6;

-    }

-  }

-

-  for (i3 = 0; i3 < 12; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      Y[iy + 6 * i3] = A[i3 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 6; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 6 * j];

-        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];

-        Y[(ipiv[jy] + 6 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 0; k < 6; k++) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 7; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 5; k > -1; k += -1) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i3] = Y[i3 + 6 * iy];

-    }

-  }

-}

-

-/*

- *

- */

-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])

-{

-  real32_T b_A[81];

-  int8_T ipiv[9];

-  int32_T i2;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[108];

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      b_A[iy + 9 * i2] = B[i2 + 9 * iy];

-    }

-

-    ipiv[i2] = (int8_T)(1 + i2);

-  }

-

-  for (j = 0; j < 8; j++) {

-    c = j * 10;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 9 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 9; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 9;

-          iy += 9;

-        }

-      }

-

-      i2 = (c - j) + 9;

-      for (jy = c + 1; jy + 1 <= i2; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 9;

-    for (k = 1; k <= 8 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i2 = (iy - j) + 18;

-        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 9;

-      iy += 9;

-    }

-  }

-

-  for (i2 = 0; i2 < 12; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      Y[iy + 9 * i2] = A[i2 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 9; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 9 * j];

-        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];

-        Y[(ipiv[jy] + 9 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 0; k < 9; k++) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 10; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 8; k > -1; k += -1) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i2] = Y[i2 + 9 * iy];

-    }

-  }

-}

-

-/* End of code generation (mrdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
deleted file mode 100755
index 2d3b0d51f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*

- * mrdivide.h

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __MRDIVIDE_H__

-#define __MRDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);

-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);

-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);

-#endif

-/* End of code generation (mrdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c
deleted file mode 100755
index 0c418cc7b..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.c
+++ /dev/null
@@ -1,54 +0,0 @@
-/*

- * norm.c

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "norm.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-real32_T norm(const real32_T x[3])

-{

-  real32_T y;

-  real32_T scale;

-  int32_T k;

-  real32_T absxk;

-  real32_T t;

-  y = 0.0F;

-  scale = 1.17549435E-38F;

-  for (k = 0; k < 3; k++) {

-    absxk = (real32_T)fabs(x[k]);

-    if (absxk > scale) {

-      t = scale / absxk;

-      y = 1.0F + y * t * t;

-      scale = absxk;

-    } else {

-      t = absxk / scale;

-      y += t * t;

-    }

-  }

-

-  return scale * (real32_T)sqrt(y);

-}

-

-/* End of code generation (norm.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h
deleted file mode 100755
index 60cf77b57..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * norm.h

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __NORM_H__

-#define __NORM_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real32_T norm(const real32_T x[3]);

-#endif

-/* End of code generation (norm.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
deleted file mode 100755
index d035dae5e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * rdivide.c

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])

-{

-  int32_T i;

-  for (i = 0; i < 3; i++) {

-    z[i] = x[i] / y;

-  }

-}

-

-/* End of code generation (rdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
deleted file mode 100755
index 4bbebebe2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * rdivide.h

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RDIVIDE_H__

-#define __RDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);

-#endif

-/* End of code generation (rdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
deleted file mode 100755
index 34164d104..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*

- * rtGetInf.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf

- */

-#include "rtGetInf.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetInf ==================================================

- * Abstract:

- * Initialize rtInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T inf = 0.0;

-  if (bitsPerReal == 32U) {

-    inf = rtGetInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return inf;

-}

-

-/* Function: rtGetInfF ==================================================

- * Abstract:

- * Initialize rtInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetInfF(void)

-{

-  IEEESingle infF;

-  infF.wordL.wordLuint = 0x7F800000U;

-  return infF.wordL.wordLreal;

-}

-

-/* Function: rtGetMinusInf ==================================================

- * Abstract:

- * Initialize rtMinusInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetMinusInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T minf = 0.0;

-  if (bitsPerReal == 32U) {

-    minf = rtGetMinusInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return minf;

-}

-

-/* Function: rtGetMinusInfF ==================================================

- * Abstract:

- * Initialize rtMinusInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetMinusInfF(void)

-{

-  IEEESingle minfF;

-  minfF.wordL.wordLuint = 0xFF800000U;

-  return minfF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetInf.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
deleted file mode 100755
index 145373cd0..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*

- * rtGetInf.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETINF_H__

-#define __RTGETINF_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetInf(void);

-extern real32_T rtGetInfF(void);

-extern real_T rtGetMinusInf(void);

-extern real32_T rtGetMinusInfF(void);

-

-#endif

-/* End of code generation (rtGetInf.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
deleted file mode 100755
index d84ca9573..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*

- * rtGetNaN.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, NaN

- */

-#include "rtGetNaN.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetNaN ==================================================

- * Abstract:

- * Initialize rtNaN needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetNaN(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T nan = 0.0;

-  if (bitsPerReal == 32U) {

-    nan = rtGetNaNF();

-  } else {

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      union {

-        LittleEndianIEEEDouble bitVal;

-        real_T fltVal;

-      } tmpVal;

-

-      tmpVal.bitVal.words.wordH = 0xFFF80000U;

-      tmpVal.bitVal.words.wordL = 0x00000000U;

-      nan = tmpVal.fltVal;

-      break;

-    }

-

-   case BigEndian:

-    {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;

-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;

-        nan = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return nan;

-}

-

-/* Function: rtGetNaNF ==================================================

- * Abstract:

- * Initialize rtNaNF needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetNaNF(void)

-{

-  IEEESingle nanF = { { 0 } };

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      nanF.wordL.wordLuint = 0xFFC00000U;

-      break;

-    }

-

-   case BigEndian:

-    {

-      nanF.wordL.wordLuint = 0x7FFFFFFFU;

-      break;

-    }

-  }

-

-  return nanF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetNaN.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
deleted file mode 100755
index 65fdaa96f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*

- * rtGetNaN.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETNAN_H__

-#define __RTGETNAN_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetNaN(void);

-extern real32_T rtGetNaNF(void);

-

-#endif

-/* End of code generation (rtGetNaN.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
deleted file mode 100755
index 356498363..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*

- * rt_defines.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_DEFINES_H__

-#define __RT_DEFINES_H__

-

-#include <stdlib.h>

-

-#define RT_PI	3.14159265358979323846

-#define RT_PIF	3.1415927F

-#define RT_LN_10	2.30258509299404568402

-#define RT_LN_10F	2.3025851F

-#define RT_LOG10E	0.43429448190325182765

-#define RT_LOG10EF	0.43429449F

-#define RT_E	2.7182818284590452354

-#define RT_EF	2.7182817F

-#endif

-/* End of code generation (rt_defines.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
deleted file mode 100755
index 303d1d9d2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*

- * rt_nonfinite.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *      MATLAB for code generation function to initialize non-finites,

- *      (Inf, NaN and -Inf).

- */

-#include "rt_nonfinite.h"

-#include "rtGetNaN.h"

-#include "rtGetInf.h"

-

-real_T rtInf;

-real_T rtMinusInf;

-real_T rtNaN;

-real32_T rtInfF;

-real32_T rtMinusInfF;

-real32_T rtNaNF;

-

-/* Function: rt_InitInfAndNaN ==================================================

- * Abstract:

- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the

- * generated code. NaN is initialized as non-signaling. Assumes IEEE.

- */

-void rt_InitInfAndNaN(size_t realSize)

-{

-  (void) (realSize);

-  rtNaN = rtGetNaN();

-  rtNaNF = rtGetNaNF();

-  rtInf = rtGetInf();

-  rtInfF = rtGetInfF();

-  rtMinusInf = rtGetMinusInf();

-  rtMinusInfF = rtGetMinusInfF();

-}

-

-/* Function: rtIsInf ==================================================

- * Abstract:

- * Test if value is infinite

- */

-boolean_T rtIsInf(real_T value)

-{

-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);

-}

-

-/* Function: rtIsInfF =================================================

- * Abstract:

- * Test if single-precision value is infinite

- */

-boolean_T rtIsInfF(real32_T value)

-{

-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);

-}

-

-/* Function: rtIsNaN ==================================================

- * Abstract:

- * Test if value is not a number

- */

-boolean_T rtIsNaN(real_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan(value)? TRUE:FALSE;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-/* Function: rtIsNaNF =================================================

- * Abstract:

- * Test if single-precision value is not a number

- */

-boolean_T rtIsNaNF(real32_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan((real_T)value)? true:false;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-

-/* End of code generation (rt_nonfinite.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
deleted file mode 100755
index bd56b30d9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*

- * rt_nonfinite.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_NONFINITE_H__

-#define __RT_NONFINITE_H__

-

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-#include <float.h>

-#endif

-#include <stddef.h>

-#include "rtwtypes.h"

-

-extern real_T rtInf;

-extern real_T rtMinusInf;

-extern real_T rtNaN;

-extern real32_T rtInfF;

-extern real32_T rtMinusInfF;

-extern real32_T rtNaNF;

-extern void rt_InitInfAndNaN(size_t realSize);

-extern boolean_T rtIsInf(real_T value);

-extern boolean_T rtIsInfF(real32_T value);

-extern boolean_T rtIsNaN(real_T value);

-extern boolean_T rtIsNaNF(real32_T value);

-

-typedef struct {

-  struct {

-    uint32_T wordH;

-    uint32_T wordL;

-  } words;

-} BigEndianIEEEDouble;

-

-typedef struct {

-  struct {

-    uint32_T wordL;

-    uint32_T wordH;

-  } words;

-} LittleEndianIEEEDouble;

-

-typedef struct {

-  union {

-    real32_T wordLreal;

-    uint32_T wordLuint;

-  } wordL;

-} IEEESingle;

-

-#endif

-/* End of code generation (rt_nonfinite.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
index 9a5c96267..b5a02a7a6 100755..100644
--- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -1,159 +1,160 @@
-/*

- * rtwtypes.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTWTYPES_H__

-#define __RTWTYPES_H__

-#ifndef TRUE

-# define TRUE (1U)

-#endif

-#ifndef FALSE

-# define FALSE (0U)

-#endif

-#ifndef __TMWTYPES__

-#define __TMWTYPES__

-

-#include <limits.h>

-

-/*=======================================================================* 

- * Target hardware information

- *   Device type: Generic->MATLAB Host Computer

- *   Number of bits:     char:   8    short:   16    int:  32

- *                       long:  32      native word size:  32

- *   Byte ordering: LittleEndian

- *   Signed integer division rounds to: Zero

- *   Shift right on a signed integer as arithmetic shift: on

- *=======================================================================*/

-

-/*=======================================================================* 

- * Fixed width word size data types:                                     * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 

- *=======================================================================*/

-

-typedef signed char int8_T;

-typedef unsigned char uint8_T;

-typedef short int16_T;

-typedef unsigned short uint16_T;

-typedef int int32_T;

-typedef unsigned int uint32_T;

-typedef float real32_T;

-typedef double real64_T;

-

-/*===========================================================================* 

- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 

- *                           ulong_T, char_T and byte_T.                     * 

- *===========================================================================*/

-

-typedef double real_T;

-typedef double time_T;

-typedef unsigned char boolean_T;

-typedef int int_T;

-typedef unsigned int uint_T;

-typedef unsigned long ulong_T;

-typedef char char_T;

-typedef char_T byte_T;

-

-/*===========================================================================* 

- * Complex number type definitions                                           * 

- *===========================================================================*/

-#define CREAL_T	

-   typedef struct {  

-     real32_T re;  

-     real32_T im;  

-   } creal32_T;  

-

-   typedef struct {  

-     real64_T re;  

-     real64_T im;  

-   } creal64_T;  

-

-   typedef struct {  

-     real_T re;  

-     real_T im;  

-   } creal_T;  

-

-   typedef struct {  

-     int8_T re;  

-     int8_T im;  

-   } cint8_T;  

-

-   typedef struct {  

-     uint8_T re;  

-     uint8_T im;  

-   } cuint8_T;  

-

-   typedef struct {  

-     int16_T re;  

-     int16_T im;  

-   } cint16_T;  

-

-   typedef struct {  

-     uint16_T re;  

-     uint16_T im;  

-   } cuint16_T;  

-

-   typedef struct {  

-     int32_T re;  

-     int32_T im;  

-   } cint32_T;  

-

-   typedef struct {  

-     uint32_T re;  

-     uint32_T im;  

-   } cuint32_T;  

-

-

-/*=======================================================================* 

- * Min and Max:                                                          * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *=======================================================================*/

-

-#define MAX_int8_T  	((int8_T)(127))

-#define MIN_int8_T  	((int8_T)(-128))

-#define MAX_uint8_T 	((uint8_T)(255))

-#define MIN_uint8_T 	((uint8_T)(0))

-#define MAX_int16_T 	((int16_T)(32767))

-#define MIN_int16_T 	((int16_T)(-32768))

-#define MAX_uint16_T	((uint16_T)(65535))

-#define MIN_uint16_T	((uint16_T)(0))

-#define MAX_int32_T 	((int32_T)(2147483647))

-#define MIN_int32_T 	((int32_T)(-2147483647-1))

-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))

-#define MIN_uint32_T	((uint32_T)(0))

-

-/* Logical type definitions */

-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)

-#  ifndef false

-#   define false (0U)

-#  endif

-#  ifndef true

-#   define true (1U)

-#  endif

-#endif

-

-/*

- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation

- * for signed integer values.

- */

-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)

-#error "This code must be compiled using a 2's complement representation for signed integer values"

-#endif

-

-/*

- * Maximum length of a MATLAB identifier (function/variable)

- * including the null-termination character. Referenced by

- * rt_logging.c and rt_matrx.c.

- */

-#define TMW_NAME_LENGTH_MAX	64

-

-#endif

-#endif

-/* End of code generation (rtwtypes.h) */

+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: ARM Compatible->ARM Cortex
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32
+ *                       native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Undefined
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+      real32_T re;  
+      real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+      real64_T re;  
+      real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+      real_T re;  
+      real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+      int8_T re;  
+      int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+      uint8_T re;  
+      uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+      int16_T re;  
+      int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+      uint16_T re;  
+      uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+      int32_T re;  
+      int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+      uint32_T re;  
+      uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index 99d0c5bf2..749b0a91c 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -39,16 +39,6 @@ MODULE_COMMAND	 = attitude_estimator_ekf
 
 SRCS		 = attitude_estimator_ekf_main.cpp \
 		   attitude_estimator_ekf_params.c \
-		   codegen/eye.c \
-		   codegen/attitudeKalmanfilter.c \
-		   codegen/mrdivide.c \
-		   codegen/rdivide.c \
-		   codegen/attitudeKalmanfilter_initialize.c \
-		   codegen/attitudeKalmanfilter_terminate.c \
-		   codegen/rt_nonfinite.c \
-		   codegen/rtGetInf.c \
-		   codegen/rtGetNaN.c \
-		   codegen/norm.c \
-		   codegen/cross.c
+		   codegen/AttitudeEKF.c
 
 MODULE_STACKSIZE = 1200
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 31c9157e1..e0bcbc6e9 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -223,7 +223,7 @@ BottleDrop::start()
 	_main_task = task_spawn_cmd("bottle_drop",
 				    SCHED_DEFAULT,
 				    SCHED_PRIORITY_DEFAULT + 15,
-				    2048,
+				    1500,
 				    (main_t)&BottleDrop::task_main_trampoline,
 				    nullptr);
 
@@ -283,7 +283,6 @@ BottleDrop::drop()
 	// force the door open if we have to
 	if (_doors_opened == 0) {
 		open_bay();
-		warnx("bay not ready, forced open");
 	}
 
 	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
 			open_bay();
 			drop();
-			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+			mavlink_log_critical(_mavlink_fd, "drop bottle");
 
 		} else if (cmd->param1 > 0.5f) {
 			open_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+			mavlink_log_critical(_mavlink_fd, "opening bay");
 
 		} else {
 			lock_release();
 			close_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+			mavlink_log_critical(_mavlink_fd, "closing bay");
 		}
 
 		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		switch ((int)(cmd->param1 + 0.5f)) {
 		case 0:
 			_drop_approval = false;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
 			break;
 
 		case 1:
 			_drop_approval = true;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position and approval");
 			break;
 
 		default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
 		break;
 
 	default:
diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
index 6b18fff55..df9f5473b 100644
--- a/src/modules/bottle_drop/module.mk
+++ b/src/modules/bottle_drop/module.mk
@@ -39,3 +39,7 @@ MODULE_COMMAND		= bottle_drop
 
 SRCS			= bottle_drop.cpp \
 			  bottle_drop_params.c
+
+MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 0cb41489f..d4cd97be6 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
+	const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 
 		/* inform user which axes are still needed */
 		mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
-				 (!data_collected[0]) ? "front " : "",
-				 (!data_collected[1]) ? "back " : "",
+				 (!data_collected[5]) ? "down " : "",
+				 (!data_collected[0]) ? "back " : "",
+				 (!data_collected[1]) ? "front " : "",
 				 (!data_collected[2]) ? "left " : "",
 				 (!data_collected[3]) ? "right " : "",
-				 (!data_collected[4]) ? "up " : "",
-				 (!data_collected[5]) ? "down " : "");
+				 (!data_collected[4]) ? "up " : "");
 
 		/* allow user enough time to read the message */
 		sleep(3);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd46..c9c8d16b5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
@@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm_disarm(true, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(true, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		arm_disarm(false, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(false, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
@@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
@@ -1543,7 +1548,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "RC signal regained");
+					mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
 					status_changed = true;
 				}
 			}
@@ -1644,8 +1649,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.rc_signal_lost) {
-				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
+				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
 				status.rc_signal_lost = true;
+				status.rc_signal_lost_timestamp=sp_man.timestamp;
 				status_changed = true;
 			}
 		}
@@ -1848,7 +1854,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			if (status.failsafe) {
+				mavlink_log_critical(mavlink_fd, "failsafe mode on");
+			} else {
+				mavlink_log_critical(mavlink_fd, "failsafe mode off");
+			}
 			failsafe_old = status.failsafe;
 		}
 
@@ -2196,18 +2206,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ACRO:
-		control_mode.flag_control_manual_enabled = true;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = false;
-		control_mode.flag_control_altitude_enabled = false;
-		control_mode.flag_control_climb_rate_enabled = false;
-		control_mode.flag_control_position_enabled = false;
-		control_mode.flag_control_velocity_enabled = false;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
 	case NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2220,64 +2218,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_OFFBOARD:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_offboard_enabled = true;
-
-		switch (sp_offboard.mode) {
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
-
-		default:
-			control_mode.flag_control_rates_enabled = false;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
-
-		break;
-
 	case NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2293,6 +2233,7 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RCRECOVER:
 	case NAVIGATION_STATE_AUTO_RTGS:
 	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
@@ -2318,6 +2259,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2331,6 +2285,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_DESCEND:
+		/* TODO: check if this makes sense */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
@@ -2344,6 +2311,63 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = true;
 		break;
 
+	case NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
+			break;
+
+		default:
+			control_mode.flag_control_rates_enabled = false;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+		}
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9568752ae..465f9cdc5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
-		const bool stay_in_failsafe)
+		   const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
+
 		/* go into failsafe
 		 * - if commanded to do so
 		 * - if we have an engine failure
-		 * - if either the datalink is enabled and lost as well as RC is lost
-		 * - if there is no datalink and the mission is finished */
+		 * - depending on datalink, RC and if the mission is finished */
+
+		/* first look at the commands */
 		if (status->engine_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->gps_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
-		/* Finished handling commands which have priority , now handle failures */
+			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
-		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+		/* datalink loss enabled:
+		 * check for datalink lost: this should always trigger RTGS */
+		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* also go into failsafe if just datalink is lost */
-		} else if (status->data_link_lost && data_link_loss_enabled) {
+		/* datalink loss disabled:
+		 * check if both, RC and datalink are lost during the mission
+		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+						       (status->rc_signal_lost && mission_finished))) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* don't bother if RC is lost and mission is not yet finished */
-		} else if (status->rc_signal_lost && !stay_in_failsafe) {
-
-			/* this mode is ok, we don't need RC for missions */
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
-			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
@@ -703,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		}
 
 		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
 			// XXX do not make this fatal yet
 		}
 	}
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index f0139a4b7..2860d1efc 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -39,3 +39,5 @@ SRCS		 = test_params.c \
 		   block/BlockParam.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index b2355d4d8..705e54be3 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
 {
 	work_q_item_t *work;
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* Initialize global variables */
 	g_key_offsets[0] = 0;
 
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
 			warnx("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
-		}
-		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
 			warnx("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-		}
-		else
+		} else {
 			warnx("Unknown restart");
-	}
-	else
+		}
+	} else {
 		warnx("Unknown restart");
+	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 685f5e12f..e7805daa9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
 			warn_index = max_warn_index;
 		}
 
-		warnx("reset: %s", feedback[warn_index]);
-		mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		// Do not warn about accel offset if we have no position updates
+		if (!(warn_index == 5 && _ekf->staticMode)) {
+			warnx("reset: %s", feedback[warn_index]);
+			mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		}
 	}
 
 	struct estimator_status_report rep;
@@ -1557,7 +1560,7 @@ FixedwingEstimator::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 5000,
+					 7500,
 					 (main_t)&FixedwingEstimator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c251dd3b2..29b7ec7b7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 5.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2900,
+		       2800,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b4911427f..a8f956ad0 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -382,11 +382,11 @@ protected:
 						clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-						struct tm t;
-						gmtime_r(&gps_time_sec, &t);
+						struct tm tt;
+						gmtime_r(&gps_time_sec, &tt);
 
 						// XXX we do not want to interfere here with the SD log app
-						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
 					}
@@ -1719,7 +1719,53 @@ protected:
 			msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
-			msg.rssi = rc.rssi;
+
+			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
+			 * available, the RSSI field is invalid, as they do not provide
+			 * an RSSI measurement. Use an out of band magic value to signal
+			 * these digital ports. XXX revise MAVLink spec to address this.
+			 * One option would be to use the top bit to toggle between RSSI
+			 * and input source mode.
+			 *
+			 * Full RSSI field: 0b 1 111 1111
+			 *
+			 *                     ^ If bit is set, RSSI encodes type + RSSI
+			 *
+			 *                       ^ These three bits encode a total of 8
+			 *                         digital RC input types.
+			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+			 *                           ^ These four bits encode a total of
+			 *                             16 RSSI levels. 15 = full, 0 = no signal
+			 *
+			 */
+
+			/* Initialize RSSI with the special mode level flag */
+			msg.rssi = (1 << 7);
+
+			/* Set RSSI */
+			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+			switch (rc.input_source) {
+				case RC_INPUT_SOURCE_PX4FMU_PPM:
+				/* fallthrough */
+				case RC_INPUT_SOURCE_PX4IO_PPM:
+					msg.rssi |= (0 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+					msg.rssi |= (1 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SBUS:
+					msg.rssi |= (2 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_ST24:
+					msg.rssi |= (3 << 4);
+					break;
+			}
+
+			if (rc.rc_lost) {
+				/* RSSI is by definition zero */
+				msg.rssi = 0;
+			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
@@ -1788,33 +1834,32 @@ protected:
 	}
 };
 
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamOpticalFlow::get_name_static();
+		return MavlinkStreamOpticalFlowRad::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "OPTICAL_FLOW";
+		return "OPTICAL_FLOW_RAD";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamOpticalFlow(mavlink);
+		return new MavlinkStreamOpticalFlowRad(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
@@ -1822,11 +1867,11 @@ private:
 	uint64_t _flow_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
-	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+	MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
 
 protected:
-	explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
 		_flow_time(0)
 	{}
@@ -1836,18 +1881,23 @@ protected:
 		struct optical_flow_s flow;
 
 		if (_flow_sub->update(&_flow_time, &flow)) {
-			mavlink_optical_flow_t msg;
+			mavlink_optical_flow_rad_t msg;
 
 			msg.time_usec = flow.timestamp;
 			msg.sensor_id = flow.sensor_id;
-			msg.flow_x = flow.flow_raw_x;
-			msg.flow_y = flow.flow_raw_y;
-			msg.flow_comp_m_x = flow.flow_comp_x_m;
-			msg.flow_comp_m_y = flow.flow_comp_y_m;
+			msg.integrated_x = flow.pixel_flow_x_integral;
+			msg.integrated_y = flow.pixel_flow_y_integral;
+			msg.integrated_xgyro = flow.gyro_x_rate_integral;
+			msg.integrated_ygyro = flow.gyro_y_rate_integral;
+			msg.integrated_zgyro = flow.gyro_z_rate_integral;
+			msg.distance = flow.ground_distance_m;
 			msg.quality = flow.quality;
-			msg.ground_distance = flow.ground_distance_m;
+			msg.integration_time_us = flow.integration_timespan;
+			msg.sensor_id = flow.sensor_id;
+			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
 		}
 	}
 };
@@ -2153,7 +2203,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bc092c7e9..e98d72afe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_int(msg);
 		break;
 
-	case MAVLINK_MSG_ID_OPTICAL_FLOW:
-		handle_message_optical_flow(msg);
+	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+		handle_message_optical_flow_rad(msg);
 		break;
 
 	case MAVLINK_MSG_ID_SET_MODE:
@@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 {
 	/* optical flow */
-	mavlink_optical_flow_t flow;
-	mavlink_msg_optical_flow_decode(msg, &flow);
+	mavlink_optical_flow_rad_t flow;
+	mavlink_msg_optical_flow_rad_decode(msg, &flow);
 
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.flow_x;
-	f.flow_raw_y = flow.flow_y;
-	f.flow_comp_x_m = flow.flow_comp_m_x;
-	f.flow_comp_y_m = flow.flow_comp_m_y;
-	f.ground_distance_m = flow.ground_distance;
+	f.timestamp = flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
+	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.integrated_x;
-	f.flow_raw_y = flow.integrated_y;
+	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
 	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -538,12 +546,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e5f2c6a73..a057074a7 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,7 +112,7 @@ private:
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_command_int(mavlink_message_t *msg);
-	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 16a3b627f..887a53508 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -458,8 +458,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
-	warnx("started");
-	fflush(stdout);
 
 	/*
 	 * do subscriptions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d52138522..5918d6bc5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
 
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
 
@@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt)
 void
 MulticopterPositionControl::task_main()
 {
-	warnx("started");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c73..e789fd10d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
 			if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
 		warnx("dll end");
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..0765e9b7c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
-	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-		/* mission item index out of bounds */
-		return false;
-	}
-
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
+
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+			/* mission item index out of bounds */
+			return false;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				if (is_current) {
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+					    &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"ERROR DO JUMP waypoint could not be written");
+								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				mavlink_log_critical(_navigator->get_mavlink_fd(),
-						 "DO JUMP repetitions completed");
+				if (is_current) {
+					mavlink_log_critical(_navigator->get_mavlink_fd(),
+							     "DO JUMP repetitions completed");
+				}
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
+	reset_mission_item_reached();
 }
 
 void
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..9cd609955 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0da..df620e5e7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
 	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
+	_pos_sp_triplet_published_invalid_once(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -289,8 +290,9 @@ Navigator::task_main()
 	navigation_capabilities_update();
 	params_update();
 
-	/* rate limit position updates to 50 Hz */
+	/* rate limit position and sensor updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
+	orb_set_interval(_sensor_combined_sub, 20);
 
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
@@ -426,12 +428,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
 				} else {
@@ -439,11 +444,13 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-					_navigation_mode = &_rtl;
+				_pos_sp_triplet_published_invalid_once = false;
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_datalinkloss_obc.get() != 0) {
 					_navigation_mode = &_dataLinkLoss;
 				} else {
@@ -451,9 +458,11 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
 			default:
@@ -467,9 +476,9 @@ Navigator::task_main()
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
-		/* if nothing is running, set position setpoint triplet invalid */
-		if (_navigation_mode == nullptr) {
-			// TODO publish empty sp only once
+		/* if nothing is running, set position setpoint triplet invalid once */
+		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+			_pos_sp_triplet_published_invalid_once = true;
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
@@ -497,8 +506,8 @@ Navigator::start()
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
-					 2000,
+					 SCHED_PRIORITY_DEFAULT + 20,
+					 1800,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
@@ -630,9 +639,6 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
-	/* reset reached bool */
-	_mission_result.reached = false;
-	_mission_result.finished = false;
 }
 
 void
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42..42392e739 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 		}
 		break;
 	case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
 		warnx("rcl end");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81bbaa658..296919c04 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 			thread_should_exit = true;
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("app is running");
+			warnx("is running");
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started");
 	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[inav] started");
 
 	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
@@ -298,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float w_flow = 0.0f;
 
 	float sonar_prev = 0.0f;
-	hrt_abstime flow_prev = 0;			// time of last flow measurement
+	//hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
 	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
 
@@ -389,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					} else {
 						wait_baro = false;
 						baro_offset /= (float) baro_init_cnt;
-						warnx("baro offs: %.2f", (double)baro_offset);
-						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
+						warnx("baro offs: %d", (int)baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
 					}
@@ -491,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
 				/* calculate time from previous update */
-				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
-				flow_prev = flow.flow_timestamp;
+//				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+//				flow_prev = flow.flow_timestamp;
 
 				if ((flow.ground_distance_m > 0.31f) &&
 					(flow.ground_distance_m < 4.0f) &&
@@ -520,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							sonar_valid_time = t;
 							sonar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
-							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
 						}
 
 					} else {
@@ -550,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* convert raw flow to angular flow (rad/s) */
 					float flow_ang[2];
-					flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					//todo check direction of x und y axis
+					flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					float flow_m[3];
 					flow_m[0] = -flow_ang[0] * flow_dist;
@@ -721,8 +720,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 							/* initialize projection */
 							map_projection_init(&ref, lat, lon);
-							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
+							// XXX replace this print
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e..58c9429b6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
 #include <stdbool.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
@@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
 	if (*dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
 					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
@@ -170,6 +171,12 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+	if (dsm_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+	}
+	if (st24_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -417,6 +424,15 @@ controls_tick() {
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
+		/*
+		  if the FMU is dead then enable override if we have a
+		  mixer and OVERRIDE_IMMEDIATE is set
+		 */
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+			override = true;                
+
 		if (override) {
 
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index c0b8ac358..66f0969de 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0;
 int
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while safety off */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+	/* do not allow a mixer change while safety off and FMU armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return 1;
 	}
 
-	/* abort if we're in the mixer */
+	/* disable mixing, will be enabled once load is complete */
+	r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
+
+	/* abort if we're in the mixer - the caller is expected to retry */
 	if (in_mixer) {
 		return 1;
 	}
@@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length)
 
 	isr_debug(2, "mix txt %u", length);
 
-	if (length < sizeof(px4io_mixdata))
+	if (length < sizeof(px4io_mixdata)) {
 		return 0;
+	}
 
-	unsigned	text_length = length - sizeof(px4io_mixdata);
+	unsigned text_length = length - sizeof(px4io_mixdata);
 
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
 		isr_debug(2, "reset");
 
-		/* FIRST mark the mixer as invalid */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 		/* THEN actually delete it */
 		mixer_group.reset();
 		mixer_text_length = 0;
@@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
-		/* disable mixing during the update */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9b2e047cb..c7e9ae3eb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,7 @@
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
 #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE	(1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 30f32b38e..14ee9cb40 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
 #define NUM_MSG 2
 static char msg[NUM_MSG][40];
 
+static void heartbeat_blink(void);
+static void ring_blink(void);
+
 /*
  * add a debug message to be printed on the console
  */
@@ -124,8 +127,64 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+ring_blink(void)
+{
 #ifdef GPIO_LED4
-	LED_RING(heartbeat);
+
+	if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+		LED_RING(1);
+		return;
+	}
+
+	// XXX this led code does have
+	// intentionally a few magic numbers.
+	const unsigned max_brightness = 118;
+
+	static unsigned counter = 0;
+	static unsigned brightness = max_brightness;
+	static unsigned brightness_counter = 0;
+	static unsigned on_counter = 0;
+
+	if (brightness_counter < max_brightness) {
+
+		bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
+
+		// XXX once led is PWM driven,
+		// remove the ! in the line below
+		// to return to the proper breathe
+		// animation / pattern (currently inverted)
+		LED_RING(!on);
+		brightness_counter++;
+
+		if (on) {
+			on_counter++;
+		}
+
+	} else {
+
+		if (counter >= 62) {
+			counter = 0;
+		}
+
+		int n;
+
+		if (counter < 32) {
+			n = counter;
+
+		} else {
+			n = 62 - counter;
+		}
+
+		brightness = (n * n) / 8;
+		brightness_counter = 0;
+		on_counter = 0;
+		counter++;
+	}
+
 #endif
 }
 
@@ -300,6 +359,8 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
+		ring_blink();
+
                 check_reboot();
 
 		/* check for debug activity (default: none) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 8186e4c78..93a33490f 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit;
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
 #define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, !(_s))
+#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, (_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49c2a9f56..f0c2cfd26 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -191,7 +191,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
 					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
-					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -406,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-			return mixer_handle_text(values, num_values * sizeof(*values));
-		}
-		break;
+		/* do not change the mixer if FMU is armed and IO's safety is off
+		 * this state defines an active system. This check is done in the
+		 * text handling function.
+		 */
+		return mixer_handle_text(values, num_values * sizeof(*values));
 
 	default:
 		/* avoid offset wrap */
@@ -582,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_REBOOT_BL:
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -629,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/**
-		 * do not allow a RC config change while outputs armed
+		 * do not allow a RC config change while safety is off
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 			break;
 		}
 
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index a28d43e72..d4a00af39 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -43,3 +43,5 @@ SRCS = sdlog2.c \
        logbuffer.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index af580f1f7..0b6861d2a 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg)
 	/* set name */
 	prctl(PR_SET_NAME, "sdlog2_writer", 0);
 
+	perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write");
+
 	int log_fd = open_log_file();
 
 	if (log_fd < 0) {
@@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
+			perf_begin(perf_write);
 			n = write(log_fd, read_ptr, n);
+			perf_end(perf_write);
 
 			should_wait = (n == available) && !is_part;
 
@@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
+	/* free performance counter */
+	perf_free(perf_write);
+
 	return NULL;
 }
 
@@ -1511,11 +1518,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- FLOW --- */
 		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
-			log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
-			log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
-			log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
-			log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
-			log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
+			log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
+			log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
+			log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
+			log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
+			log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
+			log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
 			log_msg.body.log_FLOW.quality = buf.flow.quality;
 			log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
 			LOGBUFFER_WRITE_AND_COUNT(FLOW);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fa9bdacb8..30491036a 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -200,13 +200,19 @@ struct log_ARSP_s {
 /* --- FLOW - OPTICAL FLOW --- */
 #define LOG_FLOW_MSG 15
 struct log_FLOW_s {
-	int16_t flow_raw_x;
-	int16_t flow_raw_y;
-	float flow_comp_x;
-	float flow_comp_y;
-	float distance;
-	uint8_t	quality;
+	uint64_t timestamp;
 	uint8_t sensor_id;
+	float pixel_flow_x_integral;
+	float pixel_flow_y_integral;
+	float gyro_x_rate_integral;
+	float gyro_y_rate_integral;
+	float gyro_z_rate_integral;
+	float ground_distance_m;
+	uint32_t integration_timespan;
+	uint32_t time_since_last_sonar_update;
+	uint16_t frame_count_since_last_readout;
+	int16_t gyro_temperature;
+	uint8_t	quality;
 };
 
 /* --- GPOS - GLOBAL POSITION ESTIMATE --- */
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index 5b1bc5e86..dfbba92d9 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -42,3 +42,5 @@ SRCS		= sensors.cpp \
 		  sensor_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
new file mode 100644
index 000000000..4bcf95784
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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 mcu_version.c
+ * 
+ * Read out the microcontroller version from the board
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include "mcu_version.h"
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_ARCH_CHIP_STM32
+#include <up_arch.h>
+
+#define DBGMCU_IDCODE	0xE0042000
+
+#define STM32F40x_41x	0x413
+#define STM32F42x_43x	0x419
+
+#define REVID_MASK	0xFFFF0000
+#define DEVID_MASK	0xFFF
+
+#endif
+
+
+
+int mcu_version(char* rev, char** revstr)
+{
+#ifdef CONFIG_ARCH_CHIP_STM32
+	uint32_t abc = getreg32(DBGMCU_IDCODE);
+
+	int32_t chip_version = abc & DEVID_MASK;
+	enum MCU_REV revid = (abc & REVID_MASK) >> 16;
+
+	switch (chip_version) {
+	case STM32F40x_41x:
+		*revstr = "STM32F40x";
+		break;
+	case STM32F42x_43x:
+		*revstr = "STM32F42x";
+		break;
+	default:
+		*revstr = "STM32F???";
+		break;
+	}
+
+	switch (revid) {
+
+		case MCU_REV_STM32F4_REV_A:
+			*rev = 'A';
+			break;
+		case MCU_REV_STM32F4_REV_Z:
+			*rev = 'Z';
+			break;
+		case MCU_REV_STM32F4_REV_Y:
+			*rev = 'Y';
+			break;
+		case MCU_REV_STM32F4_REV_1:
+			*rev = '1';
+			break;
+		case MCU_REV_STM32F4_REV_3:
+			*rev = '3';
+			break;
+		default:
+			*rev = '?';
+			revid = -1;
+			break;
+	}
+
+	return revid;
+#else
+	return -1;
+#endif
+}
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
new file mode 100644
index 000000000..1b3d0aba9
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/* magic numbers from reference manual */
+enum MCU_REV {
+	MCU_REV_STM32F4_REV_A = 0x1000,
+	MCU_REV_STM32F4_REV_Z = 0x1001,
+	MCU_REV_STM32F4_REV_Y = 0x1003,
+	MCU_REV_STM32F4_REV_1 = 0x1007,
+	MCU_REV_STM32F4_REV_3 = 0x2001
+};
+
+/**
+ * Reports the microcontroller version of the main CPU.
+ *
+ * @param rev The silicon revision character
+ * @param revstr The full chip name string
+ * @return The silicon revision / version number as integer
+ */
+__EXPORT int mcu_version(char* rev, char** revstr);
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index bf3428a50..0d629d610 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
 			warnx("line too long");
+			fclose(fp);
 			return -1;
 		}
 
@@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		strcat(buf, line);
 	}
 
+	fclose(fp);
 	return 0;
 }
 
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 147903aa0..fe8b7e75a 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,5 +53,7 @@ SRCS		 = err.c \
 		   otp.c \
 		   board_serial.c \
 		   pwm_limit/pwm_limit.c \
-		   circuit_breaker.c
+		   circuit_breaker.c \
+		   mcu_version.c
 
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index cd0b30dd6..71757e1f4 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -46,6 +46,7 @@
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/actuator_outputs.h"
+#include "topics/actuator_direct.h"
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
 
@@ -76,6 +77,7 @@ template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
 template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
 
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 0c29101fe..9385ce253 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -44,3 +44,5 @@ SRCS			= uORB.cpp \
 			  objects_common.cpp \
 			  Publication.cpp \
 			  Subscription.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index b91a00c1e..49dfc7834 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/actuator_direct.h"
+ORB_DEFINE(actuator_direct, struct actuator_direct_s);
+
 #include "topics/multirotor_motor_limits.h"
 ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
 
diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h
new file mode 100644
index 000000000..5f9d0f56d
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_direct.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 actuator_direct.h
+ *
+ * Actuator direct values.
+ *
+ * Values published to this topic are the direct actuator values which
+ * should be passed to actuators, bypassing mixing
+ */
+
+#ifndef TOPIC_ACTUATOR_DIRECT_H
+#define TOPIC_ACTUATOR_DIRECT_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATORS_DIRECT		16
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct actuator_direct_s {
+	uint64_t timestamp;				/**< timestamp in us since system boot */
+	float values[NUM_ACTUATORS_DIRECT];		/**< actuator values, from -1 to 1 */
+	unsigned nvalues;				/**< number of valid values */
+};
+
+/**
+ * @}
+ */
+
+/* actuator direct ORB */
+ORB_DECLARE(actuator_direct);
+
+#endif // TOPIC_ACTUATOR_DIRECT_H
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 15b55648d..13138ebc9 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
  * Switch position
  */
 typedef enum {
-	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
 	SWITCH_POS_ON,			/**< switch activated (value = 1) */
 	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
 	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
 	float aux4;			/**< default function: camera roll */
 	float aux5;			/**< default function: payload drop */
 
-	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;		/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-}; /**< manual control inputs */
+	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
+	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+};
 
 /**
  * @}
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index 0196ae86b..d3dc46ee0 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -55,16 +55,22 @@
  */
 struct optical_flow_s {
 
-	uint64_t timestamp;		/**< in microseconds since system start          */
-
-	uint64_t flow_timestamp;		/**< timestamp from flow sensor */
-	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
-	float ground_distance_m;	/**< Altitude / distance to ground in meters */
-	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint64_t timestamp;		/**< in microseconds since system start  */
 	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+	float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
+	float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
+	float gyro_x_rate_integral;	 /**< accumulated gyro value in radians around x axis */
+	float gyro_y_rate_integral;  /**< accumulated gyro value in radians around y axis */
+	float gyro_z_rate_integral;   /**< accumulated gyro value in radians around z axis */
+	float ground_distance_m;	 /**< Altitude / distance to ground in meters */
+	uint32_t integration_timespan; /**<accumulation timespan in microseconds     */
+	uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
+	uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
+	int16_t gyro_temperature;/**< 	Temperature * 100 in centi-degrees Celsius */
+	uint8_t	quality;		/**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
+
+
+
 
 };
 
diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
index 491096934..2dd90e69d 100644
--- a/src/modules/uORB/topics/test_motor.h
+++ b/src/modules/uORB/topics/test_motor.h
@@ -57,7 +57,7 @@
 struct test_motor_s {
 	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
 	unsigned 	motor_number;				/**< number of motor to spin */
-	float		value;					/**< output data, in natural output units */
+	float		value;					/**< output power, range [0..1] */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 31e616f4f..7888a50f4 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
 	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
-	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
 
 	uint8_t satellites_used;			/**< Number of satellites used */
 };
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 8e85b4835..131029061 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -108,7 +108,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
-	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
+	NAVIGATION_STATE_DESCEND,		/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
 	NAVIGATION_STATE_OFFBOARD,
 	NAVIGATION_STATE_MAX,
@@ -201,6 +201,7 @@ struct vehicle_status_s {
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	uint64_t rc_signal_lost_timestamp;		/**< Time at which the RC reception was lost */
 	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef..9f682c7e1 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
 #include "esc.hpp"
 #include <systemlib/err.h>
 
+
+#define MOTOR_BIT(x) (1<<(x))
+
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
@@ -73,7 +76,9 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
+	if ((outputs == nullptr) || 
+            (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+            (num_outputs > CONNECTED_ESC_MAX)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -95,14 +100,18 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
 
-	if (_armed) {
-		for (unsigned i = 0; i < num_outputs; i++) {
-
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
-			if (scaled < 1.0F) {
-				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
-			}
-
+                        // trim negative values back to 0. Previously
+                        // we set this to 0.1, which meant motors kept
+                        // spinning when armed, but that should be a
+                        // policy decision for a specific vehicle
+                        // type, as it is not appropriate for all
+                        // types of vehicles (eg. fixed wing).
+			if (scaled < 0.0F) {
+				scaled = 0.0F;
+                        }
 			if (scaled > cmd_max) {
 				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);
@@ -111,6 +120,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 			msg.cmd.push_back(static_cast<int>(scaled));
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+		} else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
 		}
 	}
 
@@ -121,9 +132,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
 }
 
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
+{
+	if (arm) {
+		_armed_mask = -1;
+	} else {
+		_armed_mask = 0;
+	}
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	_armed = arm;
+	if (arm) {
+		_armed_mask = MOTOR_BIT(num);
+	} else {
+		_armed_mask = 0;
+	}
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e6..12c035542 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
 
 	void update_outputs(float *outputs, unsigned num_outputs);
 
-	void arm_esc(bool arm);
+	void arm_all_escs(bool arm);
+	void arm_single_esc(int num, bool arm);
 
 private:
 	/**
@@ -99,6 +100,11 @@ private:
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
 	/*
+	 * ESC states
+	 */
+	uint32_t 			_armed_mask = 0;
+
+	/*
 	 * Perf counters
 	 */
 	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828..8741ae20d 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 {
 	auto report = ::baro_report();
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
-
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
 	report.temperature = msg.static_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf..a375db37f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401..2111ff80b 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
 
 #pragma once
 
-#include <drivers/drv_hrt.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f..35ebee542 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,8 @@
 
 #include "mag.hpp"
 
+#include <systemlib/err.h>
+
 static const orb_id_t MAG_TOPICS[3] = {
 	ORB_ID(sensor_mag0),
 	ORB_ID(sensor_mag1),
@@ -49,6 +51,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
 UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
 _sub_mag(node)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
 	_scale.x_scale = 1.0F;
 	_scale.y_scale = 1.0F;
 	_scale.z_scale = 1.0F;
@@ -69,9 +73,36 @@ int UavcanMagnetometerBridge::init()
 	return 0;
 }
 
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct mag_report);
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	if (last_read < _report.timestamp) {
+		/* copy report */
+		lock();
+		*mag_buf = _report;
+		last_read = _report.timestamp;
+		unlock();
+		return sizeof(struct mag_report);
+	} else {
+		/* no new data available, warn caller */
+		return -EAGAIN;
+	}
+}
+
 int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+		return OK;			// Pretend that this stuff is supported to keep APM happy
+	}
 	case MAGIOCSSCALE: {
 		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
 		return 0;
@@ -84,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 		return 0;           // Nothing to do
 	}
 	case MAGIOCGEXTERNAL: {
-		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+		return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
 	}
 	case MAGIOCSSAMPLERATE: {
 		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
@@ -106,18 +137,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 
 void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
 {
-	auto report = ::mag_report();
-
-	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
+	lock();
+	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
-	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
-	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
 
-	publish(msg.getSrcNodeID().get(), &report);
+	publish(msg.getSrcNodeID().get(), &_report);
 }
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f7..74077d883 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -54,6 +54,7 @@ public:
 	int init() override;
 
 private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
+	mag_report _report =  {};
 };
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 9608ce680..0999938fc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 			return;
 		}
 
+		// update device id as we now know our device node_id
+		_device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
 		// Ask the CDev helper which class instance we can take
 		const int class_instance = register_class_devname(_class_devname);
 		if (class_instance < 0 || class_instance >= int(_max_channels)) {
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 1316f7ecc..e31960537 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -112,6 +112,8 @@ protected:
 	_channels(new Channel[MaxChannels])
 	{
 		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+		_device_id.devid_s.bus = 0;
 	}
 
 	/**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a8485ee44..60901e0c7 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -46,6 +46,8 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
+#include <uORB/topics/esc_status.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
@@ -269,6 +271,24 @@ void UavcanNode::node_spin_once()
 	}
 }
 
+/*
+  add a fd to the list of polled events. This assumes you want
+  POLLIN for now.
+ */
+int UavcanNode::add_poll_fd(int fd)
+{
+	int ret = _poll_fds_num;
+	if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
+		errx(1, "uavcan: too many poll fds, exiting");
+	}
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
+	return ret;
+}
+
+
 int UavcanNode::run()
 {
 	(void)pthread_mutex_lock(&_node_mutex);
@@ -279,9 +299,10 @@ int UavcanNode::run()
 	_output_count = 2;
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
+	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
 
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
+	memset(&_outputs, 0, sizeof(_outputs));
 
 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
 	if (busevent_fd < 0)
@@ -303,11 +324,15 @@ int UavcanNode::run()
 	 * the value returned from poll() to detect whether actuator control has timed out or not.
 	 * Instead, all ORB events need to be checked individually (see below).
 	 */
-	_poll_fds_num = 0;
-	_poll_fds[_poll_fds_num] = ::pollfd();
-	_poll_fds[_poll_fds_num].fd = busevent_fd;
-	_poll_fds[_poll_fds_num].events = POLLIN;
-	_poll_fds_num += 1;
+	add_poll_fd(busevent_fd);
+
+	/*
+	 * setup poll to look for actuator direct input if we are
+	 * subscribed to the topic
+	 */
+	if (_actuator_direct_sub != -1) {
+		_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
+	}
 
 	while (!_task_should_exit) {
 		// update actuator controls subscriptions if needed
@@ -325,6 +350,8 @@ int UavcanNode::run()
 
 		node_spin_once();  // Non-blocking
 
+		bool new_output = false;
+
 		// this would be bad...
 		if (poll_ret < 0) {
 			log("poll error %d", errno);
@@ -332,67 +359,102 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
-					if (_poll_fds[poll_id].revents & POLLIN) {
+					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
 						controls_updated = true;
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 					}
-					poll_id++;
 				}
 			}
 
+			/*
+			  see if we have any direct actuator updates
+			 */
+			if (_actuator_direct_sub != -1 && 
+			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
+			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
+			    !_test_in_progress) {
+				if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
+					_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
+				}
+				memcpy(&_outputs.output[0], &_actuator_direct.values[0], 
+				       _actuator_direct.nvalues*sizeof(float));
+				_outputs.noutputs = _actuator_direct.nvalues;
+				new_output = true;
+			}
+
 			// can we mix?
-			if (controls_updated && (_mixers != nullptr)) {
+			if (_test_in_progress) {
+				memset(&_outputs, 0, sizeof(_outputs));
+				if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) {
+					_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+					_outputs.noutputs = _test_motor.motor_number+1;
+				}
+				new_output = true;
+			} else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
 				// but this driver could well serve multiple groups.
 				unsigned num_outputs_max = 8;
 
 				// Do mixing
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
-				outputs.timestamp = hrt_absolute_time();
-
-				// iterate actuators
-				for (unsigned i = 0; i < outputs.noutputs; i++) {
-					// last resort: catch NaN, INF and out-of-band errors
-					if (!isfinite(outputs.output[i])) {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = -1.0f;
-					}
+				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
 
-					// limit outputs to valid range
+				new_output = true;
+			}
+		}
 
-					// never go below min
-					if (outputs.output[i] < -1.0f) {
-						outputs.output[i] = -1.0f;
-					}
+		if (new_output) {
+			// iterate actuators, checking for valid values
+			for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+				// last resort: catch NaN, INF and out-of-band errors
+				if (!isfinite(_outputs.output[i])) {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = -1.0f;
+				}
 
-					// never go below max
-					if (outputs.output[i] > 1.0f) {
-						outputs.output[i] = 1.0f;
-					}
+				// never go below min
+				if (_outputs.output[i] < -1.0f) {
+					_outputs.output[i] = -1.0f;
 				}
 
-				// Output to the bus
-				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
+				// never go above max
+				if (_outputs.output[i] > 1.0f) {
+					_outputs.output[i] = 1.0f;
+				}
 			}
+			// Output to the bus
+			_outputs.timestamp = hrt_absolute_time();
+			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
 		}
 
-		// Check arming state
+
+		// Check motor test state
 		bool updated = false;
+		orb_check(_test_motor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+			// Update the test status and check that we're not locked down
+			_test_in_progress = (_test_motor.value > 0);
+			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+		}
+
+		// Check arming state
 		orb_check(_armed_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down
-			bool set_armed = _armed.armed && !_armed.lockdown;
+			// Update the armed status and check that we're not locked down and motor 
+			// test is not running
+			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
 			arm_actuators(set_armed);
 		}
@@ -429,7 +491,7 @@ int
 UavcanNode::arm_actuators(bool arm)
 {
 	_is_armed = arm;
-	_esc_controller.arm_esc(arm);
+	_esc_controller.arm_all_escs(arm);
 	return OK;
 }
 
@@ -440,7 +502,6 @@ UavcanNode::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	// the first fd used by CAN
-	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -453,9 +514,7 @@ UavcanNode::subscribe()
 		}
 
 		if (_control_subs[i] > 0) {
-			_poll_fds[_poll_fds_num].fd = _control_subs[i];
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num++;
+			_poll_ids[i] = add_poll_fd(_control_subs[i]);
 		}
 	}
 }
@@ -553,6 +612,36 @@ UavcanNode::print_info()
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
 	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
 
+	if (_outputs.noutputs != 0) {
+		printf("ESC output: ");
+
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d ", (int)(_outputs.output[i]*1000));
+		}
+		printf("\n");
+
+		// ESC status
+		int esc_sub = orb_subscribe(ORB_ID(esc_status));
+		struct esc_status_s esc;
+		memset(&esc, 0, sizeof(esc));
+		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+		printf("ESC Status:\n");
+		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d\t",    esc.esc[i].esc_address);
+			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+			printf("%3.2f\t", (double)esc.esc[i].esc_current);
+			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+			printf("%d\t",    esc.esc[i].esc_rpm);
+			printf("%d",      esc.esc[i].esc_errorcount);
+			printf("\n");
+		}
+
+		orb_unsubscribe(esc_sub);
+	}
+
 	// Sensor bridges
 	auto br = _sensor_bridges.getHead();
 	while (br != nullptr) {
@@ -571,7 +660,7 @@ UavcanNode::print_info()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start|status|stop}");
+	      "\tuavcan {start|status|stop|arm|disarm}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -618,6 +707,16 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
+	if (!std::strcmp(argv[1], "arm")) {
+		inst->arm_actuators(true);
+		::exit(0);
+	}
+
+	if (!std::strcmp(argv[1], "disarm")) {
+		inst->arm_actuators(false);
+		::exit(0);
+	}
+
 	if (!std::strcmp(argv[1], "stop")) {
 		delete inst;
 		::exit(0);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db9741..98f2e5ad4 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,8 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -56,6 +58,9 @@
 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
 
+// we add two to allow for actuator_direct and busevent
+#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
+
 /**
  * A UAVCAN node.
  */
@@ -96,6 +101,8 @@ private:
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();
+	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+
 
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
@@ -103,6 +110,10 @@ private:
 	actuator_armed_s	_armed;				///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
+	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	test_motor_s		_test_motor;
+	bool			_test_in_progress = false;
+
 	unsigned		_output_count = 0;		///< number of actuators currently available
 
 	static UavcanNode	*_instance;			///< singleton pointer
@@ -120,6 +131,15 @@ private:
 	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
-	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
+	pollfd			_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
 	unsigned		_poll_fds_num = 0;
+
+	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint8_t			_actuator_direct_poll_fd_num;
+	actuator_direct_s	_actuator_direct;
+
+	actuator_outputs_s	_outputs;
+
+	// index into _poll_fds for each _control_subs handle
+	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
 };
diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
index eb36d2ded..261428ef9 100644
--- a/src/systemcmds/motor_test/module.mk
+++ b/src/systemcmds/motor_test/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = motor_test
 SRCS		 = motor_test.c
 
 MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 079f99674..77dc2f0d5 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -54,32 +54,35 @@
 #include "systemlib/err.h"
 
 
-__EXPORT int	motor_test_main(int argc, char *argv[]);
+__EXPORT int motor_test_main(int argc, char *argv[]);
+
 static void motor_test(unsigned channel, float value);
 static void usage(const char *reason);
 
+static orb_advert_t _test_motor_pub;
+
 void motor_test(unsigned channel, float value)
 {
-	orb_advert_t        _test_motor_pub;
 	struct test_motor_s _test_motor;
 
 	_test_motor.motor_number = channel;
 	_test_motor.timestamp = hrt_absolute_time();
 	_test_motor.value = value;
 
-        if (_test_motor_pub > 0) {
-	    /* publish armed state */
-            orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
-        } else {
-            /* advertise and publish */
-            _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
-        }
+    if (_test_motor_pub > 0) {
+    /* publish test state */
+        orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+    } else {
+        /* advertise and publish */
+        _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+    }
 }
 
 static void usage(const char *reason)
 {
-	if (reason != NULL)
+	if (reason != NULL) {
 		warnx("%s", reason);
+	}
 
 	errx(1,
 		"usage:\n"
@@ -90,8 +93,9 @@ static void usage(const char *reason)
 
 int motor_test_main(int argc, char *argv[])
 {
-	unsigned long channel, lval;
-	float value;
+	unsigned long channel = 0;
+	unsigned long lval;
+	float value = 0.0f;
 	int ch;
 
 	if (argc != 5) {
@@ -102,18 +106,18 @@ int motor_test_main(int argc, char *argv[])
 		switch (ch) {
 
 		case 'm':
-			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			/* Read in motor number */
 			channel = strtoul(optarg, NULL, 0);
 			break;
 
 		case 'p':
-			/* Read in custom low value */
+			/* Read in power value */
 			lval = strtoul(optarg, NULL, 0);
 
 			if (lval > 100)
 				usage("value invalid");
 
-			value = (float)lval/100.f;
+			value = ((float)lval)/100.f;
 			break;
 		default:
 			usage(NULL);
@@ -122,7 +126,7 @@ int motor_test_main(int argc, char *argv[])
 
 	motor_test(channel, value);
 
-	printf("motor %d set to %.2f\n", channel, value);
+	printf("motor %d set to %.2f\n", channel, (double)value);
 
 	exit(0);
 }
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index f06c49552..edeb5c624 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -51,6 +51,8 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 
+#include <uORB/topics/actuator_armed.h>
+
 __EXPORT int nshterm_main(int argc, char *argv[]);
 
 int
@@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[])
     }
     unsigned retries = 0;
     int fd = -1;
+    int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+    struct actuator_armed_s armed;
+    /* we assume the system does not provide arming status feedback */
+    bool armed_updated = false;
+
+    /* try the first 30 seconds or if arming system is ready */
+    while ((retries < 300) || armed_updated) {
+
+        /* abort if an arming topic is published and system is armed */
+        bool updated = false;
+        if (orb_check(armed_fd, &updated)) {
+            /* the system is now providing arming status feedback.
+             * instead of timing out, we resort to abort bringing
+             * up the terminal.
+             */
+            armed_updated = true;
+            orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+            if (armed.armed) {
+                /* this is not an error, but we are done */
+                exit(0);
+            }
+        }
 
-    /* try the first 30 seconds */
-    while (retries < 300) {
         /* the retries are to cope with the behaviour of /dev/ttyACM0 */
         /* which may not be ready immediately. */
         fd = open(argv[1], O_RDWR);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 478c2a772..eeba89fa8 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -74,34 +74,34 @@ usage(const char *reason)
 		"usage:\n"
 		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
-		"  arm                      Arm output\n"
-		"  disarm                   Disarm output\n"
+		"arm\t\t\t\tArm output\n"
+		"disarm\t\t\t\tDisarm output\n"
 		"\n"
-		"  rate ...                 Configure PWM rates\n"
-		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
-		"    [-m <chanmask> ]       Directly supply channel mask\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"rate ...\t\t\tConfigure PWM rates\n"
+		"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
 		"\n"
-		"  failsafe ...      	    Configure failsafe PWM values\n"
-		"  disarmed ...      	    Configure disarmed PWM values\n"
-		"  min ...           	    Configure minimum PWM values\n"
-		"  max ...           	    Configure maximum PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"failsafe ...\t\t\tFailsafe PWM\n"
+		"disarmed ...\t\t\tDisarmed PWM\n"
+		"min ...\t\t\t\tMinimum PWM\n"
+		"max ...\t\t\t\tMaximum PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  test ...                 Directly set PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"test ...\t\t\tDirectly set PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  info                     Print information about the PWM device\n"
+		"info\t\t\t\tPrint information\n"
 		"\n"
-		"    -v                     Print verbose information\n"
-		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"\t-v\t\t\tVerbose\n"
+		"\t-d <dev>\t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[])
 	unsigned single_ch = 0;
 	unsigned pwm_value = 0;
 
-	if (argc < 1)
+	if (argc < 2)
 		usage(NULL);
 
 	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
@@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[])
 			/* Read in mask directly */
 			set_mask = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("BAD set_mask VAL");
 			break;
 
 		case 'a':
@@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[])
 		case 'p':
 			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad PWM value provided");
+				usage("BAD PWM VAL");
 			break;
 		case 'r':
 			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alternative rate provided");
+				usage("BAD rate VAL");
 			break;
 		default:
 			break;
@@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[])
 	}
 
 	if (print_verbose && set_mask > 0) {
-		warnx("Chose channels: ");
+		warnx("Channels: ");
 		printf("    ");
 		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
 			if (set_mask & 1<<i)
@@ -364,7 +364,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
 
@@ -383,7 +383,7 @@ pwm_main(int argc, char *argv[])
 
 			ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
+				errx(ret, "BAD input VAL");
 		}
 		exit(0);
 
@@ -393,7 +393,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		/* get current servo values */
 		struct pwm_output_values last_spos;
@@ -487,7 +487,7 @@ pwm_main(int argc, char *argv[])
 			steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
 			steps_timing_index++ ) {
 
-			warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+			warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
 
 			while (1) {
 				for (unsigned i = 0; i < servo_count; i++) {
@@ -526,7 +526,7 @@ pwm_main(int argc, char *argv[])
 												err(1, "PWM_SERVO_SET(%d)", i);
 										}
 									}
-						warnx("Key pressed, user abort\n");
+						warnx("User abort\n");
 						exit(0);
 					}
 				}
@@ -675,7 +675,7 @@ pwm_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
+	usage(NULL);
 	return 0;
 }
 
diff --git a/src/systemcmds/reflect/module.mk b/src/systemcmds/reflect/module.mk
new file mode 100644
index 000000000..973eb775d
--- /dev/null
+++ b/src/systemcmds/reflect/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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
+# 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.
+#
+############################################################################
+
+#
+# Dump file utility
+#
+
+MODULE_COMMAND	 = reflect
+SRCS		 = reflect.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c
new file mode 100644
index 000000000..6bb53c71a
--- /dev/null
+++ b/src/systemcmds/reflect/reflect.c
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 Andrew Tridgell. 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 reflect.c
+ *
+ * simple data reflector for load testing terminals (especially USB)
+ *
+ * @author Andrew Tridgell
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <systemlib/err.h>
+
+__EXPORT int reflect_main(int argc, char *argv[]);
+
+// memory corruption checking
+#define MAX_BLOCKS 1000
+static uint32_t nblocks;
+struct block {
+    uint32_t v[256];
+};
+static struct block *blocks[MAX_BLOCKS];
+
+#define VALUE(i) ((i*7) ^ 0xDEADBEEF)
+
+static void allocate_blocks(void)
+{
+    while (nblocks < MAX_BLOCKS) {
+        blocks[nblocks] = calloc(1, sizeof(struct block));
+        if (blocks[nblocks] == NULL) {
+            break;
+        }
+        for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
+            blocks[nblocks]->v[i] = VALUE(i);
+        }
+        nblocks++;
+    }
+    printf("Allocated %u blocks\n", nblocks);
+}
+
+static void check_blocks(void)
+{
+    for (uint32_t n=0; n<nblocks; n++) {
+        for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
+            assert(blocks[n]->v[i] == VALUE(i));
+        }
+    }
+}
+
+int
+reflect_main(int argc, char *argv[])
+{
+    uint32_t total = 0;
+    printf("Starting reflector\n");
+
+    allocate_blocks();
+
+    while (true) {
+        char buf[128];
+        ssize_t n = read(0, buf, sizeof(buf));
+        if (n < 0) {
+            break;
+        }
+        if (n > 0) {
+            write(1, buf, n);
+        }
+        total += n;
+        if (total > 1024000) {
+            check_blocks();
+            total = 0;
+        }
+    }
+    return OK;
+}
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 9ae080ee2..2ead3e632 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -44,14 +44,16 @@
 #include <string.h>
 #include <version/version.h>
 #include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
 
-// string constants for version commands
+/* string constants for version commands */
 static const char sz_ver_hw_str[] 	= "hw";
 static const char sz_ver_hwcmp_str[] = "hwcmp";
 static const char sz_ver_git_str[] 	= "git";
 static const char sz_ver_bdate_str[] = "bdate";
 static const char sz_ver_gcc_str[] 	= "gcc";
 static const char sz_ver_all_str[] 	= "all";
+static const char mcu_ver_str[]		= "mcu";
 
 static void usage(const char *reason)
 {
@@ -59,55 +61,87 @@ static void usage(const char *reason)
 		printf("%s\n", reason);
 	}
 
-	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
 }
 
 __EXPORT int ver_main(int argc, char *argv[]);
 
 int ver_main(int argc, char *argv[])
 {
-	int ret = 1;	//defaults to an error
+	/* defaults to an error */
+	int ret = 1;
 
-	// first check if there are at least 2 params
+	/* first check if there are at least 2 params */
 	if (argc >= 2) {
 		if (argv[1] != NULL) {
-			if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
-				printf("%s\n", HW_ARCH);
-				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+			if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
 				if (argc >= 3 && argv[2] != NULL) {
-					// compare 3rd parameter with HW_ARCH string, in case of match, return 0
+					/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
 					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
 
 					if (ret == 0) {
 						printf("ver hwcmp match: %s\n", HW_ARCH);
 					}
+					return ret;
 
 				} else {
 					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
 				}
+			}
 
-			} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
-				printf("%s\n", FW_GIT);
-				ret = 0;
+			/* check if we want to show all */
+			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
 
-			} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
-				printf("%s %s\n", __DATE__, __TIME__);
+			if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+				printf("HW arch: %s\n", HW_ARCH);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
-				printf("%s\n", __VERSION__);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+				printf("FW git-hash: %s\n", FW_GIT);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
-				printf("HW arch: %s\n", HW_ARCH);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
 				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
-				printf("FW git-hash: %s\n", FW_GIT);
-				printf("GCC toolchain: %s\n", __VERSION__);
 				ret = 0;
 
-			} else {
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+				printf("Toolchain: %s\n", __VERSION__);
+				ret = 0;
+
+			}
+
+			if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
+
+				char rev;
+				char* revstr;
+
+				int chip_version = mcu_version(&rev, &revstr);
+
+				if (chip_version < 0) {
+					printf("UNKNOWN MCU\n");
+
+				} else {
+					printf("MCU: %s, rev. %c\n", revstr, rev);
+
+					if (chip_version < MCU_REV_STM32F4_REV_3) {
+						printf("\nWARNING   WARNING   WARNING!\n"
+							"Revision %c has a silicon errata\n"
+							"This device can only utilize a maximum of 1MB flash safely!\n"
+							"http://px4.io/help/errata\n\n", rev);
+					}
+				}
+
+				ret = 0;
+			}
+
+			if (ret == 1) {
 				errx(1, "unknown command.\n");
 			}