Enabled manual override switch, work in progress. Added initial demix testing code to support delta mixing on the remote for convenient manual override

8 files changed, 113 insertions, 40 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..f4eddbdd3 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -177,13 +177,13 @@ comms_handle_command(const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
 
-	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	/* if the IO is armed and FMU gets disarmed, IO must also disarm */
 	if(system_state.arm_ok && !cmd->arm_ok) {
 		system_state.armed = false;
 	}
 
 	system_state.arm_ok = cmd->arm_ok;
-	system_state.mixer_use_fmu = true;
+	system_state.mixer_fmu_available = true;
 	system_state.fmu_data_received = true;
 
 
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index d4eace3df..0e0210373 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -59,6 +59,10 @@
 #define DEBUG
 #include "px4io.h"
 
+#define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
+#define RC_CHANNEL_HIGH_THRESH		1600
+#define RC_CHANNEL_LOW_THRESH		1400
+
 void
 controls_main(void)
 {
@@ -80,7 +84,22 @@ controls_main(void)
 		if (fds[1].revents & POLLIN)
 			sbus_input();
 
-		/* XXX do ppm processing, bypass mode, etc. here */
+		/* force manual input override */
+		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
+			system_state.mixer_use_fmu = false;
+		} else {
+			/* override not engaged, use FMU */
+			system_state.mixer_use_fmu = true;
+		}
+
+		/* detect rc loss event */
+		if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) {
+			system_state.rc_lost = true;
+		}
+
+		/* XXX detect fmu loss event */
+
+		/* XXX handle failsave events - RC loss and FMU loss - here */
 
 		/* do PWM output updates */
 		mixer_tick();
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 483e9fe4d..d9626c1e3 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -97,7 +97,7 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
-	if (system_state.mixer_use_fmu) {
+	if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
 		/* we have recent control data from the FMU */
 		control_count = PX4IO_OUTPUT_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
@@ -141,9 +141,10 @@ mixer_tick(void)
 
 	/*
 	 * Decide whether the servos should be armed right now.
+	 * A sufficient reason is armed state and either FMU or RC control inputs
 	 */
 
-	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 483b9bcc8..9bfe3b1e4 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -76,43 +76,58 @@ struct sys_state_s
 	bool		dsm_input_ok;	/* valid Spektrum DSM data */
 	bool		sbus_input_ok;	/* valid Futaba S.Bus data */
 
-	/*
+	/**
 	 * Data from the remote control input(s)
 	 */
 	int		rc_channels;
 	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
 	uint64_t	rc_channels_timestamp;
 
-	/*
+	/**
 	 * Control signals from FMU.
 	 */
 	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
 
-	/*
+	/**
 	 * Relay controls
 	 */
 	bool		relays[PX4IO_RELAY_CHANNELS];
 
-	/*
-	 * If true, we are using the FMU controls.
+	/**
+	 * If true, we are using the FMU controls, else RC input if available.
 	 */
 	bool		mixer_use_fmu;
 
-	/*
+	/**
+	 * If true, FMU input is available.
+	 */
+	bool		mixer_fmu_available;
+
+	/**
 	 * If true, state that should be reported to FMU has been updated.
 	 */
 	bool		fmu_report_due;
 
-	/*
+	/**
 	 * If true, new control data from the FMU has been received.
 	 */
 	bool		fmu_data_received;
 
-	/*
+	/**
 	 * Current serial interface mode, per the serial_rx_mode parameter
 	 * in the config packet.
 	 */
 	uint8_t		serial_rx_mode;
+
+	/**
+	 * If true, the RC signal has been lost for more than a timeout interval
+	 */
+	bool		rc_lost;
+
+	/**
+	 * If true, the connection to FMU has been lost for more than a timeout interval
+	 */
+	bool		fmu_lost;
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index d5bd103c1..916988af1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -58,7 +58,7 @@ static struct hrt_call failsafe_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned counter;
+static unsigned counter = 0;
 
 #define ARM_COUNTER_THRESHOLD	10
 #define DISARM_COUNTER_THRESHOLD	2
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index c83d6358e..37ba0ec3e 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -122,6 +122,8 @@ PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
 
+PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */
+
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
 
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 71feb377b..b63bfb195 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -95,6 +95,12 @@
 
 #define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
 
+enum RC_DEMIX {
+	RC_DEMIX_NONE  = 0,
+	RC_DEMIX_AUTO  = 1,
+	RC_DEMIX_DELTA = 2
+};
+
 /**
  * Sensor app start / stop handling function
  *
@@ -178,6 +184,8 @@ private:
 
 		int rc_type;
 
+		int rc_demix;					/**< enabled de-mixing of RC mixers */
+
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
@@ -204,6 +212,8 @@ private:
 		param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
+		param_t rc_demix;
+
 		param_t gyro_offset[3];
 		param_t accel_offset[3];
 		param_t accel_scale[3];
@@ -392,6 +402,8 @@ Sensors::Sensors() :
 
 	_parameter_handles.rc_type = param_find("RC_TYPE");
 
+	_parameter_handles.rc_demix = param_find("RC_DEMIX");
+
 	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
 	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
@@ -496,21 +508,16 @@ Sensors::parameters_update()
 
 	}
 
-	/* update RC function mappings */
-	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
-	_rc.function[ROLL] = _parameters.rc_map_roll - 1;
-	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
-	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
-	_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
-	_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
-	_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
-	_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
-
 	/* remote control type */
 	if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
 		warnx("Failed getting remote control type");
 	}
 
+	/* de-mixing */
+	if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) {
+		warnx("Failed getting demix setting");
+	}
+
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
 		warnx("Failed getting roll chan index");
@@ -547,6 +554,16 @@ Sensors::parameters_update()
 		warnx("Failed getting rc scaling for yaw");
 	}
 
+	/* update RC function mappings */
+	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
+	_rc.function[ROLL] = _parameters.rc_map_roll - 1;
+	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
+	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
+	_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
+	_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
+	_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
+
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
@@ -905,7 +922,7 @@ Sensors::ppm_poll()
 	 */
 	if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
 
-		for (int i = 0; i < ppm_decoded_channels; i++) {
+		for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
 			raw.values[i] = ppm_buffer[i];
 		}
 
@@ -979,35 +996,54 @@ Sensors::ppm_poll()
 
 		manual_control.timestamp = rc_input.timestamp;
 
-		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+		/* check if input needs to be de-mixed */
+		if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
+			// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
+
+			/* roll input - mixed roll and pitch channels */
+			manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled;
+			/* pitch input - mixed roll and pitch channels */
+			manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
+			/* yaw input - stick right is positive and positive rotation */
+			manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
+			/* throttle input */
+			manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+
+		/* direct pass-through of manual input */
+		} else {
+			/* roll input - rolling right is stick-wise and rotation-wise positive */
+			manual_control.roll = _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 = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
+			/* yaw input - stick right is positive and positive rotation */
+			manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
+			/* throttle input */
+			manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+		}
+
+		/* limit outputs */
 		if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
 		if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
-		if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
+		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
 			manual_control.roll *= _parameters.rc_scale_roll;
 		}
 
-		/*
-		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-		 * so reverse sign.
-		 */
-		manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
+		
 		if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
 		if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
-		if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
+		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
 			manual_control.pitch *= _parameters.rc_scale_pitch;
 		}
 
-		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
 		if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
 		if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
-		if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
+		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 		
-		/* 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;
 
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 23172d7cf..b70b322d8 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
 	VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
-	VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16,
+	VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
 	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
 	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
 	VEHICLE_MODE_FLAG_TEST_ENABLED = 2,