Send rc channel ordering and channel attributes from FMU to IO

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>
@@ -608,6 +609,35 @@ PX4IO::io_send()
 	/* set desired PWM output rate */
 	cmd.servo_rate = _update_rate;
 
+	/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
+	cmd.rc_map[0] = param_find("RC_MAP_ROLL");
+	cmd.rc_map[1] = param_find("RC_MAP_PITCH");
+	cmd.rc_map[2] = param_find("RC_MAP_YAW");
+	cmd.rc_map[3] = param_find("RC_MAP_THROTTLE");
+
+	/* set the individual channel properties */
+	char nbuf[16];
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_MIN", i);	
+		cmd.rc_min[i] = (uint16_t)param_find(nbuf);
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_TRIM", i);	
+		cmd.rc_trim[i] = (uint16_t)param_find(nbuf);
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_MAX", i);	
+		cmd.rc_max[i] = (uint16_t)param_find(nbuf);
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_REV", i);	
+		cmd.rc_rev[i] = (uint16_t)param_find(nbuf);
+	}
+	for (unsigned i = 0; i < 4; i++) {
+		sprintf(nbuf, "RC%d_DZ", i);	
+		cmd.rc_dz[i] = (uint16_t)param_find(nbuf);
+	}
+	
 	ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
 
 	if (ret)

apps/px4io/comms.c

@@ -207,6 +207,19 @@ comms_handle_command(const void *buffer, size_t length)
 		system_state.servo_rate = new_servo_rate;
 	}
 
+	/* fetch the rc mappings */
+	for (unsigned i = 0; i < 4; i++)
+		system_state.rc_map[i] = cmd->rc_map[i];
+
+	/* fetch the rc channel attributes */
+	for (unsigned i = 0; i < 4; i++) {
+		system_state.rc_min[i] = cmd->rc_min[i];
+		system_state.rc_trim[i] = cmd->rc_trim[i];
+		system_state.rc_max[i] = cmd->rc_max[i];
+		system_state.rc_rev[i] = cmd->rc_rev[i];
+		system_state.rc_dz[i] = cmd->rc_dz[i];
+	}
+
 	/*
 	 * update servo values immediately.
 	 * the updates are done in addition also

apps/px4io/mixer.cpp

@@ -67,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;
 
@@ -112,22 +118,22 @@ mixer_tick(void)
 			/* when override is on or the fmu is not available, but RC is present */
 			control_count = system_state.rc_channels;
 
-			// Remap the channels based on the radio type.
-			// FMU: Roll, Pitch, Yaw, Throttle
-			// Graupner: Throttle, Roll, Pitch, Yaw
 			sched_lock();
-			rc_channel_data[0] = system_state.rc_channel_data[1];
+
-			rc_channel_data[1] = system_state.rc_channel_data[2];
+			/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
-			rc_channel_data[2] = system_state.rc_channel_data[3];
+			rc_channel_data[ROLL]     = system_state.rc_channel_data[system_state.rc_map[ROLL]];
-			rc_channel_data[3] = system_state.rc_channel_data[0];
+			rc_channel_data[PITCH]    = system_state.rc_channel_data[system_state.rc_map[PITCH]];
-			// AETR
+			rc_channel_data[YAW]      = system_state.rc_channel_data[system_state.rc_map[YAW]];
-			//rc_channel_data[0] = system_state.rc_channel_data[0];
+			rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE]];
-			//rc_channel_data[1] = system_state.rc_channel_data[1];
+
-			//rc_channel_data[2] = system_state.rc_channel_data[3];
+			/* get the remaining channels, no remapping needed */
-			//rc_channel_data[3] = system_state.rc_channel_data[2];
+			for (unsigned i = 4; i < system_state.rc_channels; i++)
-			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;
+
 			control_values = &rc_channel_data[0];
 			sched_unlock();
 		} else {

apps/px4io/protocol.h

@@ -60,6 +60,12 @@ struct px4io_command {
 	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 */
+	uint16_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 */
 };
 
 /**

apps/px4io/px4io.h

@@ -72,6 +72,20 @@ struct sys_state_s {
 	bool		arm_ok;			/* FMU says OK to arm */
 	uint16_t	servo_rate;		/* output rate of servos in Hz */
 
+	/**
+	 * Remote control input(s) channel mappings
+	 */
+	uint16_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)
 	 */