Merge branch 'master' into seatbelt_multirotor

25 files changed, 379 insertions, 71 deletions

diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 43cbe74c7..1aef739c7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -53,10 +53,12 @@
 #include <uORB/topics/vehicle_status.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
+#include <modules/px4iofirmware/protocol.h>
 
 struct gpio_led_s {
 	struct work_s work;
 	int gpio_fd;
+	bool use_io;
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
@@ -75,51 +77,97 @@ void gpio_led_cycle(FAR void *arg);
 
 int gpio_led_main(int argc, char *argv[])
 {
-	int pin = GPIO_EXT_1;
-
 	if (argc < 2) {
-		errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
+		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
+		     "\t-p\tUse pin:\n"
+		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
+		     "\t\t2\tPX4FMU GPIO_EXT2\n"
+		     "\t\ta1\tPX4IO ACC1\n"
+		     "\t\ta2\tPX4IO ACC2\n"
+		     "\t\tr1\tPX4IO RELAY1\n"
+		     "\t\tr2\tPX4IO RELAY2");
 
 	} else {
 
-		/* START COMMAND HANDLING */
 		if (!strcmp(argv[1], "start")) {
+			if (gpio_led_started) {
+				errx(1, "already running");
+			}
+
+			bool use_io = false;
+			int pin = GPIO_EXT_1;
 
 			if (argc > 2) {
-				if (!strcmp(argv[1], "-p")) {
-					if (!strcmp(argv[2], "1")) {
+				if (!strcmp(argv[2], "-p")) {
+					if (!strcmp(argv[3], "1")) {
+						use_io = false;
 						pin = GPIO_EXT_1;
 
-					} else if (!strcmp(argv[2], "2")) {
+					} else if (!strcmp(argv[3], "2")) {
+						use_io = false;
 						pin = GPIO_EXT_2;
 
+					} else if (!strcmp(argv[3], "a1")) {
+						use_io = true;
+						pin = PX4IO_ACC1;
+
+					} else if (!strcmp(argv[3], "a2")) {
+						use_io = true;
+						pin = PX4IO_ACC2;
+
+					} else if (!strcmp(argv[3], "r1")) {
+						use_io = true;
+						pin = PX4IO_RELAY1;
+
+					} else if (!strcmp(argv[3], "r2")) {
+						use_io = true;
+						pin = PX4IO_RELAY2;
+
 					} else {
-						warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
-						exit(1);
+						errx(1, "unsupported pin: %s", argv[3]);
 					}
 				}
 			}
 
 			memset(&gpio_led_data, 0, sizeof(gpio_led_data));
+			gpio_led_data.use_io = use_io;
 			gpio_led_data.pin = pin;
 			int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
 
 			if (ret != 0) {
-				warnx("[gpio_led] Failed to queue work: %d\n", ret);
-				exit(1);
+				errx(1, "failed to queue work: %d", ret);
 
 			} else {
 				gpio_led_started = true;
+				char pin_name[24];
+
+				if (use_io) {
+					if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
+
+					} else {
+						sprintf(pin_name, "PX4IO RELAY%i", pin);
+					}
+
+				} else {
+					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
+
+				}
+
+				warnx("start, using pin: %s", pin_name);
 			}
 
 			exit(0);
 
-			/* STOP COMMAND HANDLING */
 
 		} else if (!strcmp(argv[1], "stop")) {
-			gpio_led_started = false;
+			if (gpio_led_started) {
+				gpio_led_started = false;
+				warnx("stop");
 
-			/* INVALID COMMAND */
+			} else {
+				errx(1, "not running");
+			}
 
 		} else {
 			errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@@ -131,15 +179,26 @@ void gpio_led_start(FAR void *arg)
 {
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
+	char *gpio_dev;
+
+	if (priv->use_io) {
+		gpio_dev = PX4IO_DEVICE_PATH;
+	} else {
+		gpio_dev = PX4FMU_DEVICE_PATH;
+	}
+
 	/* open GPIO device */
-	priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
+	priv->gpio_fd = open(gpio_dev, 0);
 
 	if (priv->gpio_fd < 0) {
-		warnx("[gpio_led] GPIO: open fail\n");
+		// TODO find way to print errors
+		//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
+		gpio_led_started = false;
 		return;
 	}
 
 	/* configure GPIO pin */
+	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
 	/* subscribe to vehicle status topic */
@@ -150,11 +209,11 @@ void gpio_led_start(FAR void *arg)
 	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
 
 	if (ret != 0) {
-		warnx("[gpio_led] Failed to queue work: %d\n", ret);
+		// TODO find way to print errors
+		//printf("gpio_led: failed to queue work: %d\n", ret);
+		gpio_led_started = false;
 		return;
 	}
-
-	warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
 }
 
 void gpio_led_cycle(FAR void *arg)
@@ -200,7 +259,6 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
-
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
@@ -211,7 +269,12 @@ void gpio_led_cycle(FAR void *arg)
 	if (priv->counter > 5)
 		priv->counter = 0;
 
-	/* repeat cycle at 5 Hz*/
-	if (gpio_led_started)
+	/* repeat cycle at 5 Hz */
+	if (gpio_led_started) {
 		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+
+	} else {
+		/* switch off LED on stop */
+		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
+	}
 }
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 5b8345e7e..7c1c4b175 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -471,7 +470,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 }
 
 void
-mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
 	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
 }
@@ -479,7 +478,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
 /*
  * Internal function to give access to the channel status for each channel
  */
-mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
+extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 {
 	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_status[channel];
@@ -488,7 +487,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 /*
  * Internal function to give access to the channel buffer for each channel
  */
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
+extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 {
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 0010bb341..149efda60 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
+extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
-mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
+extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
+extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
+
+#include <v1.0/common/mavlink.h>
 
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
index 819f3441b..18ca7a854 100644
--- a/src/modules/mavlink/mavlink_parameters.c
+++ b/src/modules/mavlink/mavlink_parameters.c
@@ -40,7 +40,6 @@
  */
 
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include "mavlink_parameters.h"
 #include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 33ac14860..01bbabd46 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -49,7 +49,6 @@
 #include <fcntl.h>
 #include <mqueue.h>
 #include <string.h>
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
-			warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 4b010dd59..be88b8794 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -48,7 +48,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
index c2ca735b3..c7d8f90c5 100644
--- a/src/modules/mavlink/missionlib.h
+++ b/src/modules/mavlink/missionlib.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <v1.0/common/mavlink.h>
+#include "mavlink_bridge_header.h"
 
 //extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
 //extern void	mavlink_wpm_send_gcs_string(const char *string);
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 0597555ab..edb8761b8 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -47,7 +47,6 @@
 #include <fcntl.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index cefcca468..405046750 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -45,6 +45,7 @@
 #include <unistd.h>
 #include <stdio.h>
 
+#include "mavlink_bridge_header.h"
 #include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index c32ab32e5..96a0ecd30 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -47,11 +47,11 @@
 
 #include <v1.0/mavlink_types.h>
 
-#ifndef MAVLINK_SEND_UART_BYTES
-#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
-#endif
-extern mavlink_system_t mavlink_system;
-#include <v1.0/common/mavlink.h>
+// #ifndef MAVLINK_SEND_UART_BYTES
+// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
+// #endif
+//extern mavlink_system_t mavlink_system;
+#include "mavlink_bridge_header.h"
 #include <stdbool.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index cb6d6b16a..20fb11b2c 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
index 3ad3bb617..b72bbb2b1 100644
--- a/src/modules/mavlink_onboard/mavlink_bridge_header.h
+++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h
@@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
 mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
 mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
 
+#include <v1.0/common/mavlink.h>
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 0acbea675..68d49c24b 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -50,7 +50,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3cf9ca149..43d96fb06 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -95,9 +95,16 @@ controls_tick() {
 	 */
 
 	perf_begin(c_gather_dsm);
-	bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (dsm_updated)
+	uint16_t temp_count = r_raw_rc_count;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+		else
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..ab6e3fec4 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -40,6 +40,7 @@
  */
 
 #include <nuttx/config.h>
+#include <nuttx/arch.h>
 
 #include <fcntl.h>
 #include <unistd.h>
@@ -101,6 +102,41 @@ dsm_init(const char *device)
 	return dsm_fd;
 }
 
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+	case dsm_bind_power_down:
+		// power down DSM satellite
+		POWER_RELAY1(0);
+		break;
+	case dsm_bind_power_up:
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+	case dsm_bind_set_rx_out:
+		stm32_configgpio(usart1RxAsOutp); 
+		break;
+	case dsm_bind_send_pulses:
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(50);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(50);
+		}
+		break;
+	case dsm_bind_reinit_uart:
+		// Restore USART rx pin
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+	}
+}
+
 bool
 dsm_input(uint16_t *values, uint16_t *num_values)
 {
@@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
 
 	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
-	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 * decoding in 10 or 11-bit mode has yielded anything we recognize.
 	 *
 	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
 	 *     stream, we may want to sniff for longer in some cases when we think we
@@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
+	if (channel_shift == 11)
+		*num_values |= 0x8000;
+
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
 	 */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 674f9dddd..88d8cc87c 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -105,6 +105,7 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -156,7 +157,22 @@
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+
+/* px4io relay bit definitions */
+#define PX4IO_RELAY1	(1<<0)
+#define PX4IO_RELAY2	(1<<1)
+#define PX4IO_ACC1		(1<<2)
+#define PX4IO_ACC2		(1<<3)
+
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM				7	/* DSM bind state */
+enum {                                      /* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..83feeb9b6 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -184,6 +184,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern void     dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index df7d6dcd3..a922362b6 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
+			POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
+			POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
+			POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
@@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		case PX4IO_P_SETUP_DSM:
+			dsm_bind(value & 0x0f, (value >> 4) & 7);
+			break;
+
 		default:
 			return -1;
 		}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index b90b3e178..1eb2c7b98 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -79,6 +79,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/systemlib.h>
 
@@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 17;
+	const ssize_t fdsc = 18;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
+		struct esc_status_s esc;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
+		int esc_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
 			struct log_GPOS_s log_GPOS;
+			struct log_ESC_s log_ESC;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;	
 
+	/* --- ESCs --- */
+	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
+	fds[fdsc_count].fd = subs.esc_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -1105,6 +1115,28 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(AIRS);
 			}
 
+			/* --- ESCs --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
+				for (uint8_t i=0; i<buf.esc.esc_count; i++)
+				{
+					log_msg.msg_type = LOG_ESC_MSG;
+					log_msg.body.log_ESC.counter = buf.esc.counter;
+					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
+					log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
+					log_msg.body.log_ESC.esc_num = i;
+					log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
+					log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
+					log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
+					log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
+					log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
+					log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
+					log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
+					log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
+					LOGBUFFER_WRITE_AND_COUNT(ESC);
+				}
+			}
+
 #ifdef SDLOG2_DEBUG
 				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
 #endif
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 1b2237d65..d9f1cd634 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -209,6 +209,24 @@ struct log_GPOS_s {
 	float vel_e;
 	float vel_d;
 };
+
+/* --- ESC - ESC STATE --- */
+#define LOG_ESC_MSG 64
+struct log_ESC_s {
+	uint16_t counter;
+	uint8_t esc_count;
+	uint8_t esc_connectiontype;
+
+	uint8_t esc_num;
+	uint16_t esc_address;
+	uint16_t esc_version;
+	uint16_t esc_voltage;
+	uint16_t esc_current;
+	uint16_t esc_rpm;
+	uint16_t esc_temperature;
+	float    esc_setpoint;
+	uint16_t esc_setpoint_raw;
+};
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -230,6 +248,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index f6f4d60c7..252c1b7a9 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1ded14a91..ae5a55109 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -278,6 +280,8 @@ private:
 		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
+
+		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -514,6 +518,9 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* DSM VCC relay control */
+	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -730,6 +737,11 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	/* relay 1 DSM VCC control */
+	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
+		warnx("Failed updating relay 1 DSM VCC control");
+	}
+
 	return OK;
 }
 
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 8fdff8ac0..afc5b072c 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
-//	if (!system_load.initialized)
-//	{
 	system_load.start_time = hrt_absolute_time();
 	int i;
 
@@ -80,27 +78,29 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	system_load.total_count = 0;
-
 	uint64_t now = hrt_absolute_time();
 
-	/* initialize idle thread statically */
-	system_load.tasks[0].start_time = now;
-	system_load.tasks[0].total_runtime = 0;
-	system_load.tasks[0].curr_start_time = 0;
-	system_load.tasks[0].tcb = sched_gettcb(0);
-	system_load.tasks[0].valid = true;
-	system_load.total_count++;
-
-	/* initialize init thread statically */
-	system_load.tasks[1].start_time = now;
-	system_load.tasks[1].total_runtime = 0;
-	system_load.tasks[1].curr_start_time = 0;
-	system_load.tasks[1].tcb = sched_gettcb(1);
-	system_load.tasks[1].valid = true;
-	/* count init thread */
-	system_load.total_count++;
-	//	}
+	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
+
+#ifdef CONFIG_PAGING
+	static_tasks_count++;	// include paging thread in initialization
+#endif /* CONFIG_PAGING */
+#if CONFIG_SCHED_WORKQUEUE
+	static_tasks_count++;	// include high priority work0 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+#if CONFIG_SCHED_LPWORK
+	static_tasks_count++;	// include low priority work1 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+
+	// perform static initialization of "system" threads
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
+	{
+		system_load.tasks[system_load.total_count].start_time = now;
+		system_load.tasks[system_load.total_count].total_runtime = 0;
+		system_load.tasks[system_load.total_count].curr_start_time = 0;
+		system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
+		system_load.tasks[system_load.total_count].valid = true;
+	}
 }
 
 void sched_note_start(FAR struct tcb_s *tcb)
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index e7d7e7bca..ae5fc6c61 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -169,3 +169,6 @@ ORB_DEFINE(debug_key_value, struct debug_key_value_s);
 
 #include "topics/navigation_capabilities.h"
 ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
+
+#include "topics/esc_status.h"
+ORB_DEFINE(esc_status, struct esc_status_s);
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
new file mode 100644
index 000000000..e67a39e1e
--- /dev/null
+++ b/src/modules/uORB/topics/esc_status.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Marco Bauer <marco@wtns.de>
+ *
+ * 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 esc_status.h
+ * Definition of the esc_status uORB topic.
+ *
+ * Published the state machine and the system status bitfields
+ * (see SYS_STATUS mavlink message), published only by commander app.
+ *
+ * All apps should write to subsystem_info:
+ *
+ *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> esc_status --> (mavlink app)
+ */
+
+#ifndef ESC_STATUS_H_
+#define ESC_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+/**
+ * The number of ESCs supported.
+ * Current (Q2/2013) we support 8 ESCs,
+ */
+#define CONNECTED_ESC_MAX   8
+
+enum ESC_VENDOR {
+	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
+	ESC_VENDOR_MIKROKOPTER					/**< Mikrokopter */
+};
+
+enum ESC_CONNECTION_TYPE {
+	ESC_CONNECTION_TYPE_PPM = 0,			/**< Traditional PPM ESC */
+	ESC_CONNECTION_TYPE_SERIAL,			/**< Serial Bus connected ESC */
+	ESC_CONNECTION_TYPE_ONESHOOT,			/**< One Shoot PPM */
+	ESC_CONNECTION_TYPE_I2C,				/**< I2C */
+	ESC_CONNECTION_TYPE_CAN					/**< CAN-Bus */
+};
+
+/**
+ *
+ */
+struct esc_status_s
+{
+	/* use of a counter and timestamp recommended (but not necessary) */
+
+	uint16_t counter;   		/**< incremented by the writing thread everytime new data is stored */
+	uint64_t timestamp; 		/**< in microseconds since system start, is set whenever the writing thread stores new data */
+	
+	uint8_t esc_count;			/**< number of connected ESCs */
+	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
+	
+	struct {
+		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
+		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
+		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
+		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
+		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
+		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
+		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
+		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+	} esc[CONNECTED_ESC_MAX];
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+//ORB_DECLARE(esc_status);
+ORB_DECLARE_OPTIONAL(esc_status);
+
+#endif