Merge remote-tracking branch 'upstream/master' into eagletree

22 files changed, 418 insertions, 308 deletions

diff --git a/ROMFS/Makefile b/ROMFS/Makefile
index 15167bf47..ed39ab825 100644
--- a/ROMFS/Makefile
+++ b/ROMFS/Makefile
@@ -22,6 +22,8 @@ ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
 		   $(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \
 		   $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \
 		   $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \
+		   $(SRCROOT)/scripts/rc.usb~init.d/rc.usb \
+		   $(SRCROOT)/scripts/rc.hil~init.d/rc.hil \
 		   $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \
 		   $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \
 		   $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \
diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix
index cf64ef568..ebcb66b24 100644
--- a/ROMFS/mixers/FMU_Q.mix
+++ b/ROMFS/mixers/FMU_Q.mix
@@ -25,12 +25,12 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   5000   8000      0 -10000  10000
+S: 0 0  -5000  -8000      0 -10000  10000
 S: 0 1   8000   8000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   8000   5000      0 -10000  10000
+S: 0 0  -8000  -5000      0 -10000  10000
 S: 0 1  -8000  -8000      0 -10000  10000
 
 Output 2
diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO
index 1e3963b9a..625f23bdd 100644
--- a/ROMFS/scripts/rc.PX4IO
+++ b/ROMFS/scripts/rc.PX4IO
@@ -5,7 +5,7 @@ set USB no
 set MODE camflyer
  
 #
-# Start the ORB
+# Start the ORB (first app to start)
 #
 uorb start
  
@@ -27,38 +27,65 @@ fi
 param set MAV_TYPE 1
  
 #
-# Start the sensors.
+# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
 #
-sh /etc/init.d/rc.sensors
+if [ -f /fs/microsd/px4io.bin ]
+then
+	echo "PX4IO Firmware found. Checking Upgrade.."
+	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+	then
+		echo "No newer version, skipping upgrade."
+	else
+		echo "Loading /fs/microsd/px4io.bin"
+		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+		then
+			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+		else
+			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+		fi
+	fi
+fi
  
 #
-# Start MAVLink
+# Start MAVLink (depends on orb)
 #
 mavlink start -d /dev/ttyS1 -b 57600
 usleep 5000
  
 #
-# Start the commander.
+# Start the commander (depends on orb, mavlink)
 #
 commander start
  
 #
-# Start GPS interface
+# Start PX4IO interface (depends on orb, commander)
 #
-gps start
+px4io start
  
 #
-# Start the attitude estimator
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+ 
 #
-kalman_demo start
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
  
 #
-# Start PX4IO interface
+# Start GPS interface (depends on orb)
 #
-px4io start
+gps start
+ 
+#
+# Start the attitude estimator (depends on orb)
+#
+kalman_demo start
  
 #
-# Load mixer and start controllers
+# Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 control_demo start
@@ -66,8 +93,8 @@ control_demo start
 #
 # Start logging
 #
-sdlog start -s 10
-
+#sdlog start -s 4
+ 
 #
 # Start system state
 #
@@ -77,4 +104,4 @@ then
 	blinkm systemstate
 else
 	echo "no BlinkM found, OK."
-fi
+fi
\ No newline at end of file
diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR
index 72df68e35..6af91992e 100644
--- a/ROMFS/scripts/rc.PX4IOAR
+++ b/ROMFS/scripts/rc.PX4IOAR
@@ -2,10 +2,10 @@
 #
 # Flight startup script for PX4FMU on PX4IOAR carrier board.
 #
- 
+
 # Disable the USB interface
 set USB no
- 
+
 # Disable autostarting other apps
 set MODE ardrone
  
@@ -25,13 +25,18 @@ if [ -f /fs/microsd/parameters ]
 then
 	param load /fs/microsd/parameters
 fi
- 
+
 #
 # Force some key parameters to sane values
 # MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
 #              see https://pixhawk.ethz.ch/mavlink/
 #
 param set MAV_TYPE 2
+
+#
+# Configure PX4FMU for operation with PX4IOAR
+#
+fmu mode_gpio_serial
  
 #
 # Start the sensors.
@@ -55,11 +60,6 @@ commander start
 attitude_estimator_ekf start
  
 #
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
- 
-#
 # Fire up the multi rotor attitude controller
 #
 multirotor_att_control start
@@ -68,11 +68,6 @@ multirotor_att_control start
 # Fire up the AR.Drone interface.
 #
 ardrone_interface start -d /dev/ttyS1
- 
-#
-# Start GPS capture
-#
-gps start
 
 #
 # Start logging
@@ -80,6 +75,11 @@ gps start
 sdlog start -s 10
 
 #
+# Start GPS capture
+#
+gps start
+
+#
 # Start system state
 #
 if blinkm start
@@ -95,4 +95,5 @@ fi
 # use the same UART for telemetry
 #
 echo "[init] startup done"
+
 exit
\ No newline at end of file
diff --git a/ROMFS/scripts/rc.hil b/ROMFS/scripts/rc.hil
new file mode 100644
index 000000000..980b78edd
--- /dev/null
+++ b/ROMFS/scripts/rc.hil
@@ -0,0 +1,68 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] starting.."
+
+uorb start
+
+# Tell MAVLink that this link is "fast"
+mavlink start -b 230400 -d /dev/console
+
+# Create a fake HIL /dev/pwm_output interface
+hil mode_pwm
+
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/parameters
+if [ -f /fs/microsd/parameters ]
+then
+	param load /fs/microsd/parameters
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+#              see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 1
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if [ px4io start ]
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+end
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start the attitude estimator (depends on orb)
+#
+kalman_demo start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
+control_demo start
+
+echo "[HIL] setup done, running"
+
+# Exit shell to make it available to MAVLink
+exit 
diff --git a/ROMFS/scripts/rc.usb b/ROMFS/scripts/rc.usb
new file mode 100644
index 000000000..31af3991a
--- /dev/null
+++ b/ROMFS/scripts/rc.usb
@@ -0,0 +1,14 @@
+#!nsh
+#
+# USB MAVLink start
+#
+
+echo "Starting MAVLink on this USB console"
+
+# Tell MAVLink that this link is "fast"
+mavlink start -b 230400 -d /dev/console
+
+echo "MAVLink started, exiting shell.."
+
+# Exit shell to make it available to MAVLink
+exit 
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS
index 89a767879..c0a70f7dd 100755
--- a/ROMFS/scripts/rcS
+++ b/ROMFS/scripts/rcS
@@ -21,9 +21,9 @@ set MODE autostart
 set USB autoconnect
 
 #
-# Start playing the startup tune
+
 #
-tone_alarm start
+
 
 #
 # Try to mount the microSD card.
@@ -32,8 +32,12 @@ echo "[init] looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	echo "[init] card mounted at /fs/microsd"
+	# Start playing the startup tune
+	tone_alarm start
 else
 	echo "[init] no microSD card found"
+	# Play SOS
+	tone_alarm 2
 fi
 
 #
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index 7f5711c47..1cc28b9dd 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -86,113 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r);
 }
 
-BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_psi2Phi(this, "PSI2PHI"),
-	_phi2P(this, "PHI2P"),
-	_phiLimit(this, "PHI_LIM")
-{
-}
-
-BlockHeadingHold::~BlockHeadingHold() {};
-
-float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
-{
-	float psiError = _wrap_pi(psiCmd - psi);
-	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
-	return _phi2P.update(phiCmd - phi);
-}
-
-BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_v2Theta(this, "V2THE"),
-	_theta2Q(this, "THE2Q"),
-	_theLimit(this, "THE"),
-	_vLimit(this, "V")
-{
-}
-
-BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
-
-float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
-{
-	// negative sign because nose over to increase speed
-	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
-	return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_v2Thr(this, "V2THR")
-{
-}
-
-BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
-
-float BlockVelocityHoldFrontside::update(float vCmd, float v)
-{
-	return _v2Thr.update(vCmd - v);
-}
-
-BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_h2Thr(this, "H2THR"),
-	_throttle(0)
-{
-}
-
-BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
-
-void BlockAltitudeHoldBackside::update(float hCmd, float h)
-{
-	_throttle = _h2Thr.update(hCmd - h);
-}
-
-BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_h2Theta(this, "H2THE"),
-	_theta2Q(this, "THE2Q")
-{
-}
-
-BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
-
-float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
-{
-	float thetaCmd = _h2Theta.update(hCmd - h);
-	return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
-		const char *name,
-		BlockStabilization *stabilization) :
-	SuperBlock(parent, name),
-	_stabilization(stabilization),
-	_headingHold(this, ""),
-	_velocityHold(this, ""),
-	_altitudeHold(this, ""),
-	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
-	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
-	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
-	_trimThr(this, "TRIM_THR", true) /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
-{
-}
-
-BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
-
-void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
-				    float h, float v,
-				    float phi, float theta, float psi,
-				    float p, float q, float r)
-{
-	_altitudeHold.update(hCmd, h);
-	_stabilization->update(
-		_headingHold.update(psiCmd, phi, psi, p),
-		_velocityHold.update(vCmd, v, theta, q),
-		rCmd,
-		p, q, r);
-}
-
 BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	_xtYawLimit(this, "XT2YAW"),
@@ -251,9 +144,33 @@ BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
-	_backsideAutopilot(this, "", &_stabilization),
+
+	// heading hold
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM"),
+
+	// velocity hold
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V"),
+
+	// altitude/roc hold
+	_h2Thr(this, "H2THR"),
+	_roc2Thr(this, "ROC2THR"),
+
+	// guidance block
 	_guide(this, ""),
+
+	// block params
+	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
+	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
+	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
+	_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+
 	_vCmd(this, "V_CMD"),
+	_rocMax(this, "ROC_MAX"),
 	_attPoll(),
 	_lastPosCmd(),
 	_timeStamp(0)
@@ -305,7 +222,7 @@ void BlockMultiModeBacksideAutopilot::update()
 
 	// handle autopilot modes
 	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-		_status.state_machine == SYSTEM_STATE_STABILIZED) {
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -313,19 +230,32 @@ void BlockMultiModeBacksideAutopilot::update()
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
 
-		// commands
+		// altitude hold
+		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+
+		// heading hold
+		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
+		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+		// velocity hold
+		// negative sign because nose over to increase speed
+		float thetaCmd = _theLimit.update(-_v2Theta.update(
+				_vLimit.update(_vCmd.get()) - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+		// yaw rate cmd
 		float rCmd = 0;
 
-		_backsideAutopilot.update(
-			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
-			_pos.alt, v,
-			_att.roll, _att.pitch, _att.yaw,
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed
-		);
-		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
-		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
-		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
-		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+		_actuators.control[CH_THR] = dThrottle + _trimThr.get();
 
 		// XXX limit throttle to manual setting (safety) for now.
 		// If it turns out to be confusing, it can be removed later once
@@ -336,14 +266,12 @@ void BlockMultiModeBacksideAutopilot::update()
 		if (!_status.flag_hil_enabled) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							_actuators.control[CH_THR] : _manual.throttle;
+						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-
 	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
 
 		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
 			_actuators.control[CH_AIL] = _manual.roll;
 			_actuators.control[CH_ELV] = _manual.pitch;
 			_actuators.control[CH_RDR] = _manual.yaw;
@@ -351,8 +279,60 @@ void BlockMultiModeBacksideAutopilot::update()
 
 		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
+			// calculate velocity, XXX should be airspeed, but using ground speed for now
+			float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+			// pitch channel -> rate of climb
+			// TODO, might want to put a gain on this, otherwise commanding
+			// from +1 -> -1 m/s for rate of climb
+			//float dThrottle = _roc2Thr.update(
+			//_rocMax.get()*_manual.pitch - _pos.vz);
+
+			// roll channel -> bank angle
+			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+			float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+			// throttle channel -> velocity
+			// negative sign because nose over to increase speed
+			float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
+			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+			// yaw rate cmd
+			float rCmd = 0;
+
+			// stabilization
+			_stabilization.update(pCmd, qCmd, rCmd,
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+			// output
+			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+			// currenlty using manual throttle
+			// XXX if you enable this watch out, vz might be very noisy
+			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+			_actuators.control[CH_THR] = _manual.throttle;
+
+			// XXX limit throttle to manual setting (safety) for now.
+			// If it turns out to be confusing, it can be removed later once
+			// a first binary release can be targeted.
+			// This is not a hack, but a design choice.
+
+			/* do not limit in HIL */
+			if (!_status.flag_hil_enabled) {
+				/* limit to value of manual throttle */
+				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							     _actuators.control[CH_THR] : _manual.throttle;
+			}
+		}
+
+		// body rates controller, disabled for now
+		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+
 			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
 
 			_actuators.control[CH_AIL] = _stabilization.getAileron();
 			_actuators.control[CH_ELV] = _stabilization.getElevator();
diff --git a/apps/controllib/fixedwing.hpp b/apps/controllib/fixedwing.hpp
index ea742641c..281cbb4cb 100644
--- a/apps/controllib/fixedwing.hpp
+++ b/apps/controllib/fixedwing.hpp
@@ -233,26 +233,6 @@ public:
 };
 
 /**
- * Heading hold autopilot block.
- * Aircraft Control and Simulation, Stevens and Lewis
- * Heading hold, pg. 348
- */
-class __EXPORT BlockHeadingHold : public SuperBlock
-{
-private:
-	BlockP _psi2Phi;
-	BlockP _phi2P;
-	BlockLimitSym _phiLimit;
-public:
-	BlockHeadingHold(SuperBlock *parent, const char *name);
-	virtual ~BlockHeadingHold();
-	/**
-	 * returns pCmd
-	 */
-	float update(float psiCmd, float phi, float psi, float p);
-};
-
-/**
  * Frontside/ Backside Control Systems
  *
  * Frontside :
@@ -270,106 +250,6 @@ public:
  */
 
 /**
- * Backside velocity hold autopilot block.
- * v -> theta -> q -> elevator
- */
-class __EXPORT BlockVelocityHoldBackside : public SuperBlock
-{
-private:
-	BlockPID _v2Theta;
-	BlockPID _theta2Q;
-	BlockLimit _theLimit;
-	BlockLimit _vLimit;
-public:
-	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
-	virtual ~BlockVelocityHoldBackside();
-	/**
-	 * returns qCmd
-	 */
-	float update(float vCmd, float v, float theta, float q);
-};
-
-/**
- * Frontside velocity hold autopilot block.
- * v -> throttle
- */
-class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
-{
-private:
-	BlockPID _v2Thr;
-public:
-	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
-	virtual ~BlockVelocityHoldFrontside();
-	/**
-	 * returns throttle
-	 */
-	float update(float vCmd, float v);
-};
-
-/**
- * Backside altitude hold autopilot block.
- * h -> throttle
- */
-class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
-{
-private:
-	BlockPID _h2Thr;
-	float _throttle;
-public:
-	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
-	virtual ~BlockAltitudeHoldBackside();
-	void update(float hCmd, float h);
-	float getThrottle() { return _throttle; }
-};
-
-/**
- * Frontside altitude hold autopilot block.
- * h -> theta > q -> elevator
- */
-class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
-{
-private:
-	BlockPID _h2Theta;
-	BlockPID _theta2Q;
-public:
-	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
-	virtual ~BlockAltitudeHoldFrontside();
-	/**
-	 * return qCmd
-	 */
-	float update(float hCmd, float h, float theta, float q);
-};
-
-/**
- * Backside autopilot
- */
-class __EXPORT BlockBacksideAutopilot : public SuperBlock
-{
-private:
-	BlockStabilization *_stabilization;
-	BlockHeadingHold _headingHold;
-	BlockVelocityHoldBackside _velocityHold;
-	BlockAltitudeHoldBackside _altitudeHold;
-	BlockParam<float> _trimAil;
-	BlockParam<float> _trimElv;
-	BlockParam<float> _trimRdr;
-	BlockParam<float> _trimThr;
-public:
-	BlockBacksideAutopilot(SuperBlock *parent,
-			       const char *name,
-			       BlockStabilization *stabilization);
-	virtual ~BlockBacksideAutopilot();
-	void update(float hCmd, float vCmd, float rCmd, float psiCmd,
-		    float h, float v,
-		    float phi, float theta, float psi,
-		    float p, float q, float r);
-	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
-	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
-	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
-	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
-};
-
-/**
  * Waypoint Guidance block
  */
 class __EXPORT BlockWaypointGuidance : public SuperBlock
@@ -416,10 +296,34 @@ public:
 class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
 {
 private:
+	// stabilization
 	BlockStabilization _stabilization;
-	BlockBacksideAutopilot _backsideAutopilot;
+
+	// heading hold
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+
+	// velocity hold
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+
+	// altitude/ roc hold
+	BlockPID _h2Thr;
+	BlockPID _roc2Thr;
+
+	// guidance
 	BlockWaypointGuidance _guide;
+
+	// block params
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
 	BlockParam<float> _vCmd;
+	BlockParam<float> _rocMax;
 
 	struct pollfd _attPoll;
 	vehicle_global_position_setpoint_s _lastPosCmd;
diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c
index 70245a3ec..7a02eaeb7 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_spi.c
@@ -92,21 +92,21 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	case PX4_SPIDEV_GYRO:
 		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL:
 		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		break;
 
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
@@ -125,7 +125,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
 }
 
 __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp
index 6227df72a..c7f433dd4 100644
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ b/apps/drivers/l3gd20/l3gd20.cpp
@@ -684,9 +684,10 @@ L3GD20::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 	report->timestamp = hrt_absolute_time();
-	/* XXX adjust for sensor alignment to board here */
-	report->x_raw = raw_report.x;
-	report->y_raw = raw_report.y;
+	
+	/* swap x and y and negate y */
+	report->x_raw = raw_report.y;
+	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
 	report->z_raw = raw_report.z;
 
 	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 83e01e63c..428b779b1 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -56,4 +56,16 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f);       // minimum commanded velocity
 PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f);       // commanded velocity
 PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f);       // maximum commanded velocity
 
+// rate of climb
+// this is what rate of climb is commanded (in m/s) 
+// when the pitch stick is fully defelcted in simple mode
+PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+
+//  rate of climb -> thr
+PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f);   // rate of climb to throttle PID
+PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
+
 PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 955e77b3e..6df454a55 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -61,8 +61,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	HAtt(6, 9),
 	RAtt(6, 6),
 	// position measurement ekf matrices
-	HPos(5, 9),
-	RPos(5, 5),
+	HPos(6, 9),
+	RPos(6, 6),
 	// attitude representations
 	C_nb(),
 	q(),
@@ -95,6 +95,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsVel(this, "R_GPS_VEL"),
 	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
+	_rPressAlt(this, "R_PRESS_ALT"),
 	_rAccel(this, "R_ACCEL"),
 	_magDip(this, "ENV_MAG_DIP"),
 	_magDec(this, "ENV_MAG_DEC"),
@@ -134,6 +135,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
 	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
 	HPos(4, 8) = 1.0f;
+	HPos(5, 8) = 1.0f;
 
 	// initialize all parameters
 	updateParams();
@@ -192,7 +194,7 @@ void KalmanNav::update()
 	    gpsUpdate &&
 	    _gps.fix_type > 2
 	    //&& _gps.counter_pos_valid > 10
-	    ) {
+	   ) {
 		vN = _gps.vel_n_m_s;
 		vE = _gps.vel_e_m_s;
 		vD = _gps.vel_d_m_s;
@@ -630,12 +632,13 @@ int KalmanNav::correctPos()
 	using namespace math;
 
 	// residual
-	Vector y(5);
+	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;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -685,7 +688,8 @@ int KalmanNav::correctPos()
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
 		       double(y(3) / sqrtf(RPos(3, 3))),
-		       double(y(4) / sqrtf(RPos(4, 4))));
+		       double(y(4) / sqrtf(RPos(4, 4))),
+		       double(y(5) / sqrtf(RPos(5, 5))));
 	}
 
 	return ret_ok;
@@ -740,7 +744,8 @@ void KalmanNav::updateParams()
 	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
-	float noiseAlt = _rGpsAlt.get();
+	float noiseGpsAlt = _rGpsAlt.get();
+	float noisePressAlt = _rPressAlt.get();
 
 	// bound noise to prevent singularities
 	if (noiseVel < noiseMin) noiseVel = noiseMin;
@@ -749,13 +754,16 @@ void KalmanNav::updateParams()
 
 	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
 
-	if (noiseAlt < noiseMin) noiseAlt = noiseMin;
+	if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
+
+	if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
 
 	RPos(0, 0) = noiseVel * noiseVel; // vn
 	RPos(1, 1) = noiseVel * noiseVel; // ve
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
-	RPos(4, 4) = noiseAlt * noiseAlt; // h
+	RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
+	RPos(5, 5) = noisePressAlt * noisePressAlt; // h
 	// XXX, note that RPos depends on lat, so updateParams should
 	// be called if lat changes significantly
 }
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index 7709ac697..c2bf18115 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -159,6 +159,7 @@ protected:
 	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
 	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
 	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParam<float> _rPressAlt;  /**< press altitude measurement noise */
 	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
 	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
 	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 3bfe01261..50642f067 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,16 +1,16 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
 PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
 PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
-
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index b958d5f96..644b779af 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -442,7 +442,8 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	int termios_state;
 	*is_usb = false;
 
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
+	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
 		/* Back up the original uart configuration to restore it after exit */
 		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
 			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 4d9cd964d..798e509e0 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -308,6 +308,14 @@ handle_message(mavlink_message_t *msg)
 
 		uint64_t timestamp = hrt_absolute_time();
 
+		/* TODO, set ground_press/ temp during calib */
+		static const float ground_press = 1013.25f; // mbar
+		static const float ground_tempC = 21.0f;
+		static const float ground_alt = 0.0f;
+		static const float T0 = 273.15;
+		static const float R = 287.05f;
+		static const float g = 9.806f;
+
 		if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
 
 			mavlink_raw_imu_t imu;
@@ -376,6 +384,83 @@ handle_message(mavlink_message_t *msg)
 			}
 		}
 
+		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
+
+			mavlink_highres_imu_t imu;
+			mavlink_msg_highres_imu_decode(msg, &imu);
+
+			/* packet counter */
+			static uint16_t hil_counter = 0;
+			static uint16_t hil_frames = 0;
+			static uint64_t old_timestamp = 0;
+
+			/* sensors general */
+			hil_sensors.timestamp = imu.time_usec;
+
+			/* hil gyro */
+			static const float mrad2rad = 1.0e-3f;
+			hil_sensors.gyro_counter = hil_counter;
+			hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
+			hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
+			hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
+			hil_sensors.gyro_rad_s[0] = imu.xgyro;
+			hil_sensors.gyro_rad_s[1] = imu.ygyro;
+			hil_sensors.gyro_rad_s[2] = imu.zgyro;
+
+			/* accelerometer */
+			hil_sensors.accelerometer_counter = hil_counter;
+			static const float mg2ms2 = 9.8f / 1000.0f;
+			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
+			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
+			hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
+			hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+			hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+			hil_sensors.accelerometer_m_s2[2] = imu.zacc;
+			hil_sensors.accelerometer_mode = 0; // TODO what is this?
+			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+
+			/* adc */
+			hil_sensors.adc_voltage_v[0] = 0;
+			hil_sensors.adc_voltage_v[1] = 0;
+			hil_sensors.adc_voltage_v[2] = 0;
+
+			/* magnetometer */
+			float mga2ga = 1.0e-3f;
+			hil_sensors.magnetometer_counter = hil_counter;
+			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
+			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
+			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
+			hil_sensors.magnetometer_ga[0] = imu.xmag;
+			hil_sensors.magnetometer_ga[1] = imu.ymag;
+			hil_sensors.magnetometer_ga[2] = imu.zmag;
+			hil_sensors.magnetometer_range_ga = 32.7f; // int16
+			hil_sensors.magnetometer_mode = 0; // TODO what is this
+			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+
+			hil_sensors.baro_pres_mbar = imu.abs_pressure;
+
+			float tempC =  imu.temperature;
+			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
+			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
+
+			hil_sensors.baro_alt_meter = h;
+			hil_sensors.baro_temp_celcius = imu.temperature;
+
+			/* publish */
+			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+			// increment counters
+			hil_counter += 1 ;
+			hil_frames += 1 ;
+
+			// output
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil imu at %d hz\n", hil_frames/10);
+				old_timestamp = timestamp;
+				hil_frames = 0;
+			}
+		}
+
 		if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
 
 			mavlink_gps_raw_int_t gps;
@@ -435,13 +520,6 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.timestamp = press.time_usec;
 
 			/* baro */
-			/* TODO, set ground_press/ temp during calib */
-			static const float ground_press = 1013.25f; // mbar
-			static const float ground_tempC = 21.0f;
-			static const float ground_alt = 0.0f;
-			static const float T0 = 273.15;
-			static const float R = 287.05f;
-			static const float g = 9.806f;
 
 			float tempC =  press.temperature / 100.0f;
 			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
diff --git a/apps/px4/tests/test_hott_telemetry.c b/apps/px4/tests/test_hott_telemetry.c
index 74aa0e614..270dc3857 100644
--- a/apps/px4/tests/test_hott_telemetry.c
+++ b/apps/px4/tests/test_hott_telemetry.c
@@ -46,6 +46,7 @@
 #include <drivers/drv_gpio.h>
 #include <nuttx/config.h>
 #include <sys/types.h>
+#include <systemlib/err.h>
 
 #include <debug.h>
 #include <errno.h>
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 9a0f0e5c1..e51983071 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -177,7 +177,7 @@ controls_tick() {
 					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
 
 				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
 
 				} else {
 					/* in the configured dead zone, output zero */
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 0bab992a7..230060148 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
 
 PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 8b6f18473..f9f6de9f7 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1190,7 +1190,7 @@ Sensors::ppm_poll()
 				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
 
 			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
+				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
 
 			} else {
 				/* in the configured dead zone, output zero */
diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c
index f25f812ae..9ac6f0b5e 100644
--- a/apps/systemcmds/preflight_check/preflight_check.c
+++ b/apps/systemcmds/preflight_check/preflight_check.c
@@ -54,6 +54,8 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_baro.h>
 
+#include <mavlink/mavlink_log.h>
+
 __EXPORT int preflight_check_main(int argc, char *argv[]);
 static int led_toggle(int leds, int led);
 static int led_on(int leds, int led);
@@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[])
 	bool system_ok = true;
 
 	int fd;
+	/* open text message output path */
+	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	int ret;
 
 	/* give the system some time to sample the sensors in the background */
@@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[])
 	fd = open(MAG_DEVICE_PATH, 0);
 	if (fd < 0) {
 		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("accel self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("gyro self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
 		system_ok = false;
 		goto system_eval;
 	}