From 608a089d68443c55fb5f3262bfca37e6abe90cda Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sun, 10 Apr 2011 23:07:24 +0000
Subject: [PATCH] added a limit to throttle_input = 800 for stability insurance
 found an odd bug in SIMPLE mode that broke LOITER mode. Fixed

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1871 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/ArduCopterMega.pde | 59 ++++++++++++++-----------------
 ArduCopterMega/motors.pde         |  2 +-
 ArduCopterMega/radio.pde          |  4 +++
 3 files changed, 32 insertions(+), 33 deletions(-)

diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 9e1a411fe4..b2af906c85 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -42,7 +42,7 @@ version 2.1 of the License, or (at your option) any later version.
 
 #define MAVLINK_COMM_NUM_BUFFERS 2
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
-//#include <GCS_SIMPLE.h>
+#include <GCS_SIMPLE.h>
 
 
 // Configuration
@@ -177,7 +177,7 @@ GPS         *g_gps;
 	GCS_Class           gcs;
 #endif
 
-//GCS_SIMPLE    gcs_simple(&Serial);
+GCS_SIMPLE    gcs_simple(&Serial);
 
 AP_RangeFinder_MaxsonarXL sonar;
 
@@ -933,16 +933,16 @@ void update_current_flight_mode(void)
 				if(flight_timer > 4){
 					flight_timer = 0;
 
-					current_loc.lat = 0;
-					current_loc.lng = 0;
+					tell_command.lat = 0;
+					tell_command.lng = 0;
 
 					next_WP.lng =   (float)g.rc_1.control_in *.4;  // X: 4500 / 2 = 2250 = 25 meteres
 					next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres
 
 					// calc a new bearing
-					nav_bearing 	= get_bearing(&current_loc, &next_WP) + initial_simple_bearing;
+					nav_bearing 	= get_bearing(&tell_command, &next_WP) + initial_simple_bearing;
 					nav_bearing 	= wrap_360(nav_bearing);
-					wp_distance 	= get_distance(&current_loc, &next_WP);
+					wp_distance 	= get_distance(&tell_command, &next_WP);
 					calc_bearing_error();
 					/*
 					Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
@@ -983,18 +983,7 @@ void update_current_flight_mode(void)
 				//next_WP.alt 	= home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
 				//next_WP.alt		= max(next_WP.alt, 30);
 
-				flight_timer++;
-
-				if(flight_timer >= 2){
-					flight_timer = 0;
-
-					if(g.rc_3.control_in <= 200){
-						next_WP.alt -= 1;				// 1 meter per second
-						next_WP.alt = max(next_WP.alt, 100);
-					}else if (g.rc_3.control_in > 700){
-						next_WP.alt += 1;
-					}
-				}
+				adjust_altitude();
 				// !!! testing
 				//next_WP.alt 	-= 500;
 
@@ -1033,26 +1022,15 @@ void update_current_flight_mode(void)
 
 			case LOITER:
 
-				flight_timer++;
+				adjust_altitude();
 
-				if(flight_timer >= 2){
-					flight_timer = 0;
-
-					if(g.rc_3.control_in <= 0){
-						next_WP.alt -= 1;
-					}else if (g.rc_3.control_in > 700){
-						next_WP.alt += 1;
-					}
-				}
+				// Output Pitch, Roll, Yaw and Throttle
+				// ------------------------------------
 
 				// Yaw control
 				// -----------
 				output_manual_yaw();
 
-
-				// Output Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
-
 				// apply throttle control
 				output_auto_throttle();
 
@@ -1171,3 +1149,20 @@ void update_alt()
 	calc_nav_throttle();
 }
 
+void
+adjust_altitude()
+{
+	flight_timer++;
+	if(flight_timer >= 2){
+		flight_timer = 0;
+
+		if(g.rc_3.control_in <= 200){
+			next_WP.alt -= 3;												// 1 meter per second
+			next_WP.alt = max(next_WP.alt, 100); 							// no lower than 1 meter?
+			next_WP.alt = max((current_loc.alt - 400), next_WP.alt);		// don't go more than 4 meters below current location
+		}else if (g.rc_3.control_in > 700){
+			next_WP.alt += 3;												// 1 meter per second
+			next_WP.alt = min((current_loc.alt + 400), next_WP.alt);		// don't go more than 4 meters below current location
+		}
+	}
+}
\ No newline at end of file
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 3e235eb32a..07b9a66899 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -228,8 +228,8 @@ set_servos_4()
 			gcs_simple.write_int(motor_out[CH_2]);
 			gcs_simple.write_int(motor_out[CH_3]);
 			gcs_simple.write_int(motor_out[CH_4]);
-
 			gcs_simple.write_int(g.rc_3.servo_out);
+
 			gcs_simple.write_int((int)(cos_yaw_x * 100));
 			gcs_simple.write_int((int)(sin_yaw_y * 100));
 			gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index 4b837f86d1..0aab28b225 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -86,6 +86,10 @@ void read_radio()
 	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
 	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
 
+
+	// limit our input to 800 so we can still pitch and roll
+	g.rc_3.control_in = min(g.rc_3.control_in, 800);
+
 	//throttle_failsafe(g.rc_3.radio_in);
 
 	/*