From db331efbae036e1b675ca0ad1c6ca75facc51a7b Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@gmail.com>
Date: Sun, 11 Sep 2011 11:22:01 -0700
Subject: [PATCH] Circle Mode now working 250Hz loop implemented

---
 ArduCopter/ArduCopter.pde | 13 +++++--------
 ArduCopter/motors.pde     |  9 +++++----
 ArduCopter/system.pde     |  2 +-
 ArduCopter/test.pde       |  2 +-
 4 files changed, 12 insertions(+), 14 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 37331f0ce3..541a1ec586 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -527,7 +527,7 @@ void loop()
 {
 	// We want this to execute fast
 	// ----------------------------
-	if (millis() - fast_loopTimer >= 5) {
+	if (millis() - fast_loopTimer >= 4) {
 		//PORTK |= B00010000;
 		delta_ms_fast_loop 	= millis() - fast_loopTimer;
 		fast_loopTimer		= millis();
@@ -543,6 +543,7 @@ void loop()
 		fast_loop();
 		fast_loopTimeStamp = millis();
 	}
+	//PORTK &= B11101111;
 
 	if (millis() - fiftyhz_loopTimer > 19) {
 		delta_ms_fiftyhz 		= millis() - fiftyhz_loopTimer;
@@ -577,7 +578,6 @@ void loop()
         }
 		//PORTK &= B10111111;
 	}
-	//PORTK &= B11101111;
 }
 //  PORTK |= B01000000;
 //	PORTK &= B10111111;
@@ -1268,16 +1268,13 @@ static void update_navigation()
 			// calculates desired Yaw
 			update_auto_yaw();
 			{
-				circle_angle += dTnav; //1000 * (dTnav/1000);
-
-				if (circle_angle >= 36000)
-					circle_angle -= 36000;
+				//circle_angle += dTnav; //1000 * (dTnav/1000);
+				circle_angle = wrap_360(target_bearing + 2000 + 18000);
 
 				target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle));
 				target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle));
 			}
 
-
 			// calc the lat and long error to the target
 			calc_location_error(&target_WP);
 
@@ -1301,7 +1298,7 @@ static void read_AHRS(void)
 		hil.update();
 	#endif
 
-	dcm.update_DCM(G_Dt);//, _tog);
+	dcm.update_DCM_fast(G_Dt);//, _tog);
 	omega = dcm.get_gyro();
 }
 
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 4e64316376..5a4d0e3e9a 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -34,8 +34,9 @@ static void arm_motors()
 					// Tune down DCM
 					// -------------------
 					#if HIL_MODE != HIL_MODE_ATTITUDE
-					dcm.kp_roll_pitch(0.030000);
-					dcm.ki_roll_pitch(0.000006);
+					  dcm.kp_roll_pitch(0.030000);
+					  dcm.ki_roll_pitch(0.00001278),	// 50 hz I term
+					//dcm.ki_roll_pitch(0.000006);
 					#endif
 
 					// tune down compass
@@ -95,8 +96,8 @@ static void arm_motors()
 				// Tune down DCM
 				// -------------------
 				#if HIL_MODE != HIL_MODE_ATTITUDE
-				dcm.kp_roll_pitch(0.12);		// higher for quads
-				dcm.ki_roll_pitch(0.00000319); 	// 1/4 of the normal rate for 200 hz loop
+				dcm.kp_roll_pitch(0.12);			// higher for fast recovery
+				//dcm.ki_roll_pitch(0.00000319); 	// 1/4 of the normal rate for 200 hz loop
 				#endif
 
 				// tune up compass
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 3b8ef34edd..70daa8e178 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "ArduCopter 2.0.41 Beta", main_menu_commands);
+MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands);
 
 #endif // CLI_ENABLED
 
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 6dcd5614f4..895b41d55b 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -447,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 								accels.x, accels.y, accels.z,
 								gyros.x,  gyros.y,  gyros.z);
 			*/
-			///*
+			/*
 			Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
 								cos_pitch_x,
 								sin_pitch_y,