Merge branch 'rc_timeout' into mpc_rc

2026-05-22 01:12:31 +00:00 · 2014-04-13 11:06:49 +04:00
parent dfd9601b57 0b97dd2b77
commit 88149311ea
31 changed files with 858 additions and 696 deletions
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -87,6 +87,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>

 #include <drivers/airspeed/airspeed.h>

@@ -121,6 +122,14 @@ protected:
 	virtual int	collect();

 	math::LowPassFilter2p	_filter;
+
+	/**
+	 * Correct for 5V rail voltage variations
+	 */
+	void voltage_correction(float &diff_pres_pa, float &temperature);
+
+	int _t_system_power;
+	struct system_power_s system_power;
 };

 /*
@@ -129,10 +138,11 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);

 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
-			CONVERSION_INTERVAL, path),
-			_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
+	CONVERSION_INTERVAL, path),
+	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
+	_t_system_power(-1)
 {
-
+	memset(&system_power, 0, sizeof(system_power));
 }

 int
@@ -194,19 +204,48 @@ MEASAirspeed::collect()
 	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
-	float temperature = ((200 * dT_raw) / 2047) - 50;
+	float temperature = ((200.0f * dT_raw) / 2047) - 50;

-	/* calculate differential pressure. As its centered around 8000
-	 * and can go positive or negative, enforce absolute value
-	*/
+	// Calculate differential pressure. As its centered around 8000
+	// and can go positive or negative
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+	const float PSI_to_Pa = 6894.757f;
+	/*
+	  this equation is an inversion of the equation in the
+	  pressure transfer function figure on page 4 of the datasheet

-	if (diff_press_pa < 0.0f) {
-		diff_press_pa = 0.0f;
-	}
+	  We negate the result so that positive differential pressures
+	  are generated when the bottom port is used as the static
+	  port on the pitot and top port is used as the dynamic port
+	 */
+	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
+	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;

+        // correct for 5V rail voltage if possible
+        voltage_correction(diff_press_pa_raw, temperature);
+
+	float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
+	
+	/*
+	  note that we return both the absolute value with offset
+	  applied and a raw value without the offset applied. This
+	  makes it possible for higher level code to detect if the
+	  user has the tubes connected backwards, and also makes it
+	  possible to correctly use offsets calculated by a higher
+	  level airspeed driver.
+
+	  With the above calculation the MS4525 sensor will produce a
+	  positive number when the top port is used as a dynamic port
+	  and bottom port is used as the static port
+
+	  Also note that the _diff_pres_offset is applied before the
+	  fabsf() not afterwards. It needs to be done this way to
+	  prevent a bias at low speeds, but this also means that when
+	  setting a offset you must set it based on the raw value, not
+	  the offset value
+	 */
+	
 	struct differential_pressure_s report;

 	/* track maximum differential pressure measured (so we can work out top speed). */
@@ -219,6 +258,13 @@ MEASAirspeed::collect()
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
+
+	/* the dynamics of the filter can make it overshoot into the negative range */
+	if (report.differential_pressure_filtered_pa < 0.0f) {
+		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
+	}
+
+	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;

@@ -287,6 +333,62 @@ MEASAirspeed::cycle()
 		   USEC2TICK(CONVERSION_INTERVAL));
 }

+/**
+   correct for 5V rail voltage if the system_power ORB topic is
+   available
+   
+   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
+   offset versus voltage for 3 sensors
+ */
+void
+MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	if (_t_system_power == -1) {
+		_t_system_power = orb_subscribe(ORB_ID(system_power));
+	}
+	if (_t_system_power == -1) {
+		// not available
+		return;
+	}
+	bool updated = false;
+	orb_check(_t_system_power, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
+	}
+	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
+		// not valid, skip correction
+		return;
+	}
+
+	const float slope = 65.0f;
+	/*
+	  apply a piecewise linear correction, flattening at 0.5V from 5V
+	 */
+	float voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -0.5f) {
+		voltage_diff = -0.5f;
+	}
+	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+	temperature -= voltage_diff * temp_slope;	
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+}
+
 /**
 * Local functions in support of the shell command.
 */
@@ -409,7 +511,7 @@ test()
 	}

 	warnx("single read");
-	warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);

 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -437,7 +539,7 @@ test()
 		}

 		warnx("periodic read %u", i);
-		warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}

--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -944,8 +944,23 @@ PX4IO::task_main()
 				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);

 				if (pret != OK) {
-					log("voltage scaling upload failed");
+					log("vscale upload failed");
 				}
+
+				/* send RC throttle failsafe value to IO */
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
+
+				if (failsafe_param > 0) {
+
+					param_get(failsafe_param, &failsafe_param_val);
+					uint16_t failsafe_thr = failsafe_param_val;
+					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+					if (pret != OK) {
+						log("failsafe upload failed");
+					}
+				}
+
 			}

 		}
@@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;

-		/* we do not know the RC input, only publish if RC OK flag is set */
-		/* if no raw RC, just don't publish */
-		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		/* only keep publishing RC input if we ever got a valid input */
+		if (_rc_last_valid == 0) {
+			/* we have never seen valid RC signals, abort */
 			return OK;
+		}
 	}

 	/* lazily advertise on first publication */
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -124,6 +124,8 @@ private:

 	orb_advert_t			_range_finder_topic;

+	unsigned			_consecutive_fail_count;
+
 	perf_counter_t			_sample_perf;
 	perf_counter_t			_comms_errors;
 	perf_counter_t			_buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_last_read(0),
 	_range_finder_topic(-1),
+	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -720,12 +723,17 @@ SF0X::cycle()
 		if (OK != collect_ret) {

 			/* we know the sensor needs about four seconds to initialize */
-			if (hrt_absolute_time() > 5 * 1000 * 1000LL) {
-				log("collection error");
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				log("collection error #%u", _consecutive_fail_count);
 			}
+			_consecutive_fail_count++;
+
 			/* restart the measurement state machine */
 			start();
 			return;
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
 		}

 		/* next phase is measurement */
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -41,6 +41,7 @@
 */

 #include <nuttx/config.h>
+#include <board_config.h>
 #include <drivers/device/device.h>

 #include <sys/types.h>
@@ -64,6 +65,8 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>

+#include <uORB/topics/system_power.h>
+
 /*
 * Register accessors.
 * For now, no reason not to just use ADC1.
@@ -119,6 +122,8 @@ private:
 	unsigned		_channel_count;
 	adc_msg_s		*_samples;		/**< sample buffer */

+	orb_advert_t		_to_system_power;
+
 	/** work trampoline */
 	static void		_tick_trampoline(void *arg);

@@ -134,13 +139,16 @@ private:
 	 */
 	uint16_t		_sample(unsigned channel);

+	// update system_power ORB topic, only on FMUv2
+	void update_system_power(void);
 };

 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
 	_channel_count(0),
-	_samples(nullptr)
+	_samples(nullptr),
+	_to_system_power(0)
 {
 	_debug_enabled = true;

@@ -290,6 +298,43 @@ ADC::_tick()
 	/* scan the channel set and sample each */
 	for (unsigned i = 0; i < _channel_count; i++)
 		_samples[i].am_data = _sample(_samples[i].am_channel);
+	update_system_power();
+}
+
+void
+ADC::update_system_power(void)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	system_power_s system_power;
+	system_power.timestamp = hrt_absolute_time();
+
+	system_power.voltage5V_v = 0;
+	for (unsigned i = 0; i < _channel_count; i++) {
+		if (_samples[i].am_channel == 4) {
+			// it is 2:1 scaled
+			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
+		}
+	}
+
+	// these are not ADC related, but it is convenient to
+	// publish these to the same topic
+	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+
+	// note that the valid pins are active low
+	system_power.brick_valid   = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
+	system_power.servo_valid   = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
+
+	// OC pins are active low
+	system_power.periph_5V_OC  = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
+	system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
+
+	/* lazily publish */
+	if (_to_system_power > 0) {
+		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+	} else {
+		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
+	}
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }

 uint16_t
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
    // do the filtering
    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
    if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
+        // don't allow bad values to propagate via the filter
        delay_element_0 = sample;
    }
    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
    return output;
 }

+float LowPassFilter2p::reset(float sample) {
+    _delay_element_1 = _delay_element_2 = sample;
+    return apply(sample);
+}
+
 } // namespace math

--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -52,18 +52,30 @@ public:
        _delay_element_1 = _delay_element_2 = 0;
    }

-    // change parameters
+    /**
+     * Change filter parameters
+     */
    void set_cutoff_frequency(float sample_freq, float cutoff_freq);

-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
+    /**
+     * Add a new raw value to the filter
+     *
+     * @return retrieve the filtered result
+     */
    float apply(float sample);

-    // return the cutoff frequency
+    /**
+     * Return the cutoff frequency
+     */
    float get_cutoff_freq(void) const {
        return _cutoff_freq;
    }

+    /**
+     * Reset the filter state to this value
+     */
+    float reset(float sample);
+
 private:
    float           _cutoff_freq; 
    float           _a1;
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");

-	const int calibration_count = 2500;
+	const int calibration_count = 2000;

 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+		} else {
+			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
 		close(fd);
 	}

 	if (!paramreset_successful) {
-		warn("WARNING: failed to set scale / offsets for airspeed sensor");
-		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		warn("FAILED to set scale / offsets for airspeed");
+		mavlink_log_critical(mavlink_fd, "dpress reset failed");
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		return ERROR;
 	}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)

 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;

 			if (calibration_counter % (calibration_count / 20) == 0)
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -67,6 +67,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
@@ -112,8 +113,7 @@ extern struct system_load_s system_load;

 #define MAVLINK_OPEN_INTERVAL 50000

-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
+#define STICK_ON_OFF_LIMIT 0.9f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)

@@ -208,7 +208,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool

 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);

-transition_result_t set_main_state_rc(struct vehicle_status_s *status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);

 void set_control_mode();

@@ -861,6 +861,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));

+	/* Subscribe to position setpoint triplet */
+	int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	struct position_setpoint_triplet_s pos_sp_triplet;
+	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+
 	control_status_leds(&status, &armed, true);

 	/* now initialized */
@@ -1090,6 +1095,13 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}

+		/* update subsystem */
+		orb_check(pos_sp_triplet_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+		}
+
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
@@ -1181,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
 			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
-			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {

 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1199,7 +1211,7 @@ int commander_thread_main(int argc, char *argv[])

 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (status.arming_state == ARMING_STATE_STANDBY &&
-			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1239,11 +1251,8 @@ int commander_thread_main(int argc, char *argv[])
 				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}

-			/* fill status according to mode switches */
-			check_mode_switches(&sp_man, &status);
-
 			/* evaluate the main state machine according to mode switches */
-			res = set_main_state_rc(&status);
+			res = set_main_state_rc(&status, &sp_man);

 			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
@@ -1254,6 +1263,33 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}

+			/* set navigation state */
+			/* RETURN switch, overrides MISSION switch */
+			if (sp_man.return_switch == SWITCH_POS_ON) {
+				/* switch to RTL if not already landed after RTL and home position set */
+				status.set_nav_state = NAV_STATE_RTL;
+				status.set_nav_state_timestamp = hrt_absolute_time();
+
+			} else {
+				/* MISSION switch */
+				if (sp_man.mission_switch == SWITCH_POS_ON) {
+					/* stick is in LOITER position */
+					status.set_nav_state = NAV_STATE_LOITER;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
+					/* stick is in MISSION position */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
+						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+				}
+			}
+
 		} else {
 			if (!status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
@@ -1301,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[])

 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive(armed.armed);
 			}
@@ -1541,76 +1577,29 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 	leds_counter++;
 }

-void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
-{
-	/* main mode switch */
-	if (!isfinite(sp_man->mode_switch)) {
-		/* default to manual if signal is invalid */
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_AUTO;
-
-	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else {
-		status->mode_switch = MODE_SWITCH_ASSISTED;
-	}
-
-	/* return switch */
-	if (!isfinite(sp_man->return_switch)) {
-		status->return_switch = RETURN_SWITCH_NONE;
-
-	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		status->return_switch = RETURN_SWITCH_RETURN;
-
-	} else {
-		status->return_switch = RETURN_SWITCH_NORMAL;
-	}
-
-	/* assisted switch */
-	if (!isfinite(sp_man->assisted_switch)) {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-
-	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		status->assisted_switch = ASSISTED_SWITCH_EASY;
-
-	} else {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-	}
-
-	/* mission switch  */
-	if (!isfinite(sp_man->mission_switch)) {
-		status->mission_switch = MISSION_SWITCH_NONE;
-
-	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		status->mission_switch = MISSION_SWITCH_LOITER;
-
-	} else {
-		status->mission_switch = MISSION_SWITCH_MISSION;
-	}
-}
-
 transition_result_t
-set_main_state_rc(struct vehicle_status_s *status)
+set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
 {
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;

-	switch (status->mode_switch) {
-	case MODE_SWITCH_MANUAL:
+	switch (sp_man->mode_switch) {
+	case SWITCH_POS_NONE:
+		res = TRANSITION_NOT_CHANGED;
+		break;
+
+	case SWITCH_POS_OFF:		// MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;

-	case MODE_SWITCH_ASSISTED:
-		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+	case SWITCH_POS_MIDDLE:		// ASSISTED
+		if (sp_man->assisted_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status, MAIN_STATE_EASY);

-			if (res != TRANSITION_DENIED)
+			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
+			}

 			// else fallback to SEATBELT
 			print_reject_mode(status, "EASY");
@@ -1618,29 +1607,33 @@ set_main_state_rc(struct vehicle_status_s *status)

 		res = main_state_transition(status, MAIN_STATE_SEATBELT);

-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
+		}

-		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+		if (sp_man->assisted_switch != SWITCH_POS_ON) {
 			print_reject_mode(status, "SEATBELT");
+		}

 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;

-	case MODE_SWITCH_AUTO:
+	case SWITCH_POS_ON:			// AUTO
 		res = main_state_transition(status, MAIN_STATE_AUTO);

-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}

 		// else fallback to SEATBELT (EASY likely will not work too)
 		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);

-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}

 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -173,6 +173,8 @@ private:
 		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
 		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
 		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */

 	}		_parameters;			/**< local copies of interesting parameters */

@@ -211,6 +213,8 @@ private:
 		param_t trim_yaw;
 		param_t rollsp_offset_deg;
 		param_t pitchsp_offset_deg;
+		param_t man_roll_max;
+		param_t man_pitch_max;
 	}		_parameter_handles;		/**< handles for interesting parameters */


@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
 	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");

+	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
+	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
 	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
 	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
+	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
+	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
+	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);


 	/* pitch control parameters */
@@ -660,18 +671,24 @@ FixedwingAttitudeControl::task_main()

 				float airspeed;

-				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
+				/* if airspeed is not updating, we assume the normal average speed */
+				if (!isfinite(_airspeed.true_airspeed_m_s) ||
 				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;

 				} else {
-					airspeed = _airspeed.indicated_airspeed_m_s;
+					airspeed = _airspeed.true_airspeed_m_s;
 				}

-				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
-				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+				/*
+				 * For scaling our actuators using anything less than the min (close to stall)
+				 * speed doesn't make any sense - its the strongest reasonable deflection we
+				 * want to do in flight and its the baseline a human pilot would choose.
+				 *
+				 * Forcing the scaling to this value allows reasonable handheld tests.
+				 */
+
+				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);

 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
@@ -700,8 +717,8 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
-					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
+					pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;

@@ -809,7 +826,7 @@ FixedwingAttitudeControl::task_main()
 			} else {
 				/* manual/direct control */
 				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = _manual.pitch;
+				_actuators.control[1] = -_manual.pitch;
 				_actuators.control[2] = _manual.yaw;
 				_actuators.control[3] = _manual.throttle;
 				_actuators.control[4] = _manual.flaps;
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
 // @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
 // @Range		-90.0 to 90.0
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
+
+// @DisplayName		Max Manual Roll
+// @Description		Max roll for manual control in attitude stabilized mode
+// @Range		0.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
+
+// @DisplayName		Max Manual Pitch
+// @Description		Max pitch for manual control in attitude stabilized mode
+// @Range		0.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),

-	_manual_sub(-1),
-
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_attitude_pub(-1),
@@ -222,12 +220,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			vcmd.source_component = msg->compid;
 			vcmd.confirmation =  cmd_mavlink.confirmation;

-			/* check if topic is advertised */
-			if (_cmd_pub <= 0) {
+			if (_cmd_pub < 0) {
 				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);

 			} else {
-				/* publish */
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
 			}
 		}
@@ -254,7 +250,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;

-	if (_flow_pub <= 0) {
+	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);

 	} else {
@@ -288,7 +284,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;

-	if (_cmd_pub <= 0) {
+	if (_cmd_pub < 0) {
 		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);

 	} else {
@@ -313,7 +309,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	vicon_position.pitch = pos.pitch;
 	vicon_position.yaw = pos.yaw;

-	if (_vicon_position_pub <= 0) {
+	if (_vicon_position_pub < 0) {
 		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);

 	} else {
@@ -374,7 +370,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message

 		offboard_control_sp.timestamp = hrt_absolute_time();

-		if (_offboard_control_sp_pub <= 0) {
+		if (_offboard_control_sp_pub < 0) {
 			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);

 		} else {
@@ -402,7 +398,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	tstatus.rxerrors = rstatus.rxerrors;
 	tstatus.fixed = rstatus.fixed;

-	if (_telemetry_status_pub <= 0) {
+	if (_telemetry_status_pub < 0) {
 		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);

 	} else {
@@ -416,47 +412,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	mavlink_manual_control_t man;
 	mavlink_msg_manual_control_decode(msg, &man);

-	/* rc channels */
-	{
-		struct rc_channels_s rc;
-		memset(&rc, 0, sizeof(rc));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));

-		rc.timestamp = hrt_absolute_time();
-		rc.chan_count = 4;
+	manual.timestamp = hrt_absolute_time();
+	manual.roll = man.x / 1000.0f;
+	manual.pitch = man.y / 1000.0f;
+	manual.yaw = man.r / 1000.0f;
+	manual.throttle = man.z / 1000.0f;

-		rc.chan[0].scaled = man.x / 1000.0f;
-		rc.chan[1].scaled = man.y / 1000.0f;
-		rc.chan[2].scaled = man.r / 1000.0f;
-		rc.chan[3].scaled = man.z / 1000.0f;
+	if (_manual_pub < 0) {
+		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);

-		if (_rc_pub == 0) {
-			_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
-
-		} else {
-			orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
-		}
-	}
-
-	/* manual control */
-	{
-		struct manual_control_setpoint_s manual;
-		memset(&manual, 0, sizeof(manual));
-
-		/* get a copy first, to prevent altering values that are not sent by the mavlink command */
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
-
-		manual.timestamp = hrt_absolute_time();
-		manual.roll = man.x / 1000.0f;
-		manual.pitch = man.y / 1000.0f;
-		manual.yaw = man.r / 1000.0f;
-		manual.throttle = man.z / 1000.0f;
-
-		if (_manual_pub == 0) {
-			_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
-
-		} else {
-			orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
-		}
+	} else {
+		orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
 	}
 }

@@ -620,11 +589,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.differential_pressure_timestamp = timestamp;

 		/* publish combined sensor topic */
-		if (_sensors_pub > 0) {
-			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);

 		} else {
-			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
 	}

@@ -639,11 +608,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;

-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);

 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}

@@ -695,11 +664,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.fix_type = gps.fix_type;
 	hil_gps.satellites_visible = gps.satellites_visible;

-	if (_gps_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);

 	} else {
-		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
 	}
 }

@@ -753,11 +722,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.pitchspeed = hil_state.pitchspeed;
 		hil_attitude.yawspeed = hil_state.yawspeed;

-		if (_attitude_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);

 		} else {
-			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
 	}

@@ -777,11 +746,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.eph = 2.0f;
 		hil_global_pos.epv = 4.0f;

-		if (_global_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

 		} else {
-			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
 		}
 	}

@@ -821,11 +790,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
 		hil_local_pos.landed = landed;

-		if (_local_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);

 		} else {
-			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
 		}
 	}

@@ -862,11 +831,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;

-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);

 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
@@ -890,8 +859,6 @@ MavlinkReceiver::receive_thread(void *arg)
 	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
 	prctl(PR_SET_NAME, thread_name, getpid());

-	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-
 	struct pollfd fds[1];
 	fds[0].fd = uart_fd;
 	fds[0].events = POLLIN;
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -120,7 +120,6 @@ private:

 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
-	int _manual_sub;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
 	orb_advert_t _attitude_pub;
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -157,7 +157,9 @@ private:
 		param_t yaw_rate_d;
 		param_t yaw_ff;

-		param_t rc_scale_yaw;
+		param_t man_roll_max;
+		param_t man_pitch_max;
+		param_t man_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */

 	struct {
@@ -167,7 +169,9 @@ private:
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */

-		float rc_scale_yaw;
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
 	}		_params;

 	/**
@@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");

-	_params_handles.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
+	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");

 	/* fetch initial parameter values */
 	parameters_update();
@@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update()
 {
 	float v;

-	/* roll */
+	/* roll gains */
 	param_get(_params_handles.roll_p, &v);
 	_params.att_p(0) = v;
 	param_get(_params_handles.roll_rate_p, &v);
@@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.roll_rate_d, &v);
 	_params.rate_d(0) = v;

-	/* pitch */
+	/* pitch gains */
 	param_get(_params_handles.pitch_p, &v);
 	_params.att_p(1) = v;
 	param_get(_params_handles.pitch_rate_p, &v);
@@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.pitch_rate_d, &v);
 	_params.rate_d(1) = v;

-	/* yaw */
+	/* yaw gains */
 	param_get(_params_handles.yaw_p, &v);
 	_params.att_p(2) = v;
 	param_get(_params_handles.yaw_rate_p, &v);
@@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update()

 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);

-	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+	/* manual control scale */
+	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
+	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+
+	_params.man_roll_max *= M_PI / 180.0;
+	_params.man_pitch_max *= M_PI / 180.0;
+	_params.man_yaw_max *= M_PI / 180.0;

 	return OK;
 }
@@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
 	orb_check(_manual_control_sp_sub, &updated);

 	if (updated) {
-
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
 	}
 }
@@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//	reset_yaw_sp = true;
 			//}
 		} else {
-			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
-
-			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
-				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
-
-				if (_manual_control_sp.yaw > 0.0f) {
-					yaw_sp_move_rate -= YAW_DEADZONE;
-
-				} else {
-					yaw_sp_move_rate += YAW_DEADZONE;
-				}
-
-				yaw_sp_move_rate *= _params.rc_scale_yaw;
-				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
-				_v_att_sp.R_valid = false;
-				publish_att_sp = true;
-			}
+			/* move yaw setpoint */
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt);
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
 		}

 		/* reset yaw setpint to current position if needed */
@@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt)

 		if (!_v_control_mode.flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.roll;
-			_v_att_sp.pitch_body = _manual_control_sp.pitch;
+			_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 * @group Multicopter Attitude Control
 */
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -151,9 +151,6 @@ private:
 		param_t tilt_max;
 		param_t land_speed;
 		param_t land_tilt_max;
-
-		param_t rc_scale_pitch;
-		param_t rc_scale_roll;
 	}		_params_handles;		/**< handles for interesting parameters */

 	struct {
@@ -163,9 +160,6 @@ private:
 		float land_speed;
 		float land_tilt_max;

-		float rc_scale_pitch;
-		float rc_scale_roll;
-
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
 		math::Vector<3> vel_i;
@@ -317,8 +311,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.tilt_max	= param_find("MPC_TILT_MAX");
 	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
 	_params_handles.land_tilt_max	= param_find("MPC_LAND_TILT");
-	_params_handles.rc_scale_pitch	= param_find("RC_SCALE_PITCH");
-	_params_handles.rc_scale_roll	= param_find("RC_SCALE_ROLL");

 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -366,8 +358,6 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.tilt_max, &_params.tilt_max);
 		param_get(_params_handles.land_speed, &_params.land_speed);
 		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
-		param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
-		param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);

 		float v;
 		param_get(_params_handles.xy_p, &v);
@@ -633,8 +623,8 @@ MulticopterPositionControl::task_main()
 					reset_pos_sp();

 					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
-					sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
+					sp_move_rate(0) = _manual.pitch;
+					sp_move_rate(1) = _manual.roll;
 				}

 				/* limit setpoint move rate */
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -690,84 +690,46 @@ Navigator::task_main()
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();

-			/* evaluate state machine from commander and set the navigator mode accordingly */
+			/* evaluate state requested by commander */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				bool stick_mode = false;
+				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+					/* commander requested new navigation mode, try to set it */
+					switch (_vstatus.set_nav_state) {
+					case NAV_STATE_NONE:
+						/* nothing to do */
+						break;

-				if (!_vstatus.rc_signal_lost) {
-					/* RC signal available, use control switches to set mode */
-					/* RETURN switch, overrides MISSION switch */
-					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-						/* switch to RTL if not already landed after RTL and home position set */
+					case NAV_STATE_LOITER:
+						request_loiter_or_ready();
+						break;
+
+					case NAV_STATE_MISSION:
+						request_mission_if_available();
+						break;
+
+					case NAV_STATE_RTL:
 						if (!(_rtl_state == RTL_STATE_DESCEND &&
-						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-						    _vstatus.condition_home_position_valid) {
+							  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							_vstatus.condition_home_position_valid) {
 							dispatch(EVENT_RTL_REQUESTED);
 						}

-						stick_mode = true;
+						break;

-					} else {
-						/* MISSION switch */
-						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							request_loiter_or_ready();
-							stick_mode = true;
+					case NAV_STATE_LAND:
+						dispatch(EVENT_LAND_REQUESTED);

-						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-							request_mission_if_available();
-							stick_mode = true;
-						}
+						break;

-						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
-							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-							request_mission_if_available();
-							stick_mode = true;
-						}
+					default:
+						warnx("ERROR: Requested navigation state not supported");
+						break;
 					}
-				}

-				if (!stick_mode) {
-					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-						/* commander requested new navigation mode, try to set it */
-						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-
-						switch (_vstatus.set_nav_state) {
-						case NAV_STATE_NONE:
-							/* nothing to do */
-							break;
-
-						case NAV_STATE_LOITER:
-							request_loiter_or_ready();
-							break;
-
-						case NAV_STATE_MISSION:
-							request_mission_if_available();
-							break;
-
-						case NAV_STATE_RTL:
-							if (!(_rtl_state == RTL_STATE_DESCEND &&
-							      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-							    _vstatus.condition_home_position_valid) {
-								dispatch(EVENT_RTL_REQUESTED);
-							}
-
-							break;
-
-						case NAV_STATE_LAND:
-							dispatch(EVENT_LAND_REQUESTED);
-
-							break;
-
-						default:
-							warnx("ERROR: Requested navigation state not supported");
-							break;
-						}
-
-					} else {
-						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-						if (myState == NAV_STATE_NONE) {
-							request_mission_if_available();
-						}
+				} else {
+					/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
+					if (myState == NAV_STATE_NONE) {
+						request_mission_if_available();
 					}
 				}

@@ -775,6 +737,8 @@ Navigator::task_main()
 				/* navigator shouldn't act */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
+
+			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 		}

 		/* parameters updated */
@@ -1539,7 +1503,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);

-			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}

--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -134,8 +134,6 @@ controls_tick() {

 	perf_begin(c_gather_sbus);

-	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
-
 	bool sbus_failsafe, sbus_frame_drop;
 	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);

@@ -201,94 +199,104 @@ controls_tick() {
 		/* update RC-received timestamp */
 		system_state.rc_channels_timestamp_received = hrt_absolute_time();

-		/* do not command anything in failsafe, kick in the RC loss counter */
-		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;

-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {

-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];

-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {

-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+				uint16_t raw = r_raw_rc_values[i];

-					uint16_t raw = r_raw_rc_values[i];
+				int16_t scaled;

-					int16_t scaled;
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];

-					/*
-					 * 1) Constrain to min/max values, as later processing depends on bounds.
-					 */
-					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-						raw = conf[PX4IO_P_RC_CONFIG_MIN];
-					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-						raw = conf[PX4IO_P_RC_CONFIG_MAX];
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				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_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));

-					/*
-					 * 2) Scale around the mid point differently for lower and upper range.
-					 *
-					 * This is necessary as they don't share the same endpoints and slope.
-					 *
-					 * First normalize to 0..1 range with correct sign (below or above center),
-					 * then scale to 20000 range (if center is an actual center, -10000..10000,
-					 * if parameters only support half range, scale to 10000 range, e.g. if
-					 * center == min 0..10000, if center == max -10000..0).
-					 *
-					 * As the min and max bounds were enforced in step 1), division by zero
-					 * cannot occur, as for the case of center == min or center == max the if
-					 * statement is mutually exclusive with the arithmetic NaN case.
-					 *
-					 * DO NOT REMOVE OR ALTER STEP 1!
-					 */
-					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_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]));

-					} 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]));
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}

-					} else {
-						/* in the configured dead zone, output zero */
-						scaled = 0;
-					}
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+					scaled = -scaled;
+				}

-					/* invert channel if requested */
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
+
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
 						scaled = -scaled;
-
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					if (mapped < PX4IO_CONTROL_CHANNELS) {
-
-						/* invert channel if pitch - pulling the lever down means pitching up by convention */
-						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-							scaled = -scaled;
-
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
-
 					}
+
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+						} else {
+							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+						}
+					}
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
 				}
 			}
+		}

-			/* set un-assigned controls to zero */
-			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-				if (!(assigned_channels & (1 << i)))
-					r_rc_values[i] = 0;
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i))) {
+				r_rc_values[i] = 0;
 			}
+		}

-			/* set RC OK flag, as we got an update */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		/* set RC OK flag, as we got an update */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;

-			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
-			if (assigned_channels > 4) {
-				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
-			} else {
-				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
-			}
+		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+		if (assigned_channels > 4) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}

 		/*
@@ -316,8 +324,13 @@ controls_tick() {
 	 * Handle losing RC input
 	 */

-	/* this kicks in if the receiver is gone or the system went to failsafe */
-	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	/* if we are in failsafe, clear the override flag */
+	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+	}
+
+	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;

+		/* Set raw channel count to zero */
+		r_raw_rc_count = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}

-	/* this kicks in if the receiver is completely gone */
-	if (rc_input_lost) {
-
-		/* Set channel count to zero */
-		r_raw_rc_count = 0;
-	}
-
 	/*
 	 * Check for manual override.
 	 *
 	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input.
+	 * must have R/C input (NO FAILSAFE!).
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {

 		bool override = false;

@@ -369,10 +379,10 @@ controls_tick() {
 				mixer_tick();

 		} else {
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
 	} else {
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
 }

--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -164,10 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
-#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
-#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */

 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum {							/* DSM bind states */
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
- 					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+						/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */

-#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
-#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */

-#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+						/* 12 occupied by CRC */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */

 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum {							/* DSM bind states */
 #define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */

 #define PX4IO_P_CONTROLS_GROUP_VALID		64
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */

 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD			52
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -108,6 +108,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 #endif
+#define r_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]

 #define r_control_values	(&r_page_controls[0])

--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -169,6 +169,7 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 };

 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -84,6 +84,8 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>

 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
 		struct estimator_status_report estimator_status;
+		struct system_power_s system_power;
+		struct servorail_status_s servorail_status;
 	} buf;

 	memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
+			struct log_PWR_s log_PWR;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int telemetry_sub;
 		int range_finder_sub;
 		int estimator_status_sub;
+		int system_power_sub;
+		int servorail_status_sub;
 	} subs;

 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));

 	thread_running = true;

@@ -1176,6 +1185,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
 			log_msg.body.log_RC.channel_count = buf.rc.chan_count;
+			log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
 			LOGBUFFER_WRITE_AND_COUNT(RC);
 		}

@@ -1184,6 +1194,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
+			log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
 			LOGBUFFER_WRITE_AND_COUNT(AIRS);
 		}

@@ -1226,6 +1237,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(BATT);
 		}

+		/* --- SYSTEM POWER RAILS --- */
+		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+			log_msg.msg_type = LOG_PWR_MSG;
+			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+			log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+			log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+			log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+			log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+			/* copy servo rail status topic here too */
+			orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
+			log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
+			log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
+
+			LOGBUFFER_WRITE_AND_COUNT(PWR);
+		}
+
 		/* --- TELEMETRY --- */
 		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
 			log_msg.msg_type = LOG_TELE_MSG;
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -161,6 +161,7 @@ struct log_STAT_s {
 struct log_RC_s {
 	float channel[8];
 	uint8_t channel_count;
+	uint8_t signal_lost;
 };

 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -174,6 +175,7 @@ struct log_OUT0_s {
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
+	float air_temperature_celsius;
 };

 /* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -277,6 +279,29 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };

+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 23
+struct log_ESTM_s {
+	float s[10];
+	uint8_t n_states;
+	uint8_t states_nan;
+	uint8_t covariance_nan;
+	uint8_t kalman_gain_nan;
+};
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+	float peripherals_5v;
+	float servo_rail_5v;
+	float servo_rssi;
+	uint8_t usb_ok;
+	uint8_t brick_ok;
+	uint8_t servo_ok;
+	uint8_t low_power_rail_overcurrent;
+	uint8_t high_power_rail_overcurrent;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/

 /* --- TIME - TIME STAMP --- */
@@ -299,23 +324,6 @@ struct log_PARM_s {
 	float value;
 };

-/* --- ESTM - ESTIMATOR STATUS --- */
-#define LOG_ESTM_MSG 132
-struct log_ESTM_s {
-	float s[10];
-	uint8_t n_states;
-	uint8_t states_nan;
-	uint8_t covariance_nan;
-	uint8_t kalman_gain_nan;
-};
-// struct log_ESTM_s {
-// 	float s[32];
-// 	uint8_t n_states;
-// 	uint8_t states_nan;
-// 	uint8_t covariance_nan;
-// 	uint8_t kalman_gain_nan;
-// };
-
 #pragma pack(pop)

 /* construct list of all message formats */
@@ -330,9 +338,9 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
+	LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
+	LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
@@ -342,16 +350,16 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),

 	/* system-level messages, ID >= 0x80 */
-	// FMT: don't write format of format message, it's useless
+	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
-	LOG_FORMAT(PARM, "Nf", "Name,Value"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
-	//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value")
 };

-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);

 #endif /* SDLOG2_MESSAGES_H_ */
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -647,54 +647,11 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
 PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);


-/**
- * Roll scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
-
-/**
- * Pitch scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
-
-/**
- * Yaw scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
-
-
-/**
- * Failsafe channel mapping.
- *
- * @min 0
- * @max 18
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_CH, 0);
-
-/**
- * Failsafe channel mode.
- *
- * 0 = too low means signal loss,
- * 1 = too high means signal loss
- *
- * @min 0
- * @max 1
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);
-
 /**
 * Failsafe channel PWM threshold.
 *
- * @min 0
- * @max 1
+ * @min 800
+ * @max 2200
 * @group Radio Calibration
 */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -135,7 +135,7 @@
 */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f

-#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
+#define STICK_ON_OFF_LIMIT 0.75f

 /**
 * Sensor app start / stop handling function
@@ -167,7 +167,15 @@ public:
 private:
 	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */

-	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
+	/**
+	 * Get and limit value for specified RC function. Returns NAN if not mapped.
+	 */
+	float		get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+
+	/**
+	 * Get switch position for specified function.
+	 */
+	switch_pos_t	get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);

 	/**
 	 * Gather and publish RC input data.
@@ -258,14 +266,7 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;

-		float rc_scale_roll;
-		float rc_scale_pitch;
-		float rc_scale_yaw;
-		float rc_scale_flaps;
-
-		int rc_fs_ch;
-		int rc_fs_mode;
-		float rc_fs_thr;
+		int32_t rc_fs_thr;

 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -308,13 +309,6 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;

-		param_t rc_scale_roll;
-		param_t rc_scale_pitch;
-		param_t rc_scale_yaw;
-		param_t rc_scale_flaps;
-
-		param_t rc_fs_ch;
-		param_t rc_fs_mode;
 		param_t rc_fs_thr;

 		param_t battery_voltage_scaling;
@@ -438,8 +432,6 @@ Sensors	*g_sensors = nullptr;
 }

 Sensors::Sensors() :
-	_rc_last_valid(0),
-
 	_fd_adc(-1),
 	_last_adc(0),

@@ -474,6 +466,7 @@ Sensors::Sensors() :
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
 {
+	memset(&_rc, 0, sizeof(_rc));

 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -525,15 +518,8 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");

-	_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
-	_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
-	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
-	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
-
 	/* RC failsafe */
-	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
-	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
-	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");

 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -685,12 +671,6 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
-	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
-	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
-	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
-	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
-	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
 	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));

 	/* update RC function mappings */
@@ -1033,12 +1013,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;

-		float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);

 		_airspeed.timestamp = _diff_pres.timestamp;
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.air_temperature_celsius = air_temperature_celsius;

 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1274,6 +1255,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 	}
 }

+float
+Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+{
+	if (_rc.function[func] >= 0) {
+		float value = _rc.chan[_rc.function[func]].scaled;
+		if (value < min_value) {
+			return min_value;
+
+		} else if (value > max_value) {
+			return max_value;
+
+		} else {
+			return value;
+		}
+	} else {
+		return 0.0f;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
+{
+	if (_rc.function[func] >= 0) {
+		float value = _rc.chan[_rc.function[func]].scaled;
+		if (value > STICK_ON_OFF_LIMIT) {
+			return SWITCH_POS_ON;
+
+		} else if (value < -STICK_ON_OFF_LIMIT) {
+			return SWITCH_POS_OFF;
+
+		} else {
+			return SWITCH_POS_MIDDLE;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
 void
 Sensors::rc_poll()
 {
@@ -1282,48 +1302,31 @@ Sensors::rc_poll()

 	if (rc_updated) {
 		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
-		struct rc_input_values	rc_input;
+		struct rc_input_values rc_input;

 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

-		if (rc_input.rc_lost)
-			return;
+		/* detect RC signal loss */
+		bool signal_lost;

-		struct manual_control_setpoint_s manual_control;
-		struct actuator_controls_s actuator_group_3;
+		/* check flags and require at least four channels to consider the signal valid */
+		if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
+			/* signal is lost or no enough channels */
+			signal_lost = true;

-		/* initialize to default values */
-		manual_control.roll = NAN;
-		manual_control.pitch = NAN;
-		manual_control.yaw = NAN;
-		manual_control.throttle = NAN;
+		} else {
+			/* signal looks good */
+			signal_lost = false;

-		manual_control.mode_switch = NAN;
-		manual_control.return_switch = NAN;
-		manual_control.assisted_switch = NAN;
-		manual_control.mission_switch = NAN;
-//		manual_control.auto_offboard_input_switch = NAN;
-
-		manual_control.flaps = NAN;
-		manual_control.aux1 = NAN;
-		manual_control.aux2 = NAN;
-		manual_control.aux3 = NAN;
-		manual_control.aux4 = NAN;
-		manual_control.aux5 = NAN;
-
-		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
-			return;
-
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
-
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
+			/* check throttle failsafe */
+			int8_t thr_ch = _rc.function[THROTTLE];
+			if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
+				/* throttle failsafe configured */
+				if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
+					(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
+					/* throttle failsafe triggered, signal is lost by receiver */
+					signal_lost = true;
+				}
 			}
 		}

@@ -1332,10 +1335,7 @@ Sensors::rc_poll()
 		if (channel_limit > _rc_max_chan_count)
 			channel_limit = _rc_max_chan_count;

-		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp_last_signal;
-
-		/* Read out values from raw message */
+		/* read out and scale values from raw message even if signal is invalid */
 		for (unsigned int i = 0; i < channel_limit; i++) {

 			/*
@@ -1382,130 +1382,75 @@ Sensors::rc_poll()
 		}

 		_rc.chan_count = rc_input.channel_count;
+		_rc.rssi = rc_input.rssi;
+		_rc.signal_lost = signal_lost;
 		_rc.timestamp = rc_input.timestamp_last_signal;

-		manual_control.timestamp = rc_input.timestamp_last_signal;
-
-		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
-		/*
-		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-		 * so reverse sign.
-		 */
-		manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
-		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
-		/* throttle input */
-		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-
-		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-
-		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
-
-		/* scale output */
-		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
-			manual_control.roll *= _parameters.rc_scale_roll;
-		}
-
-		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
-			manual_control.pitch *= _parameters.rc_scale_pitch;
-		}
-
-		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
-			manual_control.yaw *= _parameters.rc_scale_yaw;
-		}
-
-		/* flaps */
-		if (_rc.function[FLAPS] >= 0) {
-
-			manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
-
-			if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
-				manual_control.flaps *= _parameters.rc_scale_flaps;
-			}
-		}
-
-		/* mode switch input */
-		if (_rc.function[MODE] >= 0) {
-			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-		}
-
-		/* assisted switch input */
-		if (_rc.function[ASSISTED] >= 0) {
-			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-		}
-
-		/* mission switch input */
-		if (_rc.function[MISSION] >= 0) {
-			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-		}
-
-		/* return switch input */
-		if (_rc.function[RETURN] >= 0) {
-			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-		}
-
-//		if (_rc.function[OFFBOARD_MODE] >= 0) {
-//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-//		}
-
-		/* aux functions, only assign if valid mapping is present */
-		if (_rc.function[AUX_1] >= 0) {
-			manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
-		}
-
-		if (_rc.function[AUX_2] >= 0) {
-			manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
-		}
-
-		if (_rc.function[AUX_3] >= 0) {
-			manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
-		}
-
-		if (_rc.function[AUX_4] >= 0) {
-			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
-		}
-
-		if (_rc.function[AUX_5] >= 0) {
-			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
-		}
-
-		/* copy from mapped manual control to control group 3 */
-		actuator_group_3.control[0] = manual_control.roll;
-		actuator_group_3.control[1] = manual_control.pitch;
-		actuator_group_3.control[2] = manual_control.yaw;
-		actuator_group_3.control[3] = manual_control.throttle;
-		actuator_group_3.control[4] = manual_control.flaps;
-		actuator_group_3.control[5] = manual_control.aux1;
-		actuator_group_3.control[6] = manual_control.aux2;
-		actuator_group_3.control[7] = manual_control.aux3;
-
-		/* check if ready for publishing */
+		/* publish rc_channels topic even if signal is invalid, for debug */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);

 		} else {
-			/* advertise the rc topic */
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
 		}

-		/* check if ready for publishing */
-		if (_manual_control_pub > 0) {
-			orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+		if (!signal_lost) {
+			struct manual_control_setpoint_s manual;
+			memset(&manual, 0 , sizeof(manual));

-		} else {
-			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
-		}
+			/* fill values in manual_control_setpoint topic only if signal is valid */
+			manual.timestamp = rc_input.timestamp_last_signal;

-		/* check if ready for publishing */
-		if (_actuator_group_3_pub > 0) {
-			orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+			/* limit controls */
+			manual.roll = get_rc_value(ROLL, -1.0, 1.0);
+			manual.pitch = get_rc_value(PITCH, -1.0, 1.0);
+			manual.yaw = get_rc_value(YAW, -1.0, 1.0);
+			manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);

-		} else {
-			_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+			/* mode switches */
+			manual.mode_switch = get_rc_switch_position(MODE);
+			manual.assisted_switch = get_rc_switch_position(ASSISTED);
+			manual.mission_switch = get_rc_switch_position(MISSION);
+			manual.return_switch = get_rc_switch_position(RETURN);
+
+			/* publish manual_control_setpoint topic */
+			if (_manual_control_pub > 0) {
+				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
+
+			} else {
+				_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+			}
+
+			/* copy from mapped manual control to control group 3 */
+			struct actuator_controls_s actuator_group_3;
+			memset(&actuator_group_3, 0 , sizeof(actuator_group_3));
+
+			actuator_group_3.timestamp = rc_input.timestamp_last_signal;
+
+			actuator_group_3.control[0] = manual.roll;
+			actuator_group_3.control[1] = manual.pitch;
+			actuator_group_3.control[2] = manual.yaw;
+			actuator_group_3.control[3] = manual.throttle;
+			actuator_group_3.control[4] = manual.flaps;
+			actuator_group_3.control[5] = manual.aux1;
+			actuator_group_3.control[6] = manual.aux2;
+			actuator_group_3.control[7] = manual.aux3;
+
+			/* publish actuator_controls_3 topic */
+			if (_actuator_group_3_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+
+			} else {
+				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+			}
 		}
 	}
-
 }

 void
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
 #include "topics/servorail_status.h"
 ORB_DEFINE(servorail_status, struct servorail_status_s);

+#include "topics/system_power.h"
+ORB_DEFINE(system_power, struct system_power_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);

--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -52,9 +52,10 @@
 * Airspeed
 */
 struct airspeed_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		air_temperature_celsius;	/**< air temperature in degrees celsius, -1000 if unknown */
 };

 /**
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,13 +52,14 @@
 * Differential pressure.
 */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
-	uint64_t	error_count;
+	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
+	uint64_t	error_count;			/**< Number of errors detected by driver */
 	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
-	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
-	float		temperature;			/**< Temperature provided by sensor, -1000.0f if unknown */
+	float	voltage;				/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float	temperature;				/**< Temperature provided by sensor, -1000.0f if unknown */

 };

--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -43,6 +43,16 @@
 #include <stdint.h>
 #include "../uORB.h"

+/**
+ * Switch position
+ */
+typedef enum {
+	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_ON,			/**< switch activated (value = 1) */
+	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
+	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
+} switch_pos_t;
+
 /**
 * @addtogroup topics
 * @{
@@ -51,32 +61,25 @@
 struct manual_control_setpoint_s {
 	uint64_t timestamp;

-	float roll;			 	/**< ailerons roll / roll rate input */
-	float pitch;				/**< elevator / pitch / pitch rate */
-	float yaw;				/**< rudder / yaw rate / yaw */
-	float throttle;				/**< throttle / collective thrust / altitude */
-
-	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
-	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
-	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
-
 	/**
-	 * Any of the channels below may not be available and be set to NaN
+	 * Any of the channels may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
 	 */
-
-	// XXX needed or parameter?
-	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
-
-	float flaps;				/**< flap position */
-
+	float roll;			 	/**< ailerons roll / roll rate input, -1..1 */
+	float pitch;			/**< elevator / pitch / pitch rate, -1..1 */
+	float yaw;				/**< rudder / yaw rate / yaw, -1..1 */
+	float throttle;			/**< throttle / collective thrust / altitude, 0..1 */
+	float flaps;			/**< flap position */
 	float aux1;				/**< default function: camera yaw / azimuth */
 	float aux2;				/**< default function: camera pitch / tilt */
 	float aux3;				/**< default function: camera trigger */
 	float aux4;				/**< default function: camera roll */
 	float aux5;				/**< default function: payload drop */

+	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
+	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+	switch_pos_t mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
 }; /**< manual control inputs */

 /**
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -94,6 +94,7 @@ struct rc_channels_s {
 	char function_name[RC_CHANNELS_FUNCTION_MAX][20];
 	int8_t function[RC_CHANNELS_FUNCTION_MAX];
 	uint8_t rssi;                       /**< Overall receive signal strength */
+	bool signal_lost;		/**< control signal lost, should be checked together with topic timeout */
 }; /**< radio control channels. */

 /**
--- a/src/modules/uORB/topics/system_power.h
+++ b/src/modules/uORB/topics/system_power.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_power.h
+ *
+ * Definition of the system_power voltage and power status uORB topic.
+ */
+
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage5V_v;		/**< peripheral 5V rail voltage */
+	uint8_t         usb_connected:1;        /**< USB is connected when 1 */
+	uint8_t         brick_valid:1;          /**< brick power is good when 1 */
+	uint8_t         servo_valid:1;          /**< servo power is good when 1 */
+	uint8_t		periph_5V_OC:1;         /**< peripheral overcurrent when 1 */
+	uint8_t		hipower_5V_OC:1;        /**< hi power peripheral overcurrent when 1 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
+
+#endif
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -93,29 +93,6 @@ typedef enum {
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;

-typedef enum {
-	MODE_SWITCH_MANUAL = 0,
-	MODE_SWITCH_ASSISTED,
-	MODE_SWITCH_AUTO
-} mode_switch_pos_t;
-
-typedef enum {
-	ASSISTED_SWITCH_SEATBELT = 0,
-	ASSISTED_SWITCH_EASY
-} assisted_switch_pos_t;
-
-typedef enum {
-	RETURN_SWITCH_NONE = 0,
-	RETURN_SWITCH_NORMAL,
-	RETURN_SWITCH_RETURN
-} return_switch_pos_t;
-
-typedef enum {
-	MISSION_SWITCH_NONE = 0,
-	MISSION_SWITCH_LOITER,
-	MISSION_SWITCH_MISSION
-} mission_switch_pos_t;
-
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -187,11 +164,6 @@ struct vehicle_status_s {

 	bool is_rotary_wing;

-	mode_switch_pos_t mode_switch;
-	return_switch_pos_t return_switch;
-	assisted_switch_pos_t assisted_switch;
-	mission_switch_pos_t mission_switch;
-
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
 	bool condition_system_sensors_initialized;