Faster sensor bus resets on timeouts, massively reworked fixed wing app, tested

15 files changed, 279 insertions, 306 deletions

diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/scripts/rc.standalone
index 72153ef18..7dfd98a16 100644
--- a/ROMFS/scripts/rc.standalone
+++ b/ROMFS/scripts/rc.standalone
@@ -8,7 +8,7 @@ echo "[init] doing standalone PX4FMU startup..."
 #
 # Start the ORB
 #
-#uorb start
+uorb start
 
 #
 # Start the sensors.
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index d3c75f755..1f75ceb4b 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -32,7 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file Driver for the MS5611 barometric pressure sensor connected via I2C
+ * @file ms5611.cpp
+ * Driver for the MS5611 barometric pressure sensor connected via I2C.
  */
 
 #include <nuttx/config.h>
diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile
index 985708ae5..02d4463dd 100644
--- a/apps/fixedwing_control/Makefile
+++ b/apps/fixedwing_control/Makefile
@@ -37,7 +37,7 @@
 
 APPNAME		= fixedwing_control
 PRIORITY	= SCHED_PRIORITY_MAX - 1
-STACKSIZE	= 12288
+STACKSIZE	= 4096
 
 CSRCS		= fixedwing_control.c 
 
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index cc834e957..9faa257ca 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -53,7 +53,17 @@
 #include <drivers/drv_pwm_output.h>
 #include <nuttx/spi.h>
 #include "../mix_and_link/mix_and_link.h"
-#include "fixedwing_control.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/fixedwing_control.h>
+
+#ifndef F_M_PI
+#define F_M_PI ((float)M_PI)
+#endif
 
 __EXPORT int fixedwing_control_main(int argc, char *argv[]);
 
@@ -61,7 +71,6 @@ __EXPORT int fixedwing_control_main(int argc, char *argv[]);
 #define PID_SCALER 0.1f
 #define PID_DERIVMODE_CALC 0
 #define HIL_MODE 32
-#define RAD2DEG ((1.0/180.0)*M_PI)
 #define AUTO -1000
 #define MANUAL 3000
 #define SERVO_MIN 1000
@@ -75,7 +84,6 @@ pthread_t servo_thread;
  * Servo channels function enumerator used for
  * the servo writing part
  */
-
 enum SERVO_CHANNELS_FUNCTION {
 
 	AIL_1    = 0,
@@ -127,9 +135,9 @@ typedef struct {
 
 	/* Next waypoint*/
 
-	float wp_x;
-	float wp_y;
-	float wp_z;
+	double wp_x;
+	double wp_y;
+	double wp_z;
 
 	/* Setpoints */
 
@@ -189,14 +197,6 @@ static float calc_throttle_setpoint(void);
 static float calc_wp_distance(void);
 static void set_plane_mode(void);
 
-/*
- * The control, navigation and servo loop threads
- */
-
-static void *control_loop(void *arg);
-static void *nav_loop(void *arg);
-static void *servo_loop(void *arg);
-
 /****************************************************************************
  * Public Data
  ****************************************************************************/
@@ -210,8 +210,9 @@ float scaler = 1; //M_PI;
  ****************************************************************************/
 
 /**
- *
- * Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component.
+ * Calculates the PID control output given an error.
+ * 
+ * Incorporates PD scaling and low-pass filter for the derivative component.
  *
  * @param error the input error
  * @param error_deriv the derivative of the input error
@@ -241,7 +242,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
 
 	output += error * Kp;
 
-	if ((fabs(Kd) > 0) && (dt > 0)) {
+	if ((fabsf(Kd) > 0) && (dt > 0)) {
 
 		if (PID_DERIVMODE_CALC) {
 			derivative = (error - lerror) / delta_time;
@@ -250,7 +251,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
 			 * discrete low pass filter, cuts out the
 			 * high frequency noise that can drive the controller crazy
 			 */
-			float RC = 1 / (2 * M_PI * fCut);
+			float RC = 1.0 / (2.0f * M_PI * fCut);
 			derivative = lderiv +
 				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
 
@@ -272,7 +273,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
 	output *= scale;
 
 	/* Compute integral component if time has elapsed */
-	if ((fabs(Ki) > 0) && (dt > 0)) {
+	if ((fabsf(Ki) > 0) && (dt > 0)) {
 		integrator 		+= (error * Ki) * scaler * delta_time;
 
 		if (integrator < -imax) {
@@ -331,8 +332,8 @@ static void get_parameters()
 static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
 {
 	plane_data.p = rollspeed - sinf(pitch) * yawspeed;
-	plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed;
-	plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed;
+	plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed;
+	plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed;
 }
 
 /**
@@ -390,13 +391,13 @@ static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, fl
  * @return bearing Bearing error
  *
  */
-
 static float calc_bearing()
 {
-	float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG;
+	float bearing = F_M_PI/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
 
-	if (bearing < 0)
-		bearing += 360;
+	if (bearing < 0.0f) {
+		bearing += 2*F_M_PI;
+	}
 
 	return bearing;
 }
@@ -430,7 +431,6 @@ static float calc_roll_ail()
  *
  * @return Pitch elevators control output (-1,1)
  */
-
 static float calc_pitch_elev()
 {
 	float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER,
@@ -453,10 +453,9 @@ static float calc_pitch_elev()
  * @return Yaw rudder control output (-1,1)
  *
  */
-
 static float calc_yaw_rudder(float hdg)
 {
-	float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
+	float ret = pid((plane_data.yaw - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
 			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
 
 	if (ret < -1)
@@ -481,11 +480,11 @@ static float calc_throttle()
 	float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER,
 			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
 
-	if (ret < 0.2)
-		return 0.2;
+	if (ret < 0.2f)
+		return 0.2f;
 
-	if (ret > 1)
-		return 1;
+	if (ret > 1.0f)
+		return 1.0f;
 
 	return ret;
 }
@@ -542,11 +541,11 @@ static float calc_roll_setpoint()
 	} else {
 		setpoint = calc_bearing() - plane_data.yaw;
 
-		if (setpoint < -35)
-			return -35;
+		if (setpoint < (-35.0f/180.0f)*F_M_PI)
+			return (-35.0f/180.0f)*F_M_PI;
 
-		if (setpoint > 35)
-			return 35;
+		if (setpoint > (35/180.0f)*F_M_PI)
+			return (-35.0f/180.0f)*F_M_PI;
 	}
 
 	return setpoint;
@@ -564,19 +563,19 @@ static float calc_roll_setpoint()
 
 static float calc_pitch_setpoint()
 {
-	float setpoint = 0;
+	float setpoint = 0.0f;
 
 	if (plane_data.mode == TAKEOFF) {
-		setpoint = 35;
+		setpoint = ((35/180.0f)*F_M_PI);
 
 	} else {
-		setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG;
+		setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
 
-		if (setpoint < -35)
-			return -35;
+		if (setpoint < (-35.0f/180.0f)*F_M_PI)
+			return (-35.0f/180.0f)*F_M_PI;
 
-		if (setpoint > 35)
-			return 35;
+		if (setpoint > (35/180.0f)*F_M_PI)
+			return (-35.0f/180.0f)*F_M_PI;
 	}
 
 	return setpoint;
@@ -597,7 +596,7 @@ static float calc_throttle_setpoint()
 
 	// if TAKEOFF full throttle
 	if (plane_data.mode == TAKEOFF) {
-		setpoint = 60;
+		setpoint = 0.6f;
 	}
 
 	// if CRUISE - parameter value
@@ -607,7 +606,7 @@ static float calc_throttle_setpoint()
 
 	// if LAND no throttle
 	if (plane_data.mode == LAND) {
-		setpoint = 0;
+		setpoint = 0.0f;
 	}
 
 	return setpoint;
@@ -623,7 +622,7 @@ static float calc_throttle_setpoint()
 
 static void set_plane_mode()
 {
-	if (plane_data.alt < 10) {
+	if (plane_data.alt < 10.0f) {
 		plane_data.mode = TAKEOFF;
 
 	} else {
@@ -647,27 +646,31 @@ static void set_plane_mode()
 
 int fixedwing_control_main(int argc, char *argv[])
 {
-	/* print text */
-	printf("Fixedwing control started\n");
-	usleep(100000);
-
 	/* default values for arguments */
 	char *fixedwing_uart_name = "/dev/ttyS1";
-	char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n";
+	char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
 
 	/* read arguments */
-	int i;
+	bool verbose = false;
 
-	if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
-		if (argc > i + 1) {
-			fixedwing_uart_name = argv[i + 1];
+	for (int i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
+			if (argc > i + 1) {
+				fixedwing_uart_name = argv[i + 1];
 
-		} else {
-			printf(commandline_usage);
-			return 0;
+			} else {
+				printf(commandline_usage, argv[0]);
+				return 0;
+			}
+		}
+		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+			verbose = true;
 		}
 	}
 
+	/* welcome user */
+	printf("[fixedwing control] started\n");
+
 	/* Set up to publish fixed wing control messages */
 	struct fixedwing_control_s control;
 	int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
@@ -679,6 +682,12 @@ int fixedwing_control_main(int argc, char *argv[])
 	int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct rc_channels_s rc;
 	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	struct vehicle_global_position_setpoint_s global_setpoint;
+	int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+
+	/* Mainloop setup */
+	unsigned int loopcounter = 0;
+	unsigned int failcounter = 0;
 
 	/* Control constants */
 	control_outputs.mode = HIL_MODE;
@@ -692,7 +701,7 @@ int fixedwing_control_main(int argc, char *argv[])
 	fd = open("/dev/pwm_servo", O_RDWR);
 
 	if (fd < 0) {
-		printf("failed opening /dev/pwm_servo\n");
+		printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
 	}
 
 	ioctl(fd, PWM_SERVO_ARM, 0);
@@ -728,8 +737,7 @@ int fixedwing_control_main(int argc, char *argv[])
 	 * Main control, navigation and servo routine
 	 */
 
-	while(1)
-	{
+	while(1) {
 		/*
 		 * DATA Handling
 		 * Fetch current flight data
@@ -738,6 +746,7 @@ int fixedwing_control_main(int argc, char *argv[])
 		/* get position, attitude and rc inputs */
 		// XXX add error checking
 		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+		orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
 		orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
 		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
 
@@ -746,12 +755,12 @@ int fixedwing_control_main(int argc, char *argv[])
 		 */
 
 		/* position values*/
-		plane_data.lat = global_pos.lat / 10000000;
-		plane_data.lon = global_pos.lon / 10000000;
-		plane_data.alt = global_pos.alt / 1000;
-		plane_data.vx = global_pos.vx / 100;
-		plane_data.vy = global_pos.vy / 100;
-		plane_data.vz = global_pos.vz / 100;
+		plane_data.lat = global_pos.lat / 10000000.0;
+		plane_data.lon = global_pos.lon / 10000000.0;
+		plane_data.alt = global_pos.alt / 1000.0f;
+		plane_data.vx = global_pos.vx / 100.0f;
+		plane_data.vy = global_pos.vy / 100.0f;
+		plane_data.vz = global_pos.vz / 100.0f;
 
 		/* attitude values*/
 		plane_data.roll = att.roll;
@@ -766,37 +775,35 @@ int fixedwing_control_main(int argc, char *argv[])
 
 		/* Attitude control part */
 
-//#define MUTE
-#ifndef MUTE
-		/******************************** DEBUG OUTPUT ************************************************************/
-
-		printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
-				(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
-				(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
-				(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
+		if (verbose && loopcounter % 20 == 0) {
+			/******************************** DEBUG OUTPUT ************************************************************/
 
-//		printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
-//		printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
-//		printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
+			printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
+					(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
+					(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
+					(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
 
-//		printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
+	//		printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
+	//		printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
+	//		printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
 
-//		printf("Current Altitude: %i\n\n", (int)plane_data.alt);
+	//		printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
 
-		printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
-				(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
-				(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
+	//		printf("Current Altitude: %i\n\n", (int)plane_data.alt);
 
-//		printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
-//				(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
+			printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
+					(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
+					(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
 
-		printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
-				(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
-				(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
+	//		printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
+	//				(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
 
-		/************************************************************************************************************/
+			printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
+					(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
+					(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
 
-#endif
+			/************************************************************************************************************/
+		}
 
 		/*
 		 * Computation section
@@ -809,11 +816,11 @@ int fixedwing_control_main(int argc, char *argv[])
 		set_plane_mode();
 
 		/* Calculate the P,Q,R body rates of the aircraft */
-		calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
+		calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
 				plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
 
 		/* Calculate the body frame angles of the aircraft */
-		//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);
+		//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
 
 		/* Calculate the output values */
 		control_outputs.roll_ailerons = calc_roll_ail();
@@ -869,9 +876,6 @@ int fixedwing_control_main(int argc, char *argv[])
 
 			//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
 
-			// 10 Hz loop
-			usleep(100000);
-
 		} else {
 			control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale;
 			control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale;
@@ -927,9 +931,14 @@ int fixedwing_control_main(int argc, char *argv[])
 		int result = write(fd, &data, sizeof(data));
 
 		if (result != sizeof(data)) {
-			printf("failed writing servo outputs\n");
+			if (failcounter < 10 || failcounter % 20 == 0) {
+				printf("[fixedwing_control] failed writing servo outputs\n");
+			}
+			failcounter++;
 		}
 
+		loopcounter++;
+
 		/* 20Hz loop*/
 		usleep(50000);
 	}
diff --git a/apps/fixedwing_control/fixedwing_control.h b/apps/fixedwing_control/fixedwing_control.h
deleted file mode 100644
index 6023e3967..000000000
--- a/apps/fixedwing_control/fixedwing_control.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Ivan Ovinnikov <oivan@ethz.ch>
- *
- * 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 Definition of attitude controller
- */
-
-#ifndef FIXEDWING_CONTROL_H_
-#define FIXEDWING_CONTROL_H_
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/fixedwing_control.h>
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Internal definitions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-#endif /* FIXEDWING_CONTROL_H_ */
diff --git a/apps/fixedwing_control/pid.c b/apps/fixedwing_control/pid.c
index 81b4deac1..652e4143f 100644
--- a/apps/fixedwing_control/pid.c
+++ b/apps/fixedwing_control/pid.c
@@ -1,8 +1,7 @@
 /****************************************************************************
- * pid.c
  *
- *   Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
- *   Authors: Ivan Ovinnikov <oivan@ethz.ch>
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -14,7 +13,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -33,9 +32,10 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file pid.c
+ * Implementation of a fixed wing attitude and position controller.
+ */
 
 #include "pid.h"
 #include "fixedwing_control.h"
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 466ea7565..e3c8e5fa1 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -213,7 +213,7 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa
 		orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
 	}
 
-	printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x, param6_lon_y, param7_alt_z, param4);
+	//printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
 }
 
 
@@ -375,7 +375,7 @@ static void *receiveloop(void *arg)
 static void *uorb_receiveloop(void *arg)
 {
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink uORB async", getpid());
+	prctl(PR_SET_NAME, "mavlink uORB", getpid());
 
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
@@ -586,7 +586,10 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[6].revents & POLLIN) {
 				/* copy fixed wing control into local buffer */
 				orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
-				// XXX Output fixed wing control
+				/* send control output via MAVLink */
+				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0],
+					fw_control.attitude_control_output[1], fw_control.attitude_control_output[2],
+					fw_control.attitude_control_output[3]);
 			}
 
 			/* --- VEHICLE GLOBAL POSITION --- */
@@ -594,15 +597,15 @@ static void *uorb_receiveloop(void *arg)
 				/* copy global position data into local buffer */
 				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
 				uint64_t timestamp = global_pos.timestamp;
-				int32_t lat = (int32_t)(global_pos.lat * 1e7);
-				int32_t lon = (int32_t)(global_pos.lon * 1e7);
-				int32_t alt = (int32_t)(global_pos.alt * 1e3);
-				int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1e3);
-				int16_t vx = (int16_t)(global_pos.vx * 1e2);
-				int16_t vy = (int16_t)(global_pos.vy * 1e2);
-				int16_t vz = (int16_t)(global_pos.vz * 1e2);
+				int32_t lat = global_pos.lat;
+				int32_t lon = global_pos.lon;
+				int32_t alt = (int32_t)(global_pos.alt*1000);
+				int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
+				int16_t vx = (int16_t)(global_pos.vx * 100.0f);
+				int16_t vy = (int16_t)(global_pos.vy * 100.0f);
+				int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 				/* heading in degrees * 10, from 0 to 36.000) */
-				uint16_t hdg = (global_pos.hdg / M_PI_F) * (180 * 10) + (180 * 10);
+				uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 				mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg);
 			}
diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile
index 358b062c0..cf971ae05 100644
--- a/apps/px4/attitude_estimator_bm/Makefile
+++ b/apps/px4/attitude_estimator_bm/Makefile
@@ -37,9 +37,6 @@
 
 APPNAME		 = attitude_estimator_bm
 PRIORITY	= SCHED_PRIORITY_MAX - 10
-STACKSIZE	= 3000
-
-# XXX this is *horribly* broken
-INCLUDES	= $(TOPDIR)/../mavlink/include/mavlink
+STACKSIZE	= 4096
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index 3c6c8eec3..8551b5e1c 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -33,7 +33,10 @@
  *
  ****************************************************************************/
 
-/* @file Black Magic Attitude Estimator */
+/**
+ * @file attitude_estimator_bm.c
+ * Black Magic Attitude Estimator
+ */
 
 
 
@@ -49,6 +52,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 #include <math.h>
 #include <errno.h>
 
@@ -135,14 +139,22 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 	/* rate-limit raw data updates to 200Hz */
 	//orb_set_interval(sub_raw, 5);
 
+	bool hil_enabled = false;
+	bool publishing = false;
+
 	/* advertise attitude */
 	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	publishing = true;
 
 	struct pollfd fds[] = {
 		{ .fd = sub_raw,   .events = POLLIN },
 	};
 
-	// int paramcounter = 100;
+	/* subscribe to system status */
+	struct vehicle_status_s vstatus = {0};
+	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	unsigned int loopcounter = 0;
 
 	/* Main loop*/
 	while (true) {
@@ -217,8 +229,29 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 		att.R[0][2] = x_n_b.z;
 		// XXX add remaining entries
 
+		if (loopcounter % 250 == 0) {
+			/* Check HIL state */
+			orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+			/* switching from non-HIL to HIL mode */
+			if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
+				hil_enabled = true;
+				publishing = false;
+				close(pub_att);
+
+				/* switching from HIL to non-HIL mode */
+
+			} else if (!publishing && !hil_enabled) {
+				/* advertise the topic and make the initial publication */
+				pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+				hil_enabled = false;
+				publishing = true;
+			}
+		}
+
 		// Broadcast
-		orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+		if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+
+		loopcounter++;
 	}
 
 	/* Should never reach here */
diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp
index c3f14e3a4..3a3ff6407 100644
--- a/apps/px4/px4io/driver/px4io.cpp
+++ b/apps/px4/px4io/driver/px4io.cpp
@@ -32,7 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file Driver for the PX4IO board.
+ * @file px4io.cpp
+ * Driver for the PX4IO board.
  *
  * PX4IO is connected via serial (or possibly some other interface at a later
  * point).
@@ -555,6 +556,6 @@ px4io_main(int argc, char *argv[])
 
 
 
-	printf("need a verb, only support 'start'\n");
+	printf("need a verb, only support 'start' and 'update'\n");
 	return ERROR;
 }
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 44b62f3f9..30a62fa65 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -32,14 +32,10 @@
  ****************************************************************************/
 
 /**
- * @file Top-level logic for the PX4IO module.
+ * @file px4io.c
+ * Top-level logic for the PX4IO module.
  */
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdbool.h>
@@ -58,24 +54,8 @@
 
 #include "px4io.h"
 
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
 __EXPORT int user_start(int argc, char *argv[]);
 
-/****************************************************************************
- * user_start
- ****************************************************************************/
-
 struct sys_state_s 	system_state;
 int			gpio_fd;
 
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index a88361e1a..3343b33f4 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -90,17 +90,14 @@
 #define BAT_VOL_LOWPASS_2 0.01f
 #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
 
-/*PPM Settings*/
+/* PPM Settings */
 #define PPM_MIN 1000
 #define PPM_MAX 2000
-/*Our internal resolution is 10000*/
+/* Internal resolution is 10000 */
 #define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2)
 
 #define PPM_MID (PPM_MIN+PPM_MAX)/2
 
-/****************************************************************************
- * Definitions
- ****************************************************************************/
 static pthread_cond_t sensors_read_ready;
 static pthread_mutex_t sensors_read_ready_mutex;
 
@@ -281,7 +278,8 @@ int sensors_main(int argc, char *argv[])
 	bool baro_healthy = false;
 	bool adc_healthy = false;
 
-	bool hil_enabled = false;
+	bool hil_enabled = false;		/**< HIL is disabled by default	*/
+	bool publishing = false;		/**< the app is not publishing by default, only if HIL is disabled on first run */
 
 	int magcounter = 0;
 	int barocounter = 0;
@@ -318,16 +316,19 @@ int sensors_main(int argc, char *argv[])
 	int16_t acc_offset[3] = {0, 0, 0};
 	int16_t	gyro_offset[3] = {0, 0, 0};
 
+	#pragma pack(push,1)
 	struct adc_msg4_s {
-		uint8_t      am_channel1;               /**< The 8-bit ADC Channel 1 */
-		int32_t      am_data1;                  /**< ADC convert result 1 (4 bytes) */
-		uint8_t      am_channel2;               /**< The 8-bit ADC Channel 2 */
-		int32_t      am_data2;                  /**< ADC convert result 2 (4 bytes) */
-		uint8_t      am_channel3;               /**< The 8-bit ADC Channel 3 */
-		int32_t      am_data3;                  /**< ADC convert result 3 (4 bytes) */
-		uint8_t      am_channel4;               /**< The 8-bit ADC Channel 4 */
-		int32_t      am_data4;                  /**< ADC convert result 4 (4 bytes) */
-	} __attribute__((__packed__));;
+		uint8_t      am_channel1;	/**< The 8-bit ADC Channel 1 */
+		int32_t      am_data1;		/**< ADC convert result 1 (4 bytes) */
+		uint8_t      am_channel2;	/**< The 8-bit ADC Channel 2 */
+		int32_t      am_data2;		/**< ADC convert result 2 (4 bytes) */
+		uint8_t      am_channel3;	/**< The 8-bit ADC Channel 3 */
+		int32_t      am_data3;		/**< ADC convert result 3 (4 bytes) */
+		uint8_t      am_channel4;	/**< The 8-bit ADC Channel 4 */
+		int32_t      am_data4;		/**< ADC convert result 4 (4 bytes) */
+	};
+	#pragma pack(pop)
+
 	struct adc_msg4_s buf_adc;
 	size_t adc_readsize = 1 * sizeof(struct adc_msg4_s);
 
@@ -335,7 +336,7 @@ int sensors_main(int argc, char *argv[])
 	battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
 
 	if (-1.0f == battery_voltage_conversion) {
-		/**< default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
+		/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 		battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
 	}
 
@@ -383,13 +384,14 @@ int sensors_main(int argc, char *argv[])
 
 	/* advertise the topic and make the initial publication */
 	sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
+	publishing = true;
 
 	/* advertise the rc topic */
 	struct rc_channels_s rc = {0};
 	int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 	/* subscribe to system status */
-	struct vehicle_status_s vstatus;
+	struct vehicle_status_s vstatus = {0};
 	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 
@@ -430,14 +432,16 @@ int sensors_main(int argc, char *argv[])
 				/* switching from non-HIL to HIL mode */
 				if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
 					hil_enabled = true;
+					publishing = false;
 					close(sensor_pub);
 
 					/* switching from HIL to non-HIL mode */
 
-				} else if (!(vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && hil_enabled) {
+				} else if (!publishing && !hil_enabled) {
 					/* advertise the topic and make the initial publication */
 					sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 					hil_enabled = false;
+					publishing = true;
 				}
 
 
@@ -556,7 +560,7 @@ int sensors_main(int argc, char *argv[])
 			if (acctime > 500) printf("ACC: %d us\n", acctime);
 
 			/* MAGNETOMETER */
-			if (magcounter == 4) { //(magcounter == 4) // 100 Hz
+			if (magcounter == 4) { /* 120 Hz */
 				uint64_t start_mag = hrt_absolute_time();
 				ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
 				int errcode_mag = (int) * get_errno_ptr();
@@ -604,7 +608,7 @@ int sensors_main(int argc, char *argv[])
 			magcounter++;
 
 			/* BAROMETER */
-			if (barocounter == 5 && (fd_barometer > 0)) { //(barocounter == 4) // 100 Hz
+			if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */
 				uint64_t start_baro = hrt_absolute_time();
 				*get_errno_ptr() = 0;
 				ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer));
@@ -798,7 +802,7 @@ int sensors_main(int argc, char *argv[])
 			uint64_t total_time = hrt_absolute_time() - current_time;
 
 			/* Inform other processes that new data is available to copy */
-			if ((gyro_updated || acc_updated || magn_updated || baro_updated) && !hil_enabled) {
+			if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) {
 				/* Values changed, publish */
 				orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
 			}
diff --git a/nuttx/configs/px4fmu/include/nsh_romfsimg.h b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
index 799670c4d..9965277c7 100644
--- a/nuttx/configs/px4fmu/include/nsh_romfsimg.h
+++ b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
@@ -234,7 +234,7 @@ unsigned char romfs_img[] = {
   0x69, 0x73, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x46, 0x41, 0x49, 0x4c,
   0x45, 0x44, 0x2e, 0x22, 0x0a, 0x23, 0x09, 0x72, 0x65, 0x62, 0x6f, 0x6f,
   0x74, 0x0a, 0x23, 0x66, 0x69, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xf2,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcd, 0x52, 0xce, 0xe0, 0xfc,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcc, 0x52, 0xce, 0xe0, 0xfd,
   0x72, 0x63, 0x2e, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e,
   0x65, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
   0x23, 0x20, 0x46, 0x6c, 0x69, 0x67, 0x68, 0x74, 0x20, 0x73, 0x74, 0x61,
@@ -248,76 +248,76 @@ unsigned char romfs_img[] = {
   0x34, 0x46, 0x4d, 0x55, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70,
   0x2e, 0x2e, 0x2e, 0x22, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
   0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x4f, 0x52, 0x42, 0x0a,
-  0x23, 0x0a, 0x23, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72,
-  0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
-  0x20, 0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73,
-  0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63,
-  0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73,
-  0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
-  0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e,
-  0x6b, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e,
-  0x6b, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74,
-  0x79, 0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30,
-  0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72,
-  0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e,
-  0x64, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
-  0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64,
-  0x20, 0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
-  0x65, 0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23,
-  0x0a, 0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20,
+  0x23, 0x0a, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
+  0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
+  0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x2e,
+  0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f,
+  0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73, 0x65,
+  0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
+  0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e, 0x6b,
+  0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e, 0x6b,
+  0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74, 0x79,
+  0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30, 0x20,
   0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
-  0x20, 0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64,
-  0x65, 0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a,
-  0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73,
-  0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27,
-  0x3c, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74,
-  0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74,
-  0x69, 0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
-  0x74, 0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f,
-  0x73, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d,
-  0x61, 0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
-  0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69,
-  0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e,
-  0x74, 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23,
-  0x20, 0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
-  0x74, 0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b,
-  0x69, 0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66,
-  0x69, 0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f,
-  0x20, 0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68,
-  0x65, 0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f,
-  0x61, 0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
-  0x67, 0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69,
-  0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65,
-  0x3f, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68,
-  0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65,
-  0x20, 0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f,
-  0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72,
-  0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64,
-  0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c,
-  0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66,
-  0x69, 0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f,
-  0x72, 0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65,
-  0x20, 0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58,
-  0x58, 0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f,
-  0x0a, 0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73,
-  0x74, 0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
-  0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20,
-  0x66, 0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23,
+  0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
+  0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20,
+  0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
+  0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65,
+  0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a,
+  0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20, 0x26,
+  0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
+  0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64, 0x65,
+  0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a, 0x23,
   0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20,
-  0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e,
-  0x65, 0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61,
-  0x63, 0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23,
-  0x0a, 0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65,
-  0x76, 0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61,
-  0x6c, 0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
-  0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20,
-  0x74, 0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69,
-  0x66, 0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73,
-  0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e,
-  0x64, 0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67,
-  0x0a, 0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69,
-  0x74, 0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64,
-  0x6f, 0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
+  0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27, 0x3c,
+  0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74, 0x61,
+  0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74, 0x69,
+  0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74,
+  0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f, 0x73,
+  0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
+  0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
+  0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69, 0x78,
+  0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e, 0x74,
+  0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20,
+  0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x74,
+  0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69,
+  0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
+  0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f, 0x20,
+  0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68, 0x65,
+  0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f, 0x61,
+  0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x67,
+  0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69, 0x78,
+  0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65, 0x3f,
+  0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69,
+  0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20,
+  0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63,
+  0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
+  0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77,
+  0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20,
+  0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66, 0x69,
+  0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f, 0x72,
+  0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, 0x20,
+  0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
+  0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f, 0x0a,
+  0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73, 0x74,
+  0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
+  0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20, 0x66,
+  0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23, 0x0a,
+  0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73,
+  0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e, 0x65,
+  0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61, 0x63,
+  0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23, 0x0a,
+  0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76,
+  0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61, 0x6c,
+  0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
+  0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20, 0x74,
+  0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69, 0x66,
+  0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73, 0x68,
+  0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64,
+  0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x0a,
+  0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69, 0x74,
+  0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64, 0x6f,
+  0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x76, 0x8d, 0x9c, 0xa3, 0x80,
   0x72, 0x63, 0x53, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
index 0e6dbe2ac..2a6d04306 100644
--- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c
+++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
@@ -204,7 +204,7 @@ read_values(int16_t *data)
 			int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
 			if (hmc_status < 0)
 			  {
-			  	if (ret == ETIMEDOUT) hmc5883l_reset();
+			  	if (hmc_status == ETIMEDOUT) hmc5883l_reset();
 			  	ret = hmc_status;
 			  }
 			else
diff --git a/nuttx/configs/px4fmu/src/drv_ms5611.c b/nuttx/configs/px4fmu/src/drv_ms5611.c
index 7fea65159..c7e91c5ea 100644
--- a/nuttx/configs/px4fmu/src/drv_ms5611.c
+++ b/nuttx/configs/px4fmu/src/drv_ms5611.c
@@ -123,6 +123,16 @@ static FAR struct {
 
 static int ms5611_read_prom(void);
 
+int ms5611_reset()
+{
+	int ret;
+	printf("[ms5611 drv] Resettet I2C2 BUS\n");
+	up_i2cuninitialize(ms5611_dev.i2c);
+	ms5611_dev.i2c = up_i2cinitialize(2);
+	I2C_SETFREQUENCY(ms5611_dev.i2c, 400000);
+	return ret;
+}
+
 static bool
 read_values(float *data)
 {
@@ -279,6 +289,7 @@ read_values(float *data)
 	else
 	{
 		errno = -ret;
+		if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset();
 		return ret;
 	}
 }