Merge pull request #1390 from PX4/flaps

Enable flaps in manual override

5 files changed, 17 insertions, 10 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d212be766..3871b4a2c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1265,11 +1265,18 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+	param_get(param_find("RC_MAP_FLAPS"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 4;
 
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+		/* use out of normal bounds index to indicate special channel */
+		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+	}
+
 	/*
 	 * Iterate all possible RC inputs.
 	 */
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 7b127759a..ad60ee03e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
+static uint16_t rc_value_override = 0;
+
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 {
 	perf_begin(c_gather_dsm);
@@ -316,6 +318,9 @@ controls_tick() {
 					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
 					assigned_channels |= (1 << mapped);
 
+				} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
+					/* pick out override channel, indicated by special mapping */
+					rc_value_override = SIGNED_TO_REG(scaled);
 				}
 			}
 		}
@@ -409,7 +414,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 3e19333d8..c0b8ac358 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -60,13 +60,6 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		500000
 
-/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
-#define ROLL     0
-#define PITCH    1
-#define YAW      2
-#define THROTTLE 3
-#define OVERRIDE 4
-
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 89a470b44..a3e8a58d3 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -246,6 +246,7 @@ enum {							/* DSM bind states */
 #define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
 #define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
 #define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH	100		/**< magic value for mode switch */
 #define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 7a5a5e484..7f19e983f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -686,7 +686,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
 					disabled = true;
-				} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+				} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
+					   (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
 					count++;
 				}