Merged

6 files changed, 119 insertions, 14 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index c2396b002..332d4f882 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
+#include <systemlib/param/param.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
@@ -225,7 +226,7 @@ PX4IO::PX4IO() :
 	_relays(0),
 	_switch_armed(false),
 	_send_needed(false),
-	_config_needed(false)
+	_config_needed(true)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -454,12 +455,6 @@ PX4IO::task_main()
 			}
 		}
 
-		/* send an update to IO if required */
-		if (_send_needed) {
-			_send_needed = false;
-			io_send();
-		}
-
 		/* send a config packet to IO if required */
 		if (_config_needed) {
 			_config_needed = false;
@@ -474,6 +469,12 @@ PX4IO::task_main()
 			_mix_buf = nullptr;
 			_mix_buf_len = 0;
 		}
+
+		/* send an update to IO if required */
+		if (_send_needed) {
+			_send_needed = false;
+			io_send();
+		}
 	}
 
 	unlock();
@@ -632,7 +633,7 @@ PX4IO::io_send()
 	cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
 	/* set desired PWM output rate */
 	cmd.servo_rate = _update_rate;
-
+	
 	ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
 
 	if (ret)
@@ -647,6 +648,46 @@ PX4IO::config_send()
 
 	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
 
+	int val;
+
+	/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */		
+	param_get(param_find("RC_MAP_ROLL"), &val);
+	cfg.rc_map[0] = (uint8_t)val;
+	param_get(param_find("RC_MAP_PITCH"), &val);
+	cfg.rc_map[1] = (uint8_t)val;
+	param_get(param_find("RC_MAP_YAW"), &val);
+	cfg.rc_map[2] = (uint8_t)val;
+	param_get(param_find("RC_MAP_THROTTLE"), &val);
+	cfg.rc_map[3] = (uint8_t)val;
+
+	/* set the individual channel properties */
+	char nbuf[16];
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_MIN", i);
+		param_get(param_find(nbuf), &val);	
+		cfg.rc_min[i] = (uint16_t)val;
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_TRIM", i);	
+		param_get(param_find(nbuf), &val);	
+		cfg.rc_trim[i] = (uint16_t)val;
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_MAX", i);	
+		param_get(param_find(nbuf), &val);	
+		cfg.rc_max[i] = (uint16_t)val;
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_REV", i);	
+		param_get(param_find(nbuf), &val);	
+		cfg.rc_rev[i] = (uint16_t)val;
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_DZ", i);	
+		param_get(param_find(nbuf), &val);	
+		cfg.rc_dz[i] = (uint16_t)val;
+	}
+
 	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
 
 	if (ret)
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 8d7e90093..adedea74c 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -207,7 +207,19 @@ comms_handle_config(const void *buffer, size_t length)
 	if (length != sizeof(*cfg))
 		return;
 
-	/* XXX */
+	/* fetch the rc mappings */
+	for (unsigned i = 0; i < 4; i++) {
+		system_state.rc_map[i] = cfg->rc_map[i];
+	}
+
+	/* fetch the rc channel attributes */ 
+	for (unsigned i = 0; i < 4; i++) {
+		system_state.rc_min[i]  = cfg->rc_min[i];
+		system_state.rc_trim[i] = cfg->rc_trim[i];
+		system_state.rc_max[i]  = cfg->rc_max[i];
+		system_state.rc_rev[i]  = cfg->rc_rev[i];
+		system_state.rc_dz[i]   = cfg->rc_dz[i];
+	}
 }
 
 static void
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 297fea017..da445e3b4 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -167,8 +167,9 @@ ppm_input(void)
 		/* PPM data exists, copy it */
 		system_state.rc_channels = ppm_decoded_channels;
 
-		for (unsigned i = 0; i < ppm_decoded_channels; i++)
+		for (unsigned i = 0; i < ppm_decoded_channels; i++) {
 			system_state.rc_channel_data[i] = ppm_buffer[i];
+		}
 
 		/* copy the timestamp and clear it */
 		system_state.rc_channels_timestamp = ppm_last_valid_decode;
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 052876954..95484bc9d 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -47,11 +47,13 @@
 #include <errno.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <sched.h>
 
 #include <debug.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/device.h>
 
 #include <systemlib/mixer/mixer.h>
 
@@ -65,6 +67,12 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/* 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
+
 /* current servo arm/disarm state */
 bool mixer_servos_armed = false;
 
@@ -72,6 +80,8 @@ bool mixer_servos_armed = false;
 static uint16_t *control_values;
 static int control_count;
 
+static uint16_t rc_channel_data[PX4IO_CONTROL_CHANNELS];
+
 static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_group,
 			       uint8_t control_index,
@@ -107,13 +117,34 @@ mixer_tick(void)
 		} else if (system_state.rc_channels > 0) {
 			/* when override is on or the fmu is not available, but RC is present */
 			control_count = system_state.rc_channels;
-			control_values = &system_state.rc_channel_data[0];
+
+			sched_lock();
+
+			/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
+			rc_channel_data[ROLL]     = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
+			rc_channel_data[PITCH]    = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
+			rc_channel_data[YAW]      = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
+			rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
+
+			/* get the remaining channels, no remapping needed */
+			for (unsigned i = 4; i < system_state.rc_channels; i++)
+				rc_channel_data[i] = system_state.rc_channel_data[i];
+
+			/* scale the control inputs */
+			rc_channel_data[THROTTLE] = ((rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / 
+				(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000 + 1000;
+			//lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
+			//	system_state.rc_min[THROTTLE], system_state.rc_trim[THROTTLE], system_state.rc_max[THROTTLE], rc_channel_data[THROTTLE]);
+
+			control_values = &rc_channel_data[0];
+			sched_unlock();
 		} else {
 			/* we have no control input (no FMU, no RC) */
 
 			// XXX builtin failsafe would activate here
 			control_count = 0;
 		}
+		//lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
 
 	/* this is for multicopters, etc. where manual override does not make sense */
 	} else {
@@ -151,8 +182,9 @@ mixer_tick(void)
 			/*
 			 * If we are armed, update the servo output.
 			 */
-			if (system_state.armed && system_state.arm_ok)
+			if (system_state.armed && system_state.arm_ok) {
 				up_pwm_servo_set(i, system_state.servos[i]);
+			}
 		}
 	}
 
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 163c52c02..7aa3be02a 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -59,7 +59,7 @@ struct px4io_command {
 	bool		relay_state[PX4IO_RELAY_CHANNELS];	/**< relay states as requested by FMU */
 	bool		arm_ok;					/**< FMU allows full arming */
 	bool		vector_flight_mode_ok;			/**< FMU aquired a valid position lock, ready for pos control */
-	bool		manual_override_ok;			/**< if true, IO performs a direct manual override */ 
+	bool		manual_override_ok;			/**< if true, IO performs a direct manual override */
 };
 
 /**
@@ -85,7 +85,12 @@ struct px4io_config {
 	uint16_t	f2i_config_magic;
 #define F2I_CONFIG_MAGIC	 0x6366
 
-	/* XXX currently nothing here */
+	uint8_t		rc_map[4];	/**< channel ordering of roll, pitch, yaw, throttle */
+	uint16_t	rc_min[4];	/**< min value for each channel */
+	uint16_t	rc_trim[4];	/**< trim value for each channel */
+	uint16_t	rc_max[4];	/**< max value for each channel */
+	uint16_t	rc_rev[4];	/**< rev value for each channel */
+	uint16_t	rc_dz[4];	/**< dz value for each channel */
 };
 
 /**
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 157135854..65d33f34b 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -73,6 +73,20 @@ struct sys_state_s {
 	uint16_t	servo_rate;		/* output rate of servos in Hz */
 
 	/**
+	 * Remote control input(s) channel mappings
+	 */
+	uint8_t	rc_map[4];
+
+	/**
+	 * Remote control channel attributes 
+	 */
+	uint16_t	rc_min[4];
+	uint16_t	rc_trim[4];
+	uint16_t	rc_max[4];
+	uint16_t	rc_rev[4];
+	uint16_t	rc_dz[4];
+
+	/**
 	 * Data from the remote control input(s)
 	 */
 	unsigned	rc_channels;