From 477aa6e47a5f55e69623f36bd904f1c085efd314 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Thu, 31 May 2012 14:59:03 -0700
Subject: [PATCH] RC_Channel fix for throttle output.

throttle was outputting incorrectly and allowing the user to max out the throttle leaving nothing for attitude control
---
 ArduCopter/radio.pde                |  5 +++++
 libraries/RC_Channel/RC_Channel.cpp | 33 +++++++++++++++++------------
 libraries/RC_Channel/RC_Channel.h   |  3 +++
 3 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
index b1cb39d0d6..a3c0fbbc2a 100644
--- a/ArduCopter/radio.pde
+++ b/ArduCopter/radio.pde
@@ -66,6 +66,11 @@ static void init_rc_out()
 		read_radio();
 	}
 
+	// we want the input to be scaled correctly
+	g.rc_3.set_range_out(0,1000);
+
+
+
     // sanity check - prevent unconfigured radios from outputting
     if(g.rc_3.radio_min >= 1300){
         g.rc_3.radio_min = g.rc_3.radio_in;
diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 04db0415f8..c64c57b0b5 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -37,9 +37,18 @@ const AP_Param::GroupInfo RC_Channel::var_info[] PROGMEM = {
 void
 RC_Channel::set_range(int low, int high)
 {
-	_type 	= RC_CHANNEL_RANGE;
-	_high 	= high;
-	_low 	= low;
+	_type 		= RC_CHANNEL_RANGE;
+	_high 		= high;
+	_low 		= low;
+	_high_out 	= high;
+	_low_out 	= low;
+}
+
+void
+RC_Channel::set_range_out(int low, int high)
+{
+	_high_out	= high;
+	_low_out	= low;
 }
 
 void
@@ -79,8 +88,6 @@ void
 RC_Channel::set_type(uint8_t t)
 {
 	_type = t;
-	//Serial.print("type1: ");
-	//Serial.println(t,DEC);
 }
 
 // call after first read
@@ -92,9 +99,8 @@ RC_Channel::trim()
 
 // read input from APM_RC - create a control_in value
 void
-RC_Channel::set_pwm(int pwm)
+RC_Channel::set_pwm(int16_t pwm)
 {
-	//Serial.print(pwm,DEC);
 
 	/*if(_filter){
 		if(radio_in == 0)
@@ -105,13 +111,12 @@ RC_Channel::set_pwm(int pwm)
 		radio_in = pwm;
 	}*/
 
-	radio_in = constrain(pwm, radio_min.get(), radio_max.get());
+	radio_in = pwm;
 
 	if(_type == RC_CHANNEL_RANGE){
-		//Serial.print("range ");
 		control_in = pwm_to_range();
 		//control_in = constrain(control_in, _low, _high);
-		control_in = min(control_in, _high);
+		//control_in = min(control_in, _high);
 		control_in = (control_in < _dead_zone) ? 0 : control_in;
 
 		if (fabs(scale_output) != 1){
@@ -243,10 +248,12 @@ RC_Channel::angle_to_pwm()
 int16_t
 RC_Channel::pwm_to_range()
 {
+	int16_t r_in = constrain(radio_in, radio_min.get(), radio_max.get());
+
 	int radio_trim_low  = radio_min + _dead_zone;
 
-	if(radio_in > radio_trim_low)
-		return (_low + ((long)(_high - _low) * (long)(radio_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
+	if(r_in > radio_trim_low)
+		return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
 	else if(_dead_zone > 0)
 		return 0;
 	else
@@ -257,7 +264,7 @@ RC_Channel::pwm_to_range()
 int16_t
 RC_Channel::range_to_pwm()
 {
-	return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low);
+	return ((long)(servo_out - _low_out) * (long)(radio_max - radio_min)) / (long)(_high_out - _low_out);
 }
 
 // ------------------------------------------
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index be7c3def60..75aebc6587 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -41,6 +41,7 @@ class RC_Channel{
 
 	// setup the control preferences
 	void 		set_range(int low, int high);
+	void 		set_range_out(int low, int high);
 	void 		set_angle(int angle);
 	void 		set_reverse(bool reverse);
 	bool		get_reverse(void);
@@ -100,6 +101,8 @@ class RC_Channel{
 	uint8_t 	_type;
 	int16_t 	_high;
 	int16_t 	_low;
+	int16_t 	_high_out;
+	int16_t 	_low_out;
 };
 
 // This is ugly, but it fixes compilation on arduino