From 20987c1b89ae25f79042185393bc93e8e8a67592 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Mon, 6 Sep 2010 06:14:24 +0000
Subject: [PATCH] added servo_in

git-svn-id: https://arducopter.googlecode.com/svn/trunk@401 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/RC/APM2_RC.cpp | 39 ++++++++++++++++++++++++++++-----------
 libraries/RC/APM2_RC.h   |  7 ++++---
 libraries/RC/AP_RC.cpp   | 38 ++++++++++++++++++++++++++++++--------
 libraries/RC/AP_RC.h     |  7 ++++---
 4 files changed, 66 insertions(+), 25 deletions(-)

diff --git a/libraries/RC/APM2_RC.cpp b/libraries/RC/APM2_RC.cpp
index 957b5e1086..a3bb1ee88c 100644
--- a/libraries/RC/APM2_RC.cpp
+++ b/libraries/RC/APM2_RC.cpp
@@ -154,9 +154,34 @@ void APM2_RC::read_pwm()
 		//Serial.println(radio_in[CH2],DEC);
 	}
 
-	// output servo
-	servo_out[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
-	servo_out[CH3] = constrain(servo_out[CH3], 0, 100);
+	// output servos
+	for (uint8_t i = 0; i < 8; i++){
+		if (i == 3) continue;
+		if(radio_in[i] >= radio_trim[i])
+			servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0;
+		else
+			servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0;
+	}
+	servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
+	servo_in[CH3] = constrain(servo_out[CH3], 0, 100);	
+}
+
+void
+APM2_RC::output()
+{
+	uint16_t out;
+	for (uint8_t i = 0; i < 8; i++){
+		if (i == 3) continue;
+		if(radio_in[i] >= radio_trim[i])
+			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
+		else
+			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
+		set_ch_pwm(i, out);
+	}
+
+	out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
+	out += radio_min[CH3];
+	set_ch_pwm(CH3, out);
 }
 
 void 
@@ -173,14 +198,6 @@ APM2_RC::trim()
 		radio_trim[y] = radio_in[y];
 }
 
-void
-APM2_RC::set_throttle(float percent)
-{
-	uint16_t out = (percent * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
-	out += radio_min[CH3];
-	set_ch_pwm(CH3, out);
-}
-
 void
 APM2_RC::set_ch_pwm(uint8_t ch, uint16_t pwm)
 {	
diff --git a/libraries/RC/APM2_RC.h b/libraries/RC/APM2_RC.h
index e7541b474f..e31c6be90b 100644
--- a/libraries/RC/APM2_RC.h
+++ b/libraries/RC/APM2_RC.h
@@ -9,10 +9,10 @@ class APM2_RC : public RC
 {
   public:
 	APM2_RC();
-	void read_pwm();
-	void set_ch_pwm(uint8_t ch, uint16_t pwm);
 	void init();
-	void set_throttle(float percent);
+	void read_pwm();
+	void output();
+	void set_ch_pwm(uint8_t ch, uint16_t pwm);
 	void trim();
 
 	void force_out_0_1(void);
@@ -24,6 +24,7 @@ class APM2_RC : public RC
 	int16_t radio_trim[8];
 	int16_t radio_max[8];
 	
+	int16_t servo_in[8];
 	float servo_out[8];
 
   private:
diff --git a/libraries/RC/AP_RC.cpp b/libraries/RC/AP_RC.cpp
index 2737c5ee4f..68bb23e634 100644
--- a/libraries/RC/AP_RC.cpp
+++ b/libraries/RC/AP_RC.cpp
@@ -128,9 +128,34 @@ AP_RC::read_pwm()
 	
 	check_throttle_failsafe(radio_in[CH3]);
 
-	// output servo
-	servo_out[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
-	servo_out[CH3] = constrain(servo_out[CH3], 0, 100);	
+	// output servos
+	for (uint8_t i = 0; i < 4; i++){
+		if (i == 3) continue;
+		if(radio_in[i] >= radio_trim[i])
+			servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0;
+		else
+			servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0;
+	}
+	servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
+	servo_in[CH3] = constrain(servo_out[CH3], 0, 100);	
+}
+
+void
+AP_RC::output()
+{
+	uint16_t out;
+	for (uint8_t i = 0; i < 4; i++){
+		if (i == 3) continue;
+		if(radio_in[i] >= radio_trim[i])
+			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
+		else
+			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
+		set_ch_pwm(i, out);
+	}
+
+	out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
+	out += radio_min[CH3];
+	set_ch_pwm(CH3, out);
 }
 
 void
@@ -150,14 +175,11 @@ AP_RC::trim()
 	//Serial.println(radio_trim[CH1], DEC);
 }
 
-void
+/*void
 AP_RC::set_throttle(float percent)
 {
-	uint16_t out = (percent * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
-	out += radio_min[CH3];
-	set_ch_pwm(CH3, out);
 }
-
+*/
 void
 AP_RC::set_ch_pwm(uint8_t ch, uint16_t pwm)
 {
diff --git a/libraries/RC/AP_RC.h b/libraries/RC/AP_RC.h
index 05f49b6464..184ef7e071 100644
--- a/libraries/RC/AP_RC.h
+++ b/libraries/RC/AP_RC.h
@@ -9,10 +9,10 @@ class AP_RC : public RC
 {
   public:
 	AP_RC();
-	void read_pwm();
-	void set_ch_pwm(uint8_t ch, uint16_t pwm);
 	void init();
-	void set_throttle(float percent);
+	void read_pwm();
+	void output();
+	void set_ch_pwm(uint8_t ch, uint16_t pwm);
 	void trim();
 	
 	int16_t radio_in[4];
@@ -20,6 +20,7 @@ class AP_RC : public RC
 	int16_t radio_trim[4];
 	int16_t radio_max[4];
 	
+	int16_t servo_in[4];
 	float servo_out[4];
 
   private: