Merge branch 'master' of github.com:PX4/Firmware

20 files changed, 391 insertions, 160 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index a865992ed..af88684bb 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -191,12 +191,26 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
 
 	bool motor_test_mode = false;
+	int test_motor = -1;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
 		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
 			motor_test_mode = true;
 		}
+
+		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
+			if (i+1 < argc) {
+				int motor = atoi(argv[i+1]);
+				if (motor > 0 && motor < 5) {
+					test_motor = motor;
+				} else {
+					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
+				}
+			} else {
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
 	}
 
 	struct termios uart_config_original;
@@ -205,18 +219,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
 	}
 
-	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(&uart_config_original);
-
-	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
-	gpios = ar_multiplexing_init();
-
-	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
-		thread_running = false;
-		exit(ERROR);
-	}
-
 	/* Led animation */
 	int counter = 0;
 	int led_counter = 0;
@@ -237,6 +239,18 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	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(&uart_config_original);
+
+	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
+	gpios = ar_multiplexing_init();
+
+	if (ardrone_write < 0) {
+		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
@@ -253,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	/* close uarts */
 	close(ardrone_write);
-	ar_multiplexing_deinit(gpios);
+	//ar_multiplexing_deinit(gpios);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(&uart_config_original);
@@ -279,7 +293,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 		if (motor_test_mode) {
 			/* set motors to idle speed */
-			ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
+			if (test_motor > 0 && test_motor < 5) {
+				int motors[4] = {0, 0, 0, 0};
+				motors[test_motor - 1] = 10;
+				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
+			} else {
+				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
+			}
+
 		} else {
 			/* MAIN OPERATION MODE */
 
@@ -289,7 +310,10 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 			
-			if (armed.armed) {
+			/* for now only spin if armed and immediately shut down
+			 * if in failsafe
+			 */
+			if (armed.armed && !armed.failsafe) {
 				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
 			} else {
 				/* Silently lock down motor speeds to zero */
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 380ae9c15..dd7a5655d 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -43,6 +43,8 @@
 #include <unistd.h>
 #include <drivers/drv_gpio.h>
 #include <arch/board/up_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_outputs.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -224,9 +226,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	for (i = 1; i < 5; ++i) {
 		/* Initialize motors 1-4 */
 		errcounter += ar_select_motor(gpios, i);
-
-		write(ardrone_uart, &(initbuf[3]), 1);
-		fsync(ardrone_uart);
+		usleep(200);
 
 		/*
 		 * write 0xE0 - request status
@@ -270,29 +270,34 @@ int ar_init_motors(int ardrone_uart, int gpios)
 		usleep(UART_TRANSFER_TIME_BYTE_US*11);
 
 		ar_deselect_motor(gpios, i);
-		/* sleep 400 ms */
-		usleep(400000);
+		/* sleep 200 ms */
+		usleep(200000);
 	}
 
 	/* start the multicast part */
 	errcounter += ar_select_motor(gpios, 0);
+	usleep(200);
 
 	/*
+	 * first round
 	 * write six times A0 - enable broadcast
 	 * receive nothing
 	 */
-	// for (int i = 0; i < sizeof(multicastbuf); i++) {
-	write(ardrone_uart, multicastbuf, 6);
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
 	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
 
-	usleep(200000);
-
-	write(ardrone_uart, multicastbuf, 6);
+	/*
+	 * second round
+	 * write six times A0 - enable broadcast
+	 * receive nothing
+	 */
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
 	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
 
-	/* set motors to zero speed */
+	/* set motors to zero speed (fsync is part of the write command */
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
-	fsync(ardrone_uart);
 
 	if (errcounter != 0) {
 		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@@ -324,12 +329,27 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
 int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
 	const unsigned int min_motor_interval = 4900;
 	static uint64_t last_motor_time = 0;
+
+	static struct actuator_outputs_s outputs;
+	outputs.output[0] = motor1;
+	outputs.output[1] = motor2;
+	outputs.output[2] = motor3;
+	outputs.output[3] = motor4;
+	static orb_advert_t pub = 0;
+	if (pub == 0) {
+		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	}
+
 	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
 		uint8_t buf[5] = {0};
 		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
 		int ret;
 		ret = write(ardrone_fd, buf, sizeof(buf));
 		fsync(ardrone_fd);
+
+		/* publish just written values */
+		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+
 		if (ret == sizeof(buf)) {
 			return OK;
 		} else {
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 80d2c58f8..022003ee5 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -69,6 +69,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
 #include <mavlink/mavlink_log.h>
  
 #include <systemlib/param/param.h>
@@ -425,9 +426,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	} else {
 		/* announce and set new offset */
 
-		char offset_output[50];
-		sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
-		mavlink_log_info(mavlink_fd, offset_output);
+		// char offset_output[50];
+		// sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
+		// mavlink_log_info(mavlink_fd, offset_output);
 
 		if (param_set(param_find("SENSOR_MAG_XOFF"), &(mag_offset[0]))) {
 			fprintf(stderr, "[commander] Setting X mag offset failed!\n");
@@ -505,9 +506,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 	status->flag_preflight_gyro_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
-	char offset_output[50];
-	sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-	mavlink_log_info(mavlink_fd, offset_output);
+	// char offset_output[50];
+	// sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+	// mavlink_log_info(mavlink_fd, offset_output);
 
 	close(sub_sensor_combined);
 }
@@ -573,10 +574,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	status->flag_preflight_accel_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
-	char offset_output[50];
-	sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
-		(double)accel_offset[1], (double)accel_offset[2]);
-	mavlink_log_info(mavlink_fd, offset_output);
+	// char offset_output[50];
+	// sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
+	// 	(double)accel_offset[1], (double)accel_offset[2]);
+	// mavlink_log_info(mavlink_fd, offset_output);
 
 	close(sub_sensor_combined);
 }
@@ -972,7 +973,7 @@ int commander_thread_main(int argc, char *argv[])
 	uint8_t flight_env;
 
 	/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
-	float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
+	float battery_voltage = 12.0f;
 	bool battery_voltage_valid = true;
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
@@ -1009,6 +1010,8 @@ int commander_thread_main(int argc, char *argv[])
 	/* now initialized */
 	commander_initialized = true;
 
+	uint64_t start_time = hrt_absolute_time();
+
 	while (!thread_should_exit) {
 
 		/* Get current values */
@@ -1018,7 +1021,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		battery_voltage = sensors.battery_voltage_v;
 		battery_voltage_valid = sensors.battery_voltage_valid;
-		bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+
+		/*
+		 * Only update battery voltage estimate if voltage is
+		 * valid and system has been running for two and a half seconds
+		 */
+		if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
+			bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+		}
 
 		/* Slow but important 8 Hz checks */
 		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
@@ -1083,6 +1093,38 @@ int commander_thread_main(int argc, char *argv[])
 			ioctl(buzzer, TONE_SET_ALARM, 0);
 		}
 
+		/* Check battery voltage */
+		/* write to sys_status */
+		current_status.voltage_battery = battery_voltage;
+
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
+		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+
+			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				low_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
+			}
+
+			low_voltage_counter++;
+		}
+
+		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
+		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
+				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+			}
+
+			critical_voltage_counter++;
+
+		} else {
+			low_voltage_counter = 0;
+			critical_voltage_counter = 0;
+		}
+
+		/* End battery voltage check */
+
 		/* Check if last transition deserved an audio event */
 #warning This code depends on state that is no longer? maintained
 #if 0
@@ -1141,39 +1183,9 @@ int commander_thread_main(int argc, char *argv[])
 		//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
 		/* end: check gps */
 
-		/* Check battery voltage */
-		/* write to sys_status */
-		current_status.voltage_battery = battery_voltage;
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
 
 		/* Start RC state check */
+		bool prev_lost = current_status.rc_signal_lost;
 
 		if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
 
@@ -1238,10 +1250,18 @@ int commander_thread_main(int argc, char *argv[])
 			/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
 			current_status.rc_signal_cutting_off = true;
 			current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
+
 			/* if the RC signal is gone for a full second, consider it lost */
 			if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
 		}
 
+		/* Check if this is the first loss or first gain*/
+		if ((!prev_lost && current_status.rc_signal_lost) ||
+			prev_lost && !current_status.rc_signal_lost) {
+			/* publish rc lost */
+			publish_armed_status(&current_status);
+		}
+
 		/* End mode switch */
 
 		/* END RC state check */
diff --git a/apps/commander/commander.h b/apps/commander/commander.h
index d537a0602..e9e3b24ca 100644
--- a/apps/commander/commander.h
+++ b/apps/commander/commander.h
@@ -39,8 +39,6 @@
 #ifndef COMMANDER_H_
 #define COMMANDER_H_
 
-#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f
-#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f
 #define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
 #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
 #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 4e2166a3a..51ed95114 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 	if (invalid_state == false || old_state != new_state) {
 		current_status->state_machine = new_state;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
-		struct actuator_armed_s armed;
-		armed.armed = current_status->flag_system_armed;
-		orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-		orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		publish_armed_status(current_status);
 		ret = OK;
 	}
 	if (invalid_state) {
@@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
+void publish_armed_status(const struct vehicle_status_s *current_status) {
+	struct actuator_armed_s armed;
+	armed.armed = current_status->flag_system_armed;
+	armed.failsafe = current_status->rc_signal_lost;
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+}
+
 
 /*
  * Private functions, update the state machine
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 4908c799a..c4d1b78a5 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
  */
 void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
+/**
+ * Publish the armed state depending on the current system state
+ *
+ * @param current_status the current system status
+ */
+void publish_armed_status(const struct vehicle_status_s *current_status);
 
 
 
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 1f05a3343..da3b83777 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -612,8 +612,8 @@ HMC5883::collect()
 #pragma pack(push, 1)
 	struct { /* status register and data as read back from the device */
 		uint8_t		x[2];
-		uint8_t		y[2];
 		uint8_t		z[2];
+		uint8_t		y[2];
 	}	hmc_report;
 #pragma pack(pop)
 	struct {
@@ -833,7 +833,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("measurement: %.6f  %.6f  %.6f", report.x, report.y, report.z);
+	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
 	/* set the queue depth to 10 */
@@ -863,7 +863,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("measurement: %.6f  %.6f  %.6f", report.x, report.y, report.z);
+		warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 		warnx("time:        %lld", report.timestamp);
 	}
 
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 04ba4cbbb..7b8a84f7e 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -369,8 +369,10 @@ MPU6000::init()
 	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
 	usleep(1000);
 
-	// FS & DLPF   FS=2000 deg/s, DLPF = 98Hz (low pass filter)
-	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
+	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
+	// was 90 Hz, but this ruins quality and does not improve the
+	// system response
+	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -1120,5 +1122,5 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "info"))
 		mpu6000::info();
 
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index ef0eab130..57c459430 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -75,6 +75,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/debug_key_value.h>
 #include <systemlib/param/param.h>
 
 #include "waypoints.h"
@@ -166,6 +167,7 @@ static struct mavlink_subscriptions {
 	int spa_sub;
 	int spl_sub;
 	int spg_sub;
+	int debug_key_value;
 	bool initialized;
 } mavlink_subs = {
 	.sensor_sub = 0,
@@ -183,6 +185,7 @@ static struct mavlink_subscriptions {
 	.spa_sub = 0,
 	.spl_sub = 0,
 	.spg_sub = 0,
+	.debug_key_value = 0,
 	.initialized = false
 };
 
@@ -539,10 +542,15 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
 			if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
 			if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
 			if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
+			if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_MANUAL_CONTROL:
 			/* manual_control_setpoint triggers this message */
 			if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
+			break;
+		case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
+			if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval);
+			break;
 		default:
 			/* not found */
 			ret = ERROR;
@@ -588,6 +596,7 @@ static void *uorb_receiveloop(void *arg)
 		struct actuator_outputs_s act_outputs;
 		struct manual_control_setpoint_s man_control;
 		struct actuator_controls_s actuators;
+		struct debug_key_value_s debug;
 	} buf;
 
 	/* --- SENSORS RAW VALUE --- */
@@ -704,11 +713,17 @@ static void *uorb_receiveloop(void *arg)
 	/* --- ACTUATOR CONTROL VALUE --- */
 	/* subscribe to ORB for actuator control */
 	subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(subs->actuators_sub, 2000);	/* 0.5 Hz updates */
 	fds[fdsc_count].fd = subs->actuators_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- DEBUG VALUE OUTPUT --- */
+	/* subscribe to ORB for debug value outputs */
+	subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
+	fds[fdsc_count].fd = subs->debug_key_value;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* all subscriptions initialized, return success */
 	subs->initialized = true;
 
@@ -755,6 +770,38 @@ static void *uorb_receiveloop(void *arg)
 				/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */
 				//mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
 				/* send scaled imu data */
+
+				/* mark individual fields as changed */
+				uint16_t fields_updated = 0;
+				static unsigned accel_counter = 0;
+				static unsigned gyro_counter = 0;
+				static unsigned mag_counter = 0;
+				static unsigned baro_counter = 0;
+
+				if (accel_counter != buf.raw.accelerometer_counter) {
+					/* mark first three dimensions as changed */
+					fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+					accel_counter = buf.raw.accelerometer_counter;
+				}
+
+				if (gyro_counter != buf.raw.gyro_counter) {
+					/* mark first three dimensions as changed */
+					fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+					gyro_counter = buf.raw.gyro_counter;
+				}
+
+				if (mag_counter != buf.raw.magnetometer_counter) {
+					/* mark first three dimensions as changed */
+					fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+					mag_counter = buf.raw.magnetometer_counter;
+				}
+
+				if (baro_counter != buf.raw.baro_counter) {
+					/* mark first three dimensions as changed */
+					fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+					baro_counter = buf.raw.baro_counter;
+				}
+
 				mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp,
 					buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1],
 					buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0],
@@ -762,7 +809,8 @@ static void *uorb_receiveloop(void *arg)
 					buf.raw.magnetometer_ga[0],
 					buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2],
 					buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */,
-					buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius);
+					buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius,
+					fields_updated);
 				/* send pressure */
 				//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100);
 
@@ -895,32 +943,15 @@ static void *uorb_receiveloop(void *arg)
 				/* Only send in HIL mode */
 				if (mavlink_hil_enabled) {
 
-					/* hacked HIL implementation in order for the APM Planner to work
-					 * (correct cmd: mavlink_msg_hil_controls_send())
-					 */
-
-					mavlink_msg_rc_channels_scaled_send(chan,
-									    hrt_absolute_time(),
-									    0, // port 0
-									    buf.att_sp.roll_body,
-									    buf.att_sp.pitch_body,
-									    buf.att_sp.thrust,
-									    buf.att_sp.yaw_body,
-									    0,
-									    0,
-									    0,
-									    0,
-									    1 /*rssi=1*/);
-
 					/* translate the current syste state to mavlink state and mode */
 					uint8_t mavlink_state = 0;
 					uint8_t mavlink_mode = 0;
 					get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
 
-					/* correct HIL message as per MAVLink spec */
+					/* HIL message as per MAVLink spec */
 					mavlink_msg_hil_controls_send(chan,
 										hrt_absolute_time(),
-										buf.att_sp.roll_body,
+										buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
 										buf.att_sp.pitch_body,
 										buf.att_sp.yaw_body,
 										buf.att_sp.thrust,
@@ -1060,10 +1091,17 @@ static void *uorb_receiveloop(void *arg)
 			/* --- ACTUATOR CONTROL --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]);
+				/* send, add spaces so that string buffer is at least 10 chars long */
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0     ", buf.actuators.control[0]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1     ", buf.actuators.control[1]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2     ", buf.actuators.control[2]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3     ", buf.actuators.control[3]);
+			}
+
+			/* --- DEBUG KEY/VALUE --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
 			}
 		}
 	}
@@ -1495,28 +1533,33 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 921600) {
-		/* 500 Hz / 2 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
+		/* 200 Hz / 5 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
 		/* 200 Hz / 5 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
 		/* 5 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
 	} else if (baudrate >= 460800) {
-		/* 250 Hz / 4 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
+		/* 200 Hz / 5 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
+		/* 50 Hz / 10 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
+		/* 20 Hz / 50 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else if (baudrate >= 115200) {
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
+		/* 20 Hz / 50 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
 		/* 10 Hz / 100 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
 		/* 1 Hz */
@@ -1527,6 +1570,8 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
 		/* 5 Hz / 200 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
+		/* 5 Hz / 200 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
 		/* 0.2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
@@ -1535,6 +1580,8 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
+		/* 1 Hz / 1000 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
 		/* 0.5 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
 		/* 0.1 Hz */
diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index 8278837e8..2e15174bb 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -189,7 +189,7 @@ static int mavlink_pm_save_eeprom()
 	int result = param_export(fd, false);
 	close(fd);
 
-	if (result < 0) {
+	if (result != 0) {
 		unlink(mavlink_parameter_file);
 		warn("error exporting parameters to '%s'", mavlink_parameter_file);
 		return -2;
@@ -218,7 +218,7 @@ mavlink_pm_load_eeprom()
 	int result = param_load(fd);
 	close(fd);
 
-	if (result < 0) {
+	if (result != 0) {
 		warn("error reading parameters from '%s'", mavlink_parameter_file);
 		return -2;
 	}
@@ -346,7 +346,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 			}
 
 			/* send back command result */
-			// mavlink_message_t tx;
+			//mavlink_msg_command_ack_send(chan, cmd.command, result);
 		} break;
 	}
 }
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 23373b998..350308fa3 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -144,6 +144,7 @@ mc_thread_main(int argc, char *argv[])
 		att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
 		att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
 		att_sp.yaw_body = -manual.yaw * M_PI_F;
+		att_sp.timestamp = hrt_absolute_time();
 		if (motor_test_mode) {
 			att_sp.roll_body = 0.0f;
 			att_sp.pitch_body = 0.0f;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index c6547b199..312942acb 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -231,7 +231,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
 		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu);
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
+		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
 	}
 
 	/* calculate current control outputs */
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index 0f6b6044f..45267f315 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -226,7 +226,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 			att.timestamp = sensor_combined_s_local.timestamp;
 			att.roll = euler.x;
 			att.pitch = euler.y;
-			att.yaw = euler.z - M_PI_F;
+			att.yaw = euler.z;
 
 			if (att.yaw > M_PI_F) {
 				att.yaw -= 2.0f * M_PI_F;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 79f9c19e7..963c54b8e 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -653,9 +653,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
 		accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
 		accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		raw.accelerometer_counter++;
 
 	} else {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+		bool accel_updated;
+		orb_check(_accel_sub, &accel_updated);
+
+		if (accel_updated) {
+			orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+			raw.accelerometer_counter++;
+		}
 	}
 
 	raw.accelerometer_m_s2[0] = accel_report.x;
@@ -665,8 +672,6 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	raw.accelerometer_raw[0] = accel_report.x_raw;
 	raw.accelerometer_raw[1] = accel_report.y_raw;
 	raw.accelerometer_raw[2] = accel_report.z_raw;
-
-	raw.accelerometer_raw_counter++;
 }
 
 void
@@ -691,9 +696,6 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
 		gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
 
-	} else {
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
-
 		raw.gyro_rad_s[0] = gyro_report.x;
 		raw.gyro_rad_s[1] = gyro_report.y;
 		raw.gyro_rad_s[2] = gyro_report.z;
@@ -702,40 +704,69 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		raw.gyro_raw[1] = gyro_report.y_raw;
 		raw.gyro_raw[2] = gyro_report.z_raw;
 
-		raw.gyro_raw_counter++;		
+		raw.gyro_counter++;
+
+	} else {
+
+		bool gyro_updated;
+		orb_check(_gyro_sub, &gyro_updated);
+
+		if (gyro_updated) {
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
+
+			raw.gyro_rad_s[0] = gyro_report.x;
+			raw.gyro_rad_s[1] = gyro_report.y;
+			raw.gyro_rad_s[2] = gyro_report.z;
+
+			raw.gyro_raw[0] = gyro_report.x_raw;
+			raw.gyro_raw[1] = gyro_report.y_raw;
+			raw.gyro_raw[2] = gyro_report.z_raw;
+
+			raw.gyro_counter++;
+		}
 	}
 }
 
 void
 Sensors::mag_poll(struct sensor_combined_s &raw)
 {
-	struct mag_report	mag_report;
+	bool mag_updated;
+	orb_check(_mag_sub, &mag_updated);
 
-	orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+	if (mag_updated) {
+		struct mag_report	mag_report;
 
-	raw.magnetometer_ga[0] = mag_report.x;
-	raw.magnetometer_ga[1] = mag_report.y;
-	raw.magnetometer_ga[2] = mag_report.z;
+		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-	raw.magnetometer_raw[0] = mag_report.x_raw;
-	raw.magnetometer_raw[1] = mag_report.y_raw;
-	raw.magnetometer_raw[2] = mag_report.z_raw;
-	
-	raw.magnetometer_raw_counter++;
+		raw.magnetometer_ga[0] = mag_report.x;
+		raw.magnetometer_ga[1] = mag_report.y;
+		raw.magnetometer_ga[2] = mag_report.z;
+
+		raw.magnetometer_raw[0] = mag_report.x_raw;
+		raw.magnetometer_raw[1] = mag_report.y_raw;
+		raw.magnetometer_raw[2] = mag_report.z_raw;
+		
+		raw.magnetometer_counter++;
+	}
 }
 
 void
 Sensors::baro_poll(struct sensor_combined_s &raw)
 {
-	struct baro_report	baro_report;
+	bool baro_updated;
+	orb_check(_baro_sub, &baro_updated);
+
+	if (baro_updated) {
+		struct baro_report	baro_report;
 
-	orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
 
-	raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
-	raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
-	raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
+		raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
+		raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
+		raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
 
-	raw.baro_raw_counter++;
+		raw.baro_counter++;
+	}
 }
 
 void
@@ -779,8 +810,6 @@ Sensors::parameter_update_poll(bool forced)
 		struct parameter_update_s update;
 		orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
-		printf("PARAMS UPDATED\n");
-
 		/* update parameters */
 		parameters_update();
 
@@ -824,7 +853,7 @@ Sensors::parameter_update_poll(bool forced)
 			warn("WARNING: failed to set scale / offsets for mag");
 		close(fd);
 
-#if 1
+#if 0
 		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n",  (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor*10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
 		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n",  (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor*10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
 		printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled*100), (int)(_rc.chan[1].scaled*100));
@@ -857,7 +886,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 			/* Voltage in volts */
 			raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
 
-			if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
+			if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
 				raw.battery_voltage_valid = false;
 				raw.battery_voltage_v = 0.f;
 
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 27878401e..46ced013d 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -44,7 +44,7 @@
 #include "tinybson.h"
 
 
-#if 1
+#if 0
 # define debug(fmt, args...)		do { warnx("BSON: " fmt, ##args); } while(0)
 #else
 # define debug(fmt, args...)		do { } while(0)
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 3db55f967..a01c170a6 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -479,7 +479,7 @@ param_export(int fd, bool only_unsaved)
 		 * If we are only saving values changed since last save, and this
 		 * one hasn't, then skip it
 		 */
-		if (only_unsaved & !s->unsaved)
+		if (only_unsaved && !s->unsaved)
 			continue;
 
 		s->unsaved = false;
@@ -519,7 +519,7 @@ param_export(int fd, bool only_unsaved)
 			break;
 
 		default:
-			debug("unrecognised parameter type");
+			debug("unrecognized parameter type");
 			goto out;
 		}
 	}
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index de26dcf01..e40199e97 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -123,3 +123,6 @@ ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
 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/debug_key_value.h"
+ORB_DEFINE(debug_key_value, struct debug_key_value_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index 2b7d7de5d..e6d7c8670 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -67,7 +67,8 @@ ORB_DECLARE(actuator_controls_3);
 
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
-	bool	armed;
+	bool	armed;		/**< Set to true if system is armed */
+	bool	failsafe;	/**< Set to true if no valid control input is available */
 };
 
 ORB_DECLARE(actuator_armed);
diff --git a/apps/uORB/topics/debug_key_value.h b/apps/uORB/topics/debug_key_value.h
new file mode 100644
index 000000000..a9d1b83fd
--- /dev/null
+++ b/apps/uORB/topics/debug_key_value.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.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 debug_key_value.h
+ * Definition of the debug named value uORB topic. Allows to send a 10-char key
+ * with a float as value.
+ */
+
+#ifndef TOPIC_DEBUG_KEY_VALUE_H_
+#define TOPIC_DEBUG_KEY_VALUE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+/**
+ * Key/value pair for debugging
+ */
+struct debug_key_value_s {
+
+	/*
+	 * Actual data, this is specific to the type of data which is stored in this struct
+	 * A line containing L0GME will be added by the Python logging code generator to the
+	 * logged dataset.
+	 */
+	uint32_t timestamp_ms;		/**< in milliseconds since system start */
+	char key[10];				/**< max. 10 characters as key / name */
+	float value;				/**< the value to send as debug output */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(debug_key_value);
+
+#endif
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
index 8fae78858..8c1847753 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/apps/uORB/topics/sensor_combined.h
@@ -80,11 +80,11 @@ struct sensor_combined_s {
 	uint64_t timestamp;			/**< Timestamp in microseconds since boot       LOGME  */
 
 	int16_t	gyro_raw[3];			/**< Raw sensor values of angular velocity      LOGME  */
-	uint16_t gyro_raw_counter;		/**< Number of raw measurments taken            LOGME  */
+	uint16_t gyro_counter;			/**< Number of raw measurments taken            LOGME  */
 	float gyro_rad_s[3];			/**< Angular velocity in radian per seconds     LOGME  */
 	
 	int16_t accelerometer_raw[3];		/**< Raw acceleration in NED body frame         LOGME  */
-	uint16_t accelerometer_raw_counter;	/**< Number of raw acc measurements taken       LOGME  */
+	uint32_t accelerometer_counter;		/**< Number of raw acc measurements taken       LOGME  */
 	float accelerometer_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2   LOGME  */
 	int accelerometer_mode;			/**< Accelerometer measurement mode */
 	float accelerometer_range_m_s2;		/**< Accelerometer measurement range in m/s^2 */
@@ -94,15 +94,15 @@ struct sensor_combined_s {
 	int magnetometer_mode;			/**< Magnetometer measurement mode */
 	float magnetometer_range_ga;		/**< ± measurement range in Gauss */
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
-	uint16_t magnetometer_raw_counter;	/**< Number of raw mag measurements taken       LOGME  */
+	uint32_t magnetometer_counter;		/**< Number of raw mag measurements taken       LOGME  */
 	
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.   LOGME  */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.              LOGME  */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius             LOGME  */
 	float battery_voltage_v;		/**< Battery voltage in volts, filtered         LOGME  */
 	float adc_voltage_v[3];			/**< ADC voltages of ADC Chan 11/12/13 or -1    LOGME  */
-	uint16_t baro_raw_counter;		/**< Number of raw baro measurements taken      LOGME  */
-	uint16_t battery_voltage_counter;	/**< Number of voltage measurements taken       LOGME  */
+	uint32_t baro_counter;			/**< Number of raw baro measurements taken      LOGME  */
+	uint32_t battery_voltage_counter;	/**< Number of voltage measurements taken       LOGME  */
 	bool battery_voltage_valid;		/**< True if battery voltage can be measured    LOGME  */
 
 };