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

15 files changed, 170 insertions, 42 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 97d7fdd75..191d20f30 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -661,10 +661,10 @@ int KalmanNav::correctPos()
 	Vector y(6);
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
-	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
-	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = double(_gps.alt) / 1.0e3 - alt;
-	y(5) = double(_sensors.baro_alt_meter) - alt;
+	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
+	y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
+	y(4) = _gps.alt / 1.0e3f - alt;
+	y(5) = _sensors.baro_alt_meter - alt;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -698,7 +698,7 @@ int KalmanNav::correctPos()
 	vD += xCorrect(VD);
 	lat += double(xCorrect(LAT));
 	lon += double(xCorrect(LON));
-	alt += double(xCorrect(ALT));
+	alt += xCorrect(ALT);
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -710,7 +710,7 @@ int KalmanNav::correctPos()
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
 		warnx("fault in gps: beta = %8.4f", (double)beta);
-		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
+		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
 		       double(y(0) / sqrtf(RPos(0, 0))),
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 67f053e22..928d9b85e 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -1389,6 +1389,7 @@ int commander_thread_main(int argc, char *argv[])
 	uint64_t start_time = hrt_absolute_time();
 	uint64_t failsave_ll_start_time = 0;
 
+	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
 	bool state_changed = true;
 	bool param_init_forced = true;
 
@@ -1828,8 +1829,9 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
 
-					/* bottom stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+					/* bottom stick position, set default */
+					/* this MUST be mapped to extremal position to switch easy in case of emergency */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
 
 				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
 
@@ -1837,8 +1839,14 @@ int commander_thread_main(int argc, char *argv[])
 					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
 
 				} else {
-					/* center stick position, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+					/* center stick position, set altitude hold */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+				}
+
+				if (current_status.manual_sas_mode != manual_sas_mode) {
+					/* publish SAS mode changes immediately */
+					manual_sas_mode = current_status.manual_sas_mode;
+					state_changed = true;
 				}
 
 				/*
@@ -1849,8 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
 				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
 				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
 				    ) &&
-				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
-				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
+					current_status.flag_control_manual_enabled &&
+					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
+				    sp_man.yaw < -STICK_ON_OFF_LIMIT &&
+				    sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
 						stick_on_counter = 0;
@@ -1862,7 +1872,10 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* check if left stick is in lower right position --> arm */
-				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
+				if (current_status.flag_control_manual_enabled &&
+					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
+					sp_man.yaw > STICK_ON_OFF_LIMIT &&
+					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
 						stick_on_counter = 0;
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 8b4c0cb30..1aef739c7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -53,11 +53,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
-
-#define PX4IO_RELAY1	(1<<0)
-#define PX4IO_RELAY2	(1<<1)
-#define PX4IO_ACC1		(1<<2)
-#define PX4IO_ACC2		(1<<3)
+#include <modules/px4iofirmware/protocol.h>
 
 struct gpio_led_s {
 	struct work_s work;
@@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg)
 	char *gpio_dev;
 
 	if (priv->use_io) {
-		gpio_dev = "/dev/px4io";
-
+		gpio_dev = PX4IO_DEVICE_PATH;
 	} else {
-		gpio_dev = "/dev/px4fmu";
+		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
 
 	/* open GPIO device */
@@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg)
 	}
 
 	/* 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 */
@@ -263,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);
 		}
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 405046750..eea928a17 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -373,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
 			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5a95a8aa9..74037d1c2 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 fa57dfc3f..dc5c40638 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -106,6 +106,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) /* [1] VBatt is very close to regulator dropout */
@@ -169,7 +170,14 @@
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* [2] servo voltage correction factor (float) */
- 					     /*	7 */
+#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
+};
  					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ccf175e45..bd78f2638 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -200,6 +200,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 873ee73f1..9606faa86 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -360,16 +360,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
 #ifdef POWER_RELAY1
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
 #endif
 #ifdef POWER_RELAY2
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
 #endif
 #ifdef POWER_ACC1
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
 #endif
 #ifdef POWER_ACC2
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
 #endif
 			break;
 
@@ -382,6 +382,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 3e6b20472..3713e0b30 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,6 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/optical_flow.h>
@@ -615,7 +616,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 = 18;
+	const ssize_t fdsc = 19;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
+		struct vehicle_global_position_setpoint_s global_pos_sp;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
+		int global_pos_sp_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
@@ -689,6 +692,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_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 		} body;
 	} log_msg = {
@@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GLOBAL POSITION SETPOINT--- */
+	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	fds[fdsc_count].fd = subs.global_pos_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- VICON POSITION --- */
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
 	fds[fdsc_count].fd = subs.vicon_pos_sub;
@@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
+			/* --- GLOBAL POSITION SETPOINT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+				log_msg.msg_type = LOG_GPSP_MSG;
+				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
+				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
+				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
+				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
+				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
+				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
+				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
+				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
+				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
+				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
+				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+				LOGBUFFER_WRITE_AND_COUNT(GPSP);
+			}
+
 			/* --- VICON POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index abc882d23..934e4dec8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -149,15 +149,15 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	unsigned char state;
-	unsigned char flight_mode;
-	unsigned char manual_control_mode;
-	unsigned char manual_sas_mode;
-	unsigned char armed;
+	uint8_t state;
+	uint8_t flight_mode;
+	uint8_t manual_control_mode;
+	uint8_t manual_sas_mode;
+	uint8_t armed;
 	float battery_voltage;
 	float battery_current;
 	float battery_remaining;
-	unsigned char battery_warning;
+	uint8_t battery_warning;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -210,13 +210,29 @@ struct log_GPOS_s {
 	float vel_d;
 };
 
+/* --- GPSP - GLOBAL POSITION SETPOINT --- */
+#define LOG_GPSP_MSG 17
+struct log_GPSP_s {
+	uint8_t altitude_is_relative;
+	int32_t lat;
+	int32_t lon;
+	float altitude;
+	float yaw;
+	float loiter_radius;
+	int8_t loiter_direction;
+	uint8_t nav_cmd;
+	float param1;
+	float param2;
+	float param3;
+	float param4;
+};
+
 /* --- ESC - ESC STATE --- */
-#define LOG_ESC_MSG 64
+#define LOG_ESC_MSG 18
 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;
@@ -227,6 +243,7 @@ struct log_ESC_s {
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
 };
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -248,6 +265,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(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 };
 
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/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index e67a39e1e..00cf59b28 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -63,7 +63,8 @@
 
 enum ESC_VENDOR {
 	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
-	ESC_VENDOR_MIKROKOPTER					/**< Mikrokopter */
+	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
 };
 
 enum ESC_CONNECTION_TYPE {
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index 3ae3ff28c..5c8ce1e4d 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
 	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
 	float param1;
 	float param2;