Fixed RC mapping transmission, tested. Fixed RC scaling in manual mode

4 files changed, 46 insertions, 30 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 332d4f882..70b289fd6 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -652,40 +652,41 @@ PX4IO::config_send()
 
 	/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */		
 	param_get(param_find("RC_MAP_ROLL"), &val);
-	cfg.rc_map[0] = (uint8_t)val;
+	cfg.rc_map[0] = val;
 	param_get(param_find("RC_MAP_PITCH"), &val);
-	cfg.rc_map[1] = (uint8_t)val;
+	cfg.rc_map[1] = val;
 	param_get(param_find("RC_MAP_YAW"), &val);
-	cfg.rc_map[2] = (uint8_t)val;
+	cfg.rc_map[2] = val;
 	param_get(param_find("RC_MAP_THROTTLE"), &val);
-	cfg.rc_map[3] = (uint8_t)val;
+	cfg.rc_map[3] = val;
 
 	/* set the individual channel properties */
 	char nbuf[16];
+	float float_val;
 	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;
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		param_get(param_find(nbuf), &float_val);	
+		cfg.rc_min[i] = float_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;
+		sprintf(nbuf, "RC%d_TRIM", i + 1);	
+		param_get(param_find(nbuf), &float_val);	
+		cfg.rc_trim[i] = float_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;
+		sprintf(nbuf, "RC%d_MAX", i + 1);	
+		param_get(param_find(nbuf), &float_val);	
+		cfg.rc_max[i] = float_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;
+		sprintf(nbuf, "RC%d_REV", i + 1);	
+		param_get(param_find(nbuf), &float_val);	
+		cfg.rc_rev[i] = float_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;
+		sprintf(nbuf, "RC%d_DZ", i + 1);	
+		param_get(param_find(nbuf), &float_val);	
+		cfg.rc_dz[i] = float_val;
 	}
 
 	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 95484bc9d..93fe93b39 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -80,7 +80,7 @@ bool mixer_servos_armed = false;
 static uint16_t *control_values;
 static int control_count;
 
-static uint16_t rc_channel_data[PX4IO_CONTROL_CHANNELS];
+static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
 
 static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_group,
@@ -127,14 +127,25 @@ mixer_tick(void)
 			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++)
+			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] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / 
+				(float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000;
 
-			/* 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]);
+			if (rc_channel_data[THROTTLE] > 2000) {
+				rc_channel_data[THROTTLE] = 2000;
+			}
+
+			if (rc_channel_data[THROTTLE] < 1000) {
+				rc_channel_data[THROTTLE] = 1000;
+			}
+			
+			// lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
+			// 	(int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]),
+			// 	(int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE]));
 
 			control_values = &rc_channel_data[0];
 			sched_unlock();
@@ -218,7 +229,11 @@ mixer_callback(uintptr_t handle,
 		return -1;
 
 	/* scale from current PWM units (1000-2000) to mixer input values */
-	control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+	if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) {
+		control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
+	} else {
+		control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+	}
 
 	return 0;
 }
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 7aa3be02a..90236b40c 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -89,7 +89,7 @@ struct px4io_config {
 	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 */
+	int8_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 65d33f34b..139bbb321 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -75,7 +75,7 @@ struct sys_state_s {
 	/**
 	 * Remote control input(s) channel mappings
 	 */
-	uint8_t	rc_map[4];
+	uint8_t		rc_map[4];
 
 	/**
 	 * Remote control channel attributes 
@@ -83,7 +83,7 @@ struct sys_state_s {
 	uint16_t	rc_min[4];
 	uint16_t	rc_trim[4];
 	uint16_t	rc_max[4];
-	uint16_t	rc_rev[4];
+	int16_t		rc_rev[4];
 	uint16_t	rc_dz[4];
 
 	/**