From 5d756decd28f99f1f6213767d0d18d7b7353304f Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Thu, 9 Aug 2012 16:45:47 -0700
Subject: [PATCH] ACM : Attitude.pde

yaw limit, moved toy code out of att to it's own pre
---
 ArduCopter/Attitude.pde | 137 +++-------------------------------------
 1 file changed, 10 insertions(+), 127 deletions(-)

diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 52828ac3a2..1cef36ec0b 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -338,8 +338,16 @@ get_rate_yaw(int32_t target_rate)
 	}
 #endif
 
-	// constrain output
-	return output;
+	#if FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME
+		// constrain output
+		return output;
+	#else
+		// output control:
+		int16_t yaw_limit = 2000 + abs(g.rc_4.control_in);
+		// smoother Yaw control:
+		return constrain(output, -yaw_limit, yaw_limit);
+	#endif
+
 }
 
 static int16_t
@@ -773,128 +781,3 @@ get_of_pitch(int32_t control_pitch)
     return control_pitch;
 #endif
 }
-
-
-// THOR
-// The function call for managing the flight mode Toy
-void roll_pitch_toy()
-{
-	bool manual_control = false;
-
-	if(g.rc_2.control_in != 0){
-		// If we pitch forward or back, resume manually control
-		manual_control  = true;
-	}
-
-	// Yaw control - Yaw is always available, and will NOT exit the
-	// user from Loiter mode
-	int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
-
-	if(g.rc_1.control_in != 0){ // roll
-		g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
-		yaw_stopped = false;
-		yaw_timer = 150;
-
-	}else if (!yaw_stopped){
-		g.rc_4.servo_out = get_acro_yaw(0);
-		yaw_timer--;
-
-		if((yaw_timer == 0) || (fabs(omega.z) < .17)){
-			yaw_stopped = true;
-			nav_yaw = ahrs.yaw_sensor;
-		}
-	}else{
-		if(motors.armed() == false)
-			nav_yaw = ahrs.yaw_sensor;
-		g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
-	}
-
-	if(manual_control){
-		// user is in control: reset count-up timer
-		toy_input_timer = 0;
-
-		// roll_rate is the outcome of the linear equation or lookup table
-		// based on speed and Yaw rate
-		int16_t roll_rate = 0;
-
-		// We manually set out modes based on the state of Toy mode:
-		// Handle throttle manually
-		throttle_mode 	= THROTTLE_MANUAL;
-
-		// Dont try to navigate or integrate a nav error
-		wp_control 		= NO_NAV_MODE;
-
-	#if TOY_LOOKUP == 1
-		uint8_t xx, yy;
-		// Lookup value
-		xx	= g_gps->ground_speed / 200;
-		yy	= abs(yaw_rate / 500);
-
-		// constrain to lookup Array range
-		xx = constrain(xx, 0, 3);
-		yy = constrain(yy, 0, 8);
-
-		roll_rate = toy_lookup[yy * 4 + xx];
-
-		if(yaw_rate == 0)
-			roll_rate = 0;
-		else if(yaw_rate < 0)
-			roll_rate = -roll_rate;
-
-		int16_t roll_limit = 4500 / g.toy_yaw_rate;
-		roll_rate = constrain(roll_rate, -roll_limit, roll_limit);
-	#else
-		// yaw_rate = roll angle
-		// Linear equation for Yaw:Speed to Roll
-		// default is 1000, lower for more roll action
-		//roll_rate = ((float)g_gps->ground_speed / 600) * (float)yaw_rate;
-		roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
-		//Serial.printf("roll_rate: %d\n",roll_rate);
-
-		// limit roll rate to 15, 30, or 45 deg per second.
-		//int16_t roll_limit = 4500 / g.toy_yaw_rate;
-		//roll_rate = constrain(roll_rate, -roll_limit, roll_limit);
-
-		roll_rate = constrain(roll_rate, -2500, 2500);
-
-		//Serial.printf("yaw_rate %d, roll_rate %d, lim %d\n",yaw_rate, roll_rate, roll_limit);
-	#endif
-
-		// Output the attitude
-		g.rc_1.servo_out = get_stabilize_roll(roll_rate);
-		g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
-
-	}else{
-		//no user input
-		// Count-up to decision tp Loiter
-		toy_input_timer++;
-
-		//if (toy_input_timer == TOY_DELAY){
-		if((wp_control != LOITER_MODE) && ((g_gps->ground_speed < 150) || (toy_input_timer == TOY_DELAY))){
-
-			// clear our I terms for Nav or we will carry over old values
-			reset_wind_I();
-			// loiter
-			wp_control = LOITER_MODE;
-
-			// we are in an alt hold throttle with manual override
-			throttle_mode 	= THROTTLE_HOLD;
-
-			set_next_WP(&current_loc);
-		}
-
-		if (wp_control == LOITER_MODE){
-			// prevent timer overflow
-			toy_input_timer = TOY_DELAY;
-
-			// outputs the needed nav_control to maintain speed and direction
-			g.rc_1.servo_out 	= get_stabilize_roll(auto_roll);
-			g.rc_2.servo_out 	= get_stabilize_pitch(auto_pitch);
-
-		}else{
-			// Coast
-			g.rc_1.servo_out 	= get_stabilize_roll(0);
-			g.rc_2.servo_out 	= get_stabilize_pitch(0);
-		}
-	}
-}
\ No newline at end of file