From 312c0e6b3b26f0c01cb7c65718d5e9764dabc17a Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sun, 14 Aug 2011 02:30:37 +0000
Subject: [PATCH] removed throttle_slew - Just wasn't working well enough and
 added too much complexity moved reading sonar to the 50hz loop so the Mode
 filter could do it's thing without causing so much latency

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3094 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/ArduCopterMega.pde | 36 ++++++++++++++++++++-----------
 ArduCopterMega/Attitude.pde       |  2 +-
 ArduCopterMega/motors.pde         |  4 ++--
 3 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index f717086450..7cd32724c5 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -398,7 +398,7 @@ static int		landing_distance;					// meters;
 static long 	old_alt;							// used for managing altitude rates
 static int		velocity_land;
 static byte 	yaw_tracking = MAV_ROI_WPNEXT;		// no tracking, point at next wp, or at a target
-static int		throttle_slew;						// used to smooth throttle tranistions
+//static int		throttle_slew;						// used to smooth throttle tranistions
 
 // Loiter management
 // -----------------
@@ -782,6 +782,12 @@ static void fifty_hz_loop()
 	// ------------------------------
 	throttle_integrator += g.rc_3.servo_out;
 
+	// Read Sonar
+	// ----------
+	if(g.sonar_enabled){
+		sonar_alt 			= sonar.read();
+	}
+
 	#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
 		// HIL for a copter needs very fast update of the servo values
 		hil.send_message(MSG_RADIO_OUT);
@@ -790,12 +796,12 @@ static void fifty_hz_loop()
 	// use Yaw to find our bearing error
 	calc_bearing_error();
 
-	if (throttle_slew < 0)
-		throttle_slew++;
-	else if (throttle_slew > 0)
-		throttle_slew--;
+	//if (throttle_slew < 0)
+	//	throttle_slew++;
+	//else if (throttle_slew > 0)
+	//	throttle_slew--;
 
-	throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
+	//throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
 
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
@@ -1360,6 +1366,10 @@ static void update_navigation()
 
 			// rotate that pitch to the copter frame of reference
 			calc_nav_output();
+
+			//if(wp_distance < 4 )
+			//	set_mode(LOITER);
+
 			break;
 
 			// switch passthrough to LOITER
@@ -1443,7 +1453,6 @@ static void update_alt()
 
 		// read barometer
 		baro_alt 			= read_barometer();
-		sonar_alt 			= sonar.read();
 
 		if(baro_alt < 1000){
 			scale = (sonar_alt - 400) / 200;
@@ -1585,15 +1594,16 @@ static void auto_throttle()
 	nav_throttle = get_nav_throttle(altitude_error);
 
 	// apply throttle control
-	g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew);
+	//g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew);
+	g.rc_3.servo_out = get_throttle(nav_throttle);
 
-	if(motor_armed){
+	//if(motor_armed){
 		// remember throttle offset
-		throttle_slew = g.rc_3.servo_out - g.rc_3.control_in;
-	}else{
+		//throttle_slew = g.rc_3.servo_out - g.rc_3.control_in;
+	//}else{
 		// don't allow
-		throttle_slew = 0;
-	}
+		//throttle_slew = 0;
+	//}
 
 	// clear the new data flag
 	invalid_throttle = false;
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 2b793c6299..caf9b6c0f1 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -150,7 +150,7 @@ static int
 get_throttle(int throttle_input)
 {
 	throttle_input = (float)throttle_input * angle_boost();
-	throttle_input = max(throttle_slew, throttle_input);
+	//throttle_input = max(throttle_slew, throttle_input);
 	return  max(throttle_input, 0);
 }
 
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index f371c7e472..00e90d5a14 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -33,7 +33,7 @@ static void arm_motors()
 
 					// Clear throttle slew
 					// -------------------
-					throttle_slew = 0;
+					//throttle_slew = 0;
 
 					// Remember Orientation
 					// --------------------
@@ -83,7 +83,7 @@ static void arm_motors()
 
 				// Clear throttle slew
 				// -------------------
-				throttle_slew = 0;
+				//throttle_slew = 0;
 
 				arming_counter++;