From 30102d28e41035ba09c0a0dc391c9c6eda99bdfd Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sun, 6 Feb 2011 07:06:36 +0000
Subject: [PATCH] removed D for nav, added extra timer for FBW and Nav.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1600 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/ArduCopterMega.pde | 11 +++++++++--
 ArduCopterMega/config.h           | 10 +++++-----
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index f23999b4a3..12775e1d97 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -388,11 +388,13 @@ byte 			slow_loopCounter;
 byte 			superslow_loopCounter;
 
 unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
+unsigned long 	nav2_loopTimer;				// used to track the elapsed ime for GPS nav
 
 uint8_t			delta_ms_medium_loop;
 uint8_t 		delta_ms_fast_loop; 		// Delta Time in miliseconds
 
 unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
+unsigned long 	dTnav2;						// Delta Time in milliseconds for navigation computations
 unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
 
@@ -535,6 +537,8 @@ void medium_loop()
 			
 			// calc pitch and roll to target
 			// -----------------------------
+			dTnav2 				= millis() - nav2_loopTimer;
+			nav2_loopTimer 		= millis();
 			calc_nav();
 
 			break;
@@ -860,6 +864,7 @@ void update_current_flight_mode(void)
 					fbw_timer = 0;
 
 					if(home_is_set == false){
+						scaleLongDown = 1;
 						// we are not using GPS
 						// reset the location
 						// RTL won't function
@@ -869,8 +874,8 @@ void update_current_flight_mode(void)
 						dTnav = 200;
 					}
 					
-					next_WP.lat = home.lat + rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
-					next_WP.lng = home.lng + rc_1.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
+					next_WP.lat = home.lat + rc_2.control_in / 2; // 4500 / 2 = 2250 = 25 meteres 
+					next_WP.lng = home.lng + rc_1.control_in / 2; // 4500 / 2 = 900 = 25 meteres 
 				}
 
 				// Output Pitch, Roll, Yaw and Throttle
@@ -1013,4 +1018,6 @@ void read_AHRS(void)
 	//-----------------------------------------------
 	dcm.update_DCM(G_Dt);
 	omega 	= dcm.get_gyro();
+	dcm.roll_sensor = 0;
+	dcm.pitch_sensor = 0;
 }
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 2e0131392b..278f23f2b9 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -355,7 +355,7 @@
 //
 // how much to we pitch towards the target
 #ifndef PITCH_MAX
-# define PITCH_MAX				21
+# define PITCH_MAX				25
 #endif
 
 
@@ -363,13 +363,13 @@
 // Navigation control gains
 //
 #ifndef NAV_P
-# define NAV_P					2.4
+# define NAV_P					1.5
 #endif
 #ifndef NAV_I
-# define NAV_I					0.01
+# define NAV_I					0.00
 #endif
 #ifndef NAV_D
-# define NAV_D					0.005
+# define NAV_D					0.00
 #endif
 #ifndef NAV_IMAX
 # define NAV_IMAX				250
@@ -386,7 +386,7 @@
 # define THROTTLE_BARO_I		0.04
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.15
+# define THROTTLE_BARO_D		0.45
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		100