Merge remote-tracking branch 'upstream/master'

 - Mikrokopter BLCTRL seems to be updated
 - HMC5883L calibration problem has been corrected.
    (This is because of RAM mis allocation?)
    See https://groups.google.com/forum/?fromgroups#!topic/px4users/yTYJiDBBKfo
 - Fixed wing control updated
    https://groups.google.com/forum/?fromgroups#!topic/px4users/s7owpvZN8UI
 - GPIO module has been removed.
 - STM32 DRV updated

9 files changed, 629 insertions, 257 deletions

diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk
index 4cf650a98..437f1cd17 100644
--- a/makefiles/config_px4fmu_default.mk
+++ b/makefiles/config_px4fmu_default.mk
@@ -56,6 +56,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= modules/commander
 MODULES		+= modules/mavlink
 MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
 
 #
 # Estimation modules (EKF / other filters)
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 874e7154c..c75a08bd1 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -219,7 +219,7 @@ endef
 define PRELINK
 	@$(ECHO) "PRELINK: $1"
 	@$(MKDIR) -p $(dir $1)
-	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+	$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
 endef
 
 # Update the archive $1 with the files in $2
@@ -235,7 +235,7 @@ endef
 define LINK
 	@$(ECHO) "LINK:    $1"
 	@$(MKDIR) -p $(dir $1)
-	$(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
+	$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
 endef
 
 # Convert $1 from a linked object to a raw binary in $2
@@ -280,6 +280,7 @@ define BIN_TO_OBJ
 	$(Q) $(OBJCOPY) $2 \
 		--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
 		--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
-		--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
+		--strip-symbol $(call BIN_SYM_PREFIX,$1)_end \
+		--rename-section .data=.rodata
 	$(Q) $(REMOVE) $2.c $2.c.o
 endef
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 78eda327c..7de394f24 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1224,10 +1224,12 @@ start()
 		errx(1, "already started");
 
 	/* create the driver, attempt expansion bus first */
+	warnx("probing for external sensor..");
 	g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
 	if (g_dev != nullptr && OK != g_dev->init()) {
 		delete g_dev;
 		g_dev = nullptr;
+		warnx("no external sensor, using internal..");
 	}
 			
 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 3a735e26f..c67276f8a 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -76,7 +76,6 @@
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
 
 #define I2C_BUS_SPEED					400000
 #define UPDATE_RATE						400
@@ -114,6 +113,7 @@ public:
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 	virtual int	init(unsigned motors);
+	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
 	int		set_mode(Mode mode);
 	int		set_pwm_rate(unsigned rate);
@@ -177,9 +177,10 @@ private:
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 	int			mk_servo_arm(bool status);
 
-	int 		mk_servo_set(unsigned int chan, float val);
-	int 		mk_servo_set_test(unsigned int chan, float val);
+	int 		mk_servo_set(unsigned int chan, short val);
+	int 		mk_servo_set_value(unsigned int chan, short val);
 	int 		mk_servo_test(unsigned int chan);
+	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
 
 
 };
@@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };	// Addresstranslat
 
 const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
 
-int addrTranslator[] = {0,0,0,0,0,0,0,0};
+int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
 
-struct MotorData_t
-{
+struct MotorData_t {
 	unsigned int Version;                        // the version of the BL (0 = old)
-  unsigned int SetPoint;                       // written by attitude controller
-  unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
-  unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
-  unsigned int ReadMode;                       // select data to read
-  // the following bytes must be exactly in that order!
-  unsigned int Current;                        // in 0.1 A steps, read back from BL
-  unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
-  unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
-  unsigned int RoundCount;
+	unsigned int SetPoint;                       // written by attitude controller
+	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+	unsigned int ReadMode;                       // select data to read
+	unsigned short RawPwmValue;							// length of PWM pulse
+	// the following bytes must be exactly in that order!
+	unsigned int Current;                        // in 0.1 A steps, read back from BL
+	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int RoundCount;
 };
 
 MotorData_t Motor[MAX_MOTORS];
@@ -314,7 +315,7 @@ MK::init(unsigned motors)
 	/* start the IO interface task */
 	_task = task_spawn("mkblctrl",
 			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_MAX -20,
+			   SCHED_PRIORITY_MAX - 20,
 			   2048,
 			   (main_t)&MK::task_main_trampoline,
 			   nullptr);
@@ -346,27 +347,11 @@ MK::set_mode(Mode mode)
 	 */
 	switch (mode) {
 	case MODE_2PWM:
-		if(_num_outputs == 4) {
-			//debug("MODE_QUAD");
-		} else if(_num_outputs == 6) {
-			//debug("MODE_HEXA");
-		} else if(_num_outputs == 8) {
-			//debug("MODE_OCTO");
-		}
-		//up_pwm_servo_init(0x3);
 		up_pwm_servo_deinit();
 		_update_rate = UPDATE_RATE;	/* default output rate */
 		break;
 
 	case MODE_4PWM:
-		if(_num_outputs == 4) {
-			//debug("MODE_QUADRO");
-		} else if(_num_outputs == 6) {
-			//debug("MODE_HEXA");
-		} else if(_num_outputs == 8) {
-			//debug("MODE_OCTO");
-		}
-		//up_pwm_servo_init(0xf);
 		up_pwm_servo_deinit();
 		_update_rate = UPDATE_RATE;	/* default output rate */
 		break;
@@ -412,45 +397,55 @@ MK::set_frametype(int frametype)
 int
 MK::set_motor_count(unsigned count)
 {
-	if(count > 0) {
+	if (count > 0) {
 
 		_num_outputs = count;
 
-		if(_px4mode == MAPPING_MK) {
-			if(_frametype == FRAME_PLUS) {
+		if (_px4mode == MAPPING_MK) {
+			if (_frametype == FRAME_PLUS) {
 				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
-			} else if(_frametype == FRAME_X) {
+
+			} else if (_frametype == FRAME_X) {
 				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
 			}
-			if(_num_outputs == 4) {
-				if(_frametype == FRAME_PLUS) {
+
+			if (_num_outputs == 4) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
 				}
-			} else if(_num_outputs == 6) {
-				if(_frametype == FRAME_PLUS) {
+
+			} else if (_num_outputs == 6) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
 				}
-			} else if(_num_outputs == 8) {
-				if(_frametype == FRAME_PLUS) {
+
+			} else if (_num_outputs == 8) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
 				}
 			}
+
 		} else {
 			fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
 			memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
 		}
 
-		if(_num_outputs == 4) {
+		if (_num_outputs == 4) {
 			fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
-		} else 	if(_num_outputs == 6) {
+
+		} else 	if (_num_outputs == 6) {
 			fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
-		} else 	if(_num_outputs == 8) {
+
+		} else 	if (_num_outputs == 8) {
 			fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
 		}
 
@@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest)
 	return OK;
 }
 
+short
+MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
+{
+	short retVal = 0;
+
+	retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin;
+
+	if (retVal < outMin) {
+		retVal = outMin;
+
+	} else if (retVal > outMax) {
+		retVal = outMax;
+	}
+
+	return retVal;
+}
 
 void
 MK::task_main()
 {
+	long update_rate_in_us = 0;
+	float tmpVal = 0;
+
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
 	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
+	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
@@ -492,6 +506,8 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
+
+
 	/* advertise the effective control inputs */
 	actuator_controls_effective_s controls_effective;
 	memset(&controls_effective, 0, sizeof(controls_effective));
@@ -499,21 +515,14 @@ MK::task_main()
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
 				   &controls_effective);
 
+
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_armed;
 	fds[1].events = POLLIN;
 
-	// rc input, published to ORB
-	struct rc_input_values rc_in;
-	orb_advert_t to_input_rc = 0;
-
-	memset(&rc_in, 0, sizeof(rc_in));
-	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
-	
 	log("starting");
-	long update_rate_in_us = 0;
 
 	/* loop until killed */
 	while (!_task_should_exit) {
@@ -528,6 +537,7 @@ MK::task_main()
 				update_rate_in_ms = 2;
 				_update_rate = 500;
 			}
+
 			/* reject slower than 50 Hz updates */
 			if (update_rate_in_ms > 20) {
 				update_rate_in_ms = 20;
@@ -539,8 +549,8 @@ MK::task_main()
 			_current_update_rate = _update_rate;
 		}
 
-		/* sleep waiting for data, but no more than a second */
-		int ret = ::poll(&fds[0], 2, 1000);
+		/* sleep waiting for data max 100ms */
+		int ret = ::poll(&fds[0], 2, 100);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -553,7 +563,7 @@ MK::task_main()
 		if (fds[0].revents & POLLIN) {
 
 			/* get controls - must always do this to avoid spinning */
-			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
 
 			/* can we mix? */
 			if (_mixers != nullptr) {
@@ -565,53 +575,52 @@ MK::task_main()
 				// XXX output actual limited values
 				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
 
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
 				/* iterate actuators */
 				for (unsigned int i = 0; i < _num_outputs; i++) {
 
 					/* last resort: catch NaN, INF and out-of-band errors */
 					if (i < outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
+					    isfinite(outputs.output[i]) &&
+					    outputs.output[i] >= -1.0f &&
+					    outputs.output[i] <= 1.0f) {
 						/* scale for PWM output 900 - 2100us */
-						//outputs.output[i] = 1500 + (600 * outputs.output[i]);
-						//outputs.output[i] = 127 + (127 * outputs.output[i]);
+						/* nothing to do here */
 					} else {
 						/*
 						 * Value is NaN, INF or out of band - set to the minimum value.
 						 * This will be clearly visible on the servo status and will limit the risk of accidentally
 						 * spinning motors. It would be deadly in flight.
 						 */
-						if(outputs.output[i] < -1.0f) {
+						if (outputs.output[i] < -1.0f) {
 							outputs.output[i] = -1.0f;
-						} else if(outputs.output[i] > 1.0f) {
+
+						} else if (outputs.output[i] > 1.0f) {
 							outputs.output[i] = 1.0f;
+
 						} else {
 							outputs.output[i] = -1.0f;
 						}
 					}
 
 					/* don't go under BLCTRL_MIN_VALUE */
-					if(outputs.output[i] < BLCTRL_MIN_VALUE) {
+					if (outputs.output[i] < BLCTRL_MIN_VALUE) {
 						outputs.output[i] = BLCTRL_MIN_VALUE;
 					}
-					//_motortest = true;
+
 					/* output to BLCtrl's */
-					if(_motortest == true) {
+					if (_motortest == true) {
 						mk_servo_test(i);
+
 					} else {
-						//mk_servo_set(i, outputs.output[i]);
-						mk_servo_set_test(i, outputs.output[i]);	// 8Bit
+						mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
 					}
 
-
 				}
 
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
 			}
+
+
+
 		}
 
 		/* how about an arming update? */
@@ -622,29 +631,9 @@ MK::task_main()
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
-			////up_pwm_servo_arm(aa.armed && !aa.lockdown);
 			mk_servo_arm(aa.armed && !aa.lockdown);
 		}
 
-		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
-			// we have a new PPM frame. Publish it.
-			rc_in.channel_count = ppm_decoded_channels;
-			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
-				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
-			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
-				rc_in.values[i] = ppm_buffer[i];
-			}
-			rc_in.timestamp = ppm_last_valid_decode;
-
-			/* lazily advertise on first publication */
-			if (to_input_rc == 0) {
-				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else {
-				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
-			}
-		}
 
 	}
 
@@ -666,7 +655,7 @@ MK::task_main()
 }
 
 
-int 
+int
 MK::mk_servo_arm(bool status)
 {
 	_armed = status;
@@ -680,12 +669,13 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 	_retries = 50;
 	uint8_t foundMotorCount = 0;
 
-	for(unsigned i=0; i<MAX_MOTORS; i++) {
+	for (unsigned i = 0; i < MAX_MOTORS; i++) {
 		Motor[i].Version = 0;
 		Motor[i].SetPoint = 0;
 		Motor[i].SetPointLowerBits = 0;
 		Motor[i].State = 0;
 		Motor[i].ReadMode = 0;
+		Motor[i].RawPwmValue = 0;
 		Motor[i].Current = 0;
 		Motor[i].MaxPWM = 0;
 		Motor[i].Temperature = 0;
@@ -695,34 +685,37 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 	uint8_t msg = 0;
 	uint8_t result[3];
 
-	for(unsigned i=0; i< count; i++) {
+	for (unsigned i = 0; i < count; i++) {
 		result[0] = 0;
 		result[1] = 0;
 		result[2] = 0;
-			
-		set_address( BLCTRL_BASE_ADDR + i );
-		
+
+		set_address(BLCTRL_BASE_ADDR + i);
+
 		if (OK == transfer(&msg, 1, &result[0], 3)) {
 			Motor[i].Current = result[0];
 			Motor[i].MaxPWM = result[1];
 			Motor[i].Temperature = result[2];
 			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
 			foundMotorCount++;
-			if(Motor[i].MaxPWM == 250) {
+
+			if (Motor[i].MaxPWM == 250) {
 				Motor[i].Version = BLCTRL_NEW;
+
 			} else {
 				Motor[i].Version = BLCTRL_OLD;
 			}
 		}
 	}
 
-	if(showOutput) {
-		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
-		for(unsigned i=0; i< foundMotorCount; i++) {
-			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
+	if (showOutput) {
+		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount);
+
+		for (unsigned i = 0; i < foundMotorCount; i++) {
+			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+		if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
 			_task_should_exit = true;
 		}
 	}
@@ -734,122 +727,136 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 
 
 int
-MK::mk_servo_set(unsigned int chan, float val)
+MK::mk_servo_set(unsigned int chan, short val)
 {
-	float tmpVal = 0;
+	short tmpVal = 0;
 	_retries = 0;
-	uint8_t result[3] = { 0,0,0 };
-	uint8_t msg[2] = { 0,0 };
-	uint8_t rod=0;
+	uint8_t result[3] = { 0, 0, 0 };
+	uint8_t msg[2] = { 0, 0 };
+	uint8_t rod = 0;
 	uint8_t bytesToSendBL2 = 2;
 
+	tmpVal = val;
 
-	tmpVal = (1023 + (1023 * val));
-	if(tmpVal > 2047) {
-		tmpVal = 2047;
+	if (tmpVal > 1024) {
+		tmpVal = 1024;
+
+	} else if (tmpVal < 0) {
+		tmpVal = 0;
 	}
 
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
+	//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
 
-	Motor[chan].SetPoint = (uint8_t) tmpVal / 3;	// divide 8
-	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8;	// rest of divide 8
-	//rod = (uint8_t) tmpVal % 8;
-	//Motor[chan].SetPointLowerBits = rod<<1;	// rest of divide 8
 	Motor[chan].SetPointLowerBits = 0;
 
-	if(_armed == false) {
+	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
 
 	//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
-		set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
-
-		if(Motor[chan].Version == BLCTRL_OLD) {
-			/*
-			*	Old BL-Ctrl 8Bit served. Version < 2.0
-			*/
-			msg[0] = Motor[chan].SetPoint;
-			if(Motor[chan].RoundCount >= 16) {
-				// on each 16th cyle we read out the status messages from the blctrl
-				if (OK == transfer(&msg[0], 1, &result[0], 2)) {
-					Motor[chan].Current = result[0];
-					Motor[chan].MaxPWM = result[1];
-					Motor[chan].Temperature = 255;;
-				} else {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
-				Motor[chan].RoundCount = 0;
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+
+	if (Motor[chan].Version == BLCTRL_OLD) {
+		/*
+		*	Old BL-Ctrl 8Bit served. Version < 2.0
+		*/
+		msg[0] = Motor[chan].SetPoint;
+
+		if (Motor[chan].RoundCount >= 16) {
+			// on each 16th cyle we read out the status messages from the blctrl
+			if (OK == transfer(&msg[0], 1, &result[0], 2)) {
+				Motor[chan].Current = result[0];
+				Motor[chan].MaxPWM = result[1];
+				Motor[chan].Temperature = 255;;
+
 			} else {
-				if (OK != transfer(&msg[0], 1, nullptr, 0)) {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
 			}
 
+			Motor[chan].RoundCount = 0;
+
 		} else {
-			/*
-			*	New BL-Ctrl 11Bit served. Version >= 2.0
-			*/
-			msg[0] = Motor[chan].SetPoint;
-			msg[1] = Motor[chan].SetPointLowerBits;
-
-			if(Motor[chan].SetPointLowerBits == 0) {
-				bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
+			if (OK != transfer(&msg[0], 1, nullptr, 0)) {
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
 			}
+		}
+
+	} else {
+		/*
+		*	New BL-Ctrl 11Bit served. Version >= 2.0
+		*/
+		msg[0] = Motor[chan].SetPoint;
+		msg[1] = Motor[chan].SetPointLowerBits;
+
+		if (Motor[chan].SetPointLowerBits == 0) {
+			bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
+		}
+
+		if (Motor[chan].RoundCount >= 16) {
+			// on each 16th cyle we read out the status messages from the blctrl
+			if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
+				Motor[chan].Current = result[0];
+				Motor[chan].MaxPWM = result[1];
+				Motor[chan].Temperature = result[2];
 
-			if(Motor[chan].RoundCount >= 16) {
-				// on each 16th cyle we read out the status messages from the blctrl
-				if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
-					Motor[chan].Current = result[0];
-					Motor[chan].MaxPWM = result[1];
-					Motor[chan].Temperature = result[2];
-				} else {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
-				Motor[chan].RoundCount = 0;
 			} else {
-				if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
 			}
 
+			Motor[chan].RoundCount = 0;
+
+		} else {
+			if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+			}
 		}
 
-		Motor[chan].RoundCount++;
+	}
+
+	Motor[chan].RoundCount++;
 	//}
 
-	if(showDebug == true) {
+	if (showDebug == true) {
 		debugCounter++;
-		if(debugCounter == 2000) {
+
+		if (debugCounter == 2000) {
 			debugCounter = 0;
-			for(int i=0; i<_num_outputs; i++){
-				if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
+
+			for (int i = 0; i < _num_outputs; i++) {
+				if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
 					fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
 				}
 			}
+
 			fprintf(stderr, "\n");
 		}
 	}
+
 	return 0;
 }
 
 int
-MK::mk_servo_set_test(unsigned int chan, float val)
+MK::mk_servo_set_value(unsigned int chan, short val)
 {
 	_retries = 0;
 	int ret;
+	short tmpVal = 0;
+	uint8_t msg[2] = { 0, 0 };
 
-	float tmpVal = 0;
+	tmpVal = val;
 
-	uint8_t msg[2] = { 0,0 };
+	if (tmpVal > 1024) {
+		tmpVal = 1024;
 
-	tmpVal = (1023 + (1023 * val));
-	if(tmpVal > 2048) {
-		tmpVal = 2048;
+	} else if (tmpVal < 0) {
+		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
 
-	if(_armed == false) {
+	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
@@ -860,7 +867,6 @@ MK::mk_servo_set_test(unsigned int chan, float val)
 	ret = transfer(&msg[0], 1, nullptr, 0);
 
 	ret = OK;
-
 	return ret;
 }
 
@@ -868,59 +874,61 @@ MK::mk_servo_set_test(unsigned int chan, float val)
 int
 MK::mk_servo_test(unsigned int chan)
 {
-	int ret=0;
+	int ret = 0;
 	float tmpVal = 0;
 	float val = -1;
 	_retries = 0;
-	uint8_t msg[2] = { 0,0 };
+	uint8_t msg[2] = { 0, 0 };
 
-	if(debugCounter >= MOTOR_SPINUP_COUNTER) {
+	if (debugCounter >= MOTOR_SPINUP_COUNTER) {
 		debugCounter = 0;
 		_motor++;
 
-		if(_motor < _num_outputs) {
+		if (_motor < _num_outputs) {
 			fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
 		}
 
-		if(_motor >= _num_outputs) {
+		if (_motor >= _num_outputs) {
 			_motor = -1;
 			_motortest = false;
 		}
-
 	}
+
 	debugCounter++;
 
-	if(_motor == chan) {
+	if (_motor == chan) {
 		val = BLCTRL_MIN_VALUE;
+
 	} else {
 		val = -1;
 	}
 
-	tmpVal = (1023 + (1023 * val));
-	if(tmpVal > 2048) {
-		tmpVal = 2048;
+	tmpVal = (511 + (511 * val));
+
+	if (tmpVal > 1024) {
+		tmpVal = 1024;
 	}
 
-	//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
-	//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
-	Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
-	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
 
-	if(_motor != chan) {
+	if (_motor != chan) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
 
-	if(Motor[chan].Version == BLCTRL_OLD) {
+	if (Motor[chan].Version == BLCTRL_OLD) {
 		msg[0] = Motor[chan].SetPoint;
+
 	} else {
 		msg[0] = Motor[chan].SetPoint;
 		msg[1] = Motor[chan].SetPointLowerBits;
 	}
 
 	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
-	if(Motor[chan].Version == BLCTRL_OLD) {
+
+	if (Motor[chan].Version == BLCTRL_OLD) {
 		ret = transfer(&msg[0], 1, nullptr, 0);
+
 	} else {
 		ret = transfer(&msg[0], 2, nullptr, 0);
 	}
@@ -931,9 +939,9 @@ MK::mk_servo_test(unsigned int chan)
 
 int
 MK::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
+		     uint8_t control_group,
+		     uint8_t control_index,
+		     float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -947,7 +955,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 	int ret;
 
 	// XXX disabled, confusing users
-	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
 
 	/* try it as a GPIO ioctl first */
 	ret = gpio_ioctl(filp, cmd, arg);
@@ -978,32 +985,37 @@ int
 MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
 
 	lock();
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
-		////up_pwm_servo_arm(true);
 		mk_servo_arm(true);
 		break;
 
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+		// these are no-ops, as no safety switch
+		break;
+
 	case PWM_SERVO_DISARM:
-		////up_pwm_servo_arm(false);
 		mk_servo_arm(false);
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		set_pwm_rate(arg);
+		ret = OK;
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		ret = OK;
 		break;
 
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+		if (arg < 2150) {
+			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
+			mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
 
-		/* fake an update to the selected 'servo' channel */
-		if ((arg >= 0) && (arg <= 255)) {
-			channel = cmd - PWM_SERVO_SET(0);
-			//mk_servo_set(channel, arg);
 		} else {
 			ret = -EINVAL;
 		}
@@ -1012,20 +1024,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
 		/* copy the current output value from the channel */
-		*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
+		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
+
 		break;
 
-	case MIXERIOCGETOUTPUTCOUNT:
-		/*
-		if (_mode == MODE_4PWM) {
-			*(unsigned *)arg = 4;
-		} else {
-			*(unsigned *)arg = 2;
-		}
-		 */
+	case PWM_SERVO_GET_RATEGROUP(0):
+	case PWM_SERVO_GET_RATEGROUP(1):
+	case PWM_SERVO_GET_RATEGROUP(2):
+	case PWM_SERVO_GET_RATEGROUP(3):
+		//*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+		break;
 
+	case PWM_SERVO_GET_COUNT:
+	case MIXERIOCGETOUTPUTCOUNT:
 		*(unsigned *)arg = _num_outputs;
-
 		break;
 
 	case MIXERIOCRESET:
@@ -1078,6 +1090,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -1091,6 +1104,38 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
+/*
+  this implements PWM output via a write() method, for compatibility
+  with px4io
+ */
+ssize_t
+MK::write(file *filp, const char *buffer, size_t len)
+{
+	unsigned count = len / 2;
+	// loeschen uint16_t values[4];
+	uint16_t values[8];
+
+	// loeschen if (count > 4) {
+	// loeschen 	// we only have 4 PWM outputs on the FMU
+	// loeschen	count = 4;
+	// loeschen }
+	if (count > _num_outputs) {
+		// we only have 8 I2C outputs in the driver
+		count = _num_outputs;
+	}
+
+
+	// allow for misaligned values
+	memcpy(values, buffer, count * 2);
+
+	for (uint8_t i = 0; i < count; i++) {
+		Motor[i].RawPwmValue = (unsigned short)values[i];
+		mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
+	}
+
+	return count * 2;
+}
+
 void
 MK::gpio_reset(void)
 {
@@ -1229,10 +1274,10 @@ enum MappingMode {
 	MAPPING_PX4,
 };
 
-	enum FrameType {
-		FRAME_PLUS = 0,
-		FRAME_X,
-	};
+enum FrameType {
+	FRAME_PLUS = 0,
+	FRAME_X,
+};
 
 PortMode g_port_mode;
 
@@ -1297,18 +1342,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	g_mk->set_motor_test(motortest);
 
 
-	/* (re)set count of used motors */
-	////g_mk->set_motor_count(motorcount);
 	/* count used motors */
-
 	do {
-		if(g_mk->mk_check_for_blctrl(8, false) != 0) {
+		if (g_mk->mk_check_for_blctrl(8, false) != 0) {
 			shouldStop = 4;
+
 		} else {
 			shouldStop++;
 		}
+
 		sleep(1);
-	} while ( shouldStop < 3);
+	} while (shouldStop < 3);
 
 	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
 
@@ -1375,7 +1419,8 @@ mkblctrl_main(int argc, char *argv[])
 			if (argc > i + 1) {
 				bus = atoi(argv[i + 1]);
 				newMode = true;
-				} else {
+
+			} else {
 				errx(1, "missing argument for i2c bus (-b)");
 				return 1;
 			}
@@ -1384,17 +1429,21 @@ mkblctrl_main(int argc, char *argv[])
 		/* look for the optional frame parameter */
 		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
 			if (argc > i + 1) {
-				if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
+				if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
 					px4mode = MAPPING_MK;
 					newMode = true;
-					if(strcmp(argv[i + 1], "+") == 0) {
+
+					if (strcmp(argv[i + 1], "+") == 0) {
 						frametype = FRAME_PLUS;
+
 					} else {
 						frametype = FRAME_X;
 					}
+
 				} else {
 					errx(1, "only + or x for frametype supported !");
 				}
+
 			} else {
 				errx(1, "missing argument for mkmode (-mkmode)");
 				return 1;
@@ -1409,12 +1458,12 @@ mkblctrl_main(int argc, char *argv[])
 
 		/* look for the optional -h --help parameter */
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
-			showHelp == true;
+			showHelp = true;
 		}
 
 	}
 
-	if(showHelp) {
+	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
 		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
 		exit(1);
@@ -1424,6 +1473,7 @@ mkblctrl_main(int argc, char *argv[])
 	if (g_mk == nullptr) {
 		if (mk_start(bus, motorcount) != OK) {
 			errx(1, "failed to start the MK-BLCtrl driver");
+
 		} else {
 			newMode = true;
 		}
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index cec0c49ff..fd63681e3 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -330,7 +330,7 @@ static void		hrt_call_invoke(void);
 /*
  * PPM decoder tuning parameters
  */
-# define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+# define PPM_MAX_PULSE_WIDTH	550		/* maximum width of a valid pulse */
 # define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
 # define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
 # define PPM_MIN_START		2500		/* shortest valid start gap */
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 1753070e2..89fbef020 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -33,10 +33,13 @@
  ****************************************************************************/
 /**
  * @file main.c
- * Implementation of a fixed wing attitude controller. This file is a complete
- * fixed wing controller flying manual attitude control or auto waypoint control.
+ *
+ * Example implementation of a fixed wing attitude controller. This file is a complete
+ * fixed wing controller for manual attitude control or auto waypoint control.
  * There is no need to touch any other system components to extend / modify the
  * complete control architecture.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -60,7 +63,6 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
@@ -73,8 +75,15 @@
 #include "params.h"
 
 /* Prototypes */
+
 /**
  * Daemon management function.
+ *
+ * This function allows to start / stop the background task (daemon).
+ * The purpose of it is to be able to start the controller on the
+ * command line, query its status and stop it, without giving up
+ * the command line to one particular process or the need for bg/fg
+ * ^Z support by the shell.
  */
 __EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
 
@@ -88,10 +97,34 @@ int fixedwing_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
+/**
+ * Control roll and pitch angle.
+ *
+ * This very simple roll and pitch controller takes the current roll angle
+ * of the system and compares it to a reference. Pitch is controlled to zero and yaw remains
+ * uncontrolled (tutorial code, not intended for flight).
+ *
+ * @param att_sp The current attitude setpoint - the values the system would like to reach.
+ * @param att The current attitude. The controller should make the attitude match the setpoint
+ * @param speed_body The velocity of the system. Currently unused.
+ * @param rates_sp The angular rate setpoint. This is the output of the controller.
+ */
 void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
 	struct actuator_controls_s *actuators);
 
+/**
+ * Control heading.
+ *
+ * This very simple heading to roll angle controller outputs the desired roll angle based on
+ * the current position of the system, the desired position (the setpoint) and the current
+ * heading.
+ *
+ * @param pos The current position of the system
+ * @param sp The current position setpoint
+ * @param att The current attitude
+ * @param att_sp The attitude setpoint. This is the output of the controller
+ */
 void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
 	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
 
@@ -103,7 +136,7 @@ static struct params p;
 static struct param_handles ph;
 
 void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
 	struct actuator_controls_s *actuators)
 {
 
@@ -148,13 +181,23 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
 	 * Calculate heading error of current position to desired position
 	 */
 
-	/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
+	/* 
+	 * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
+	 * so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
+	 */
 	float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
 
 	/* calculate heading error */
 	float yaw_err = att->yaw - bearing;
 	/* apply control gain */
-	att_sp->roll_body = yaw_err * p.hdng_p;
+	float roll_command = yaw_err * p.hdng_p;
+
+	/* limit output, this commonly is a tuning parameter, too */
+	if (att_sp->roll_body < -0.6f) {
+		att_sp->roll_body = -0.6f;
+	} else if (att_sp->roll_body > 0.6f) {
+		att_sp->roll_body = 0.6f;
+	}
 }
 
 /* Main Thread */
@@ -176,7 +219,32 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	parameters_init(&ph);
 	parameters_update(&ph, &p);
 
-	/* declare and safely initialize all structs to zero */
+
+	/*
+	 * PX4 uses a publish/subscribe design pattern to enable
+	 * multi-threaded communication.
+	 *
+	 * The most elegant aspect of this is that controllers and
+	 * other processes can either 'react' to new data, or run
+	 * at their own pace.
+	 *
+	 * PX4 developer guide:
+	 * https://pixhawk.ethz.ch/px4/dev/shared_object_communication
+	 *
+	 * Wikipedia description:
+	 * http://en.wikipedia.org/wiki/Publish–subscribe_pattern
+	 * 
+	 */
+
+
+
+
+	/*
+	 * Declare and safely initialize all structs to zero.
+	 * 
+	 * These structs contain the system state and things
+	 * like attitude, position, the current waypoint, etc.
+	 */
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -192,20 +260,24 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	struct vehicle_global_position_setpoint_s global_sp;
 	memset(&global_sp, 0, sizeof(global_sp));
 
-	/* output structs */
+	/* output structs - this is what is sent to the mixer */
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
 
-	/* publish actuator controls */
+	/* publish actuator controls with zero values */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
 		actuators.control[i] = 0.0f;
 	}
 
+	/*
+	 * Advertise that this controller will publish actuator
+	 * control values and the rate setpoint
+	 */
 	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 
-	/* subscribe */
+	/* subscribe to topics. */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -215,8 +287,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* Setup of loop */
-	float gyro[3] = {0.0f, 0.0f, 0.0f};
 	float speed_body[3] = {0.0f, 0.0f, 0.0f};
+	/* RC failsafe check */
+	bool throttle_half_once = false;
 	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
 				{ .fd = att_sub, .events = POLLIN }};
 
@@ -235,7 +308,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		int ret = poll(fds, 2, 500);
 
 		if (ret < 0) {
-			/* poll error, this will not really happen in practice */
+			/*
+			 * Poll error, this will not really happen in practice,
+			 * but its good design practice to make output an error message. 
+			 */
 			warnx("poll error");
 
 		} else if (ret == 0) {
@@ -261,6 +337,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				orb_check(global_pos_sub, &pos_updated);
 				bool global_sp_updated;
 				orb_check(global_sp_sub, &global_sp_updated);
+				bool manual_sp_updated;
+				orb_check(manual_sp_sub, &manual_sp_updated);
 
 				/* get a local copy of attitude */
 				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -268,6 +346,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				if (global_sp_updated)
 					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
 
+				/* currently speed in body frame is not used, but here for reference */
 				if (pos_updated) {
 					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
 
@@ -285,15 +364,23 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					}
 				}
 
-				orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+				if (manual_sp_updated)
+					/* get the RC (or otherwise user based) input */ 
+					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
+				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
+				if (isfinite(manual_sp.throttle) &&
+							    (manual_sp.throttle >= 0.6f) &&
+							    (manual_sp.throttle <= 1.0f)) {
+					throttle_half_once = true;
+				}
+
+				/* get the system status and the flight mode we're in */
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 
 				/* control */
 
+				/* if in auto mode, fly global position setpoint */
 				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
 				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
 
@@ -305,7 +392,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				    	actuators.control[2] = 0.0f;
 					
 					/* simple attitude control */
-					control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+					control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
 
 					/* pass through throttle */
 					actuators.control[3] = att_sp.thrust;
@@ -313,11 +400,12 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					/* set flaps to zero */
 					actuators.control[4] = 0.0f;
 
+				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
 				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
 					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (vstatus.rc_signal_lost) {
+						if (vstatus.rc_signal_lost && throttle_half_once) {
 
 							/* put plane into loiter */
 							att_sp.roll_body = 0.3f;
@@ -348,7 +436,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						att_sp.timestamp = hrt_absolute_time();
 
 						/* attitude control */
-						control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+						control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
 
 						/* pass through throttle */
 						actuators.control[3] = att_sp.thrust;
diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c
index 8042c74f5..c2e94ff3d 100644
--- a/src/examples/fixedwing_control/params.c
+++ b/src/examples/fixedwing_control/params.c
@@ -45,7 +45,7 @@
 /**
  *
  */
-PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
+PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f);
 
 /**
  *
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
new file mode 100644
index 000000000..a80bf9cb8
--- /dev/null
+++ b/src/modules/gpio_led/gpio_led.c
@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 gpio_led.c
+ *
+ * Status LED via GPIO driver.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <stdbool.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+#include <systemlib/systemlib.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <poll.h>
+#include <drivers/drv_gpio.h>
+
+struct gpio_led_s {
+	struct work_s work;
+	int gpio_fd;
+	int pin;
+	struct vehicle_status_s status;
+	int vehicle_status_sub;
+	bool led_state;
+	int counter;
+};
+
+static struct gpio_led_s gpio_led_data;
+
+__EXPORT int gpio_led_main(int argc, char *argv[]);
+
+void gpio_led_start(FAR void *arg);
+
+void gpio_led_cycle(FAR void *arg);
+
+int gpio_led_main(int argc, char *argv[])
+{
+	int pin = GPIO_EXT_1;
+
+	if (argc > 1) {
+		if (!strcmp(argv[1], "-p")) {
+			if (!strcmp(argv[2], "1")) {
+				pin = GPIO_EXT_1;
+
+			} else if (!strcmp(argv[2], "2")) {
+				pin = GPIO_EXT_2;
+
+			} else {
+				printf("[gpio_led] Unsupported pin: %s\n", argv[2]);
+				exit(1);
+			}
+		}
+	}
+
+	memset(&gpio_led_data, 0, sizeof(gpio_led_data));
+	gpio_led_data.pin = pin;
+	int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
+
+	if (ret != 0) {
+		printf("[gpio_led] Failed to queue work: %d\n", ret);
+		exit(1);
+	}
+
+	exit(0);
+}
+
+void gpio_led_start(FAR void *arg)
+{
+	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
+
+	/* open GPIO device */
+	priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (priv->gpio_fd < 0) {
+		printf("[gpio_led] GPIO: open fail\n");
+		return;
+	}
+
+	/* configure GPIO pin */
+	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
+
+	/* subscribe to vehicle status topic */
+	memset(&priv->status, 0, sizeof(priv->status));
+	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* add worker to queue */
+	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
+
+	if (ret != 0) {
+		printf("[gpio_led] Failed to queue work: %d\n", ret);
+		return;
+	}
+
+	printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
+}
+
+void gpio_led_cycle(FAR void *arg)
+{
+	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
+
+	/* check for status updates*/
+	bool status_updated;
+	orb_check(priv->vehicle_status_sub, &status_updated);
+
+	if (status_updated)
+		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
+
+	/* select pattern for current status */
+	int pattern = 0;
+
+	if (priv->status.flag_system_armed) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+			pattern = 0x3f;	// ****** solid (armed)
+
+		} else {
+			pattern = 0x2A;	// *_*_*_ fast blink (armed, battery warning)
+		}
+
+	} else {
+		if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+			pattern = 0x00;	// ______ off (disarmed, preflight check)
+
+		} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
+			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
+
+		} else {
+			pattern = 0x28;	// *_*___ slow double blink (disarmed, not good to arm)
+		}
+	}
+
+	/* blink pattern */
+	bool led_state_new = (pattern & (1 << priv->counter)) != 0;
+
+	if (led_state_new != priv->led_state) {
+		priv->led_state = led_state_new;
+
+		if (led_state_new) {
+			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
+
+		} else {
+			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
+		}
+	}
+
+	priv->counter++;
+
+	if (priv->counter > 5)
+		priv->counter = 0;
+
+	/* repeat cycle at 5 Hz*/
+	work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+}
diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk
new file mode 100644
index 000000000..3b8553489
--- /dev/null
+++ b/src/modules/gpio_led/module.mk
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Status LED via GPIO driver
+#
+
+MODULE_COMMAND	= gpio_led
+SRCS			= gpio_led.c