From d475e7ced505a4b6be3abee5ca600a64b34c9216 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@sdsfcm02.smartdesign.com>
Date: Mon, 14 Nov 2011 16:43:30 -0800
Subject: [PATCH] Added better navigation flag for more accurate dtnav timing
 Added better support for climb rate in SIM

---
 ArduCopter/ArduCopter.pde | 48 ++++++++++++++++++++++-----------------
 1 file changed, 27 insertions(+), 21 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 0ba056a705..4487d1edd8 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -299,6 +299,7 @@ static bool 	did_ground_start	= false;		// have we ground started after first ar
 
 // Location & Navigation
 // ---------------------
+static bool	nav_ok;
 static const float radius_of_earth 	= 6378100;		// meters
 static const float gravity 			= 9.81;			// meters/ sec^2
 static int32_t		target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
@@ -640,18 +641,14 @@ static void medium_loop()
 			medium_loopCounter++;
 
 			// Auto control modes:
-			if(g_gps->new_data && g_gps->fix){
+			if(nav_ok){
+				// clear nav flag
+				nav_ok = false;
 
 				// invalidate GPS data
+				// -------------------
 				g_gps->new_data 	= false;
 
-				// we are not tracking I term on navigation, so this isn't needed
-				dTnav 				= (float)(millis() - nav_loopTimer)/ 1000.0;
-				nav_loopTimer 		= millis();
-
-				// prevent runup from bad GPS
-				dTnav = min(dTnav, 1.0);
-
 				// calculate the copter's desired bearing and WP distance
 				// ------------------------------------------------------
 				if(navigate()){
@@ -663,10 +660,7 @@ static void medium_loop()
 					if (g.log_bitmask & MASK_LOG_NTUN)
 						Log_Write_Nav_Tuning();
 				}
-			}else{
-				g_gps->new_data = false;
 			}
-
 			break;
 
 		// command processing
@@ -900,18 +894,27 @@ static void update_GPS(void)
 		gps_watchdog++;
 	}else{
 		// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
-		// commented temporarily
-		//nav_roll  >>= 1;
-		//nav_pitch >>= 1;
+		nav_roll  >>= 1;
+		nav_pitch >>= 1;
 	}
 
     if (g_gps->new_data && g_gps->fix) {
 		gps_watchdog = 0;
 
+		// OK to run the nav routines
+		nav_ok = true;
+
 		// for performance
 		// ---------------
 		gps_fix_count++;
 
+		// we are not tracking I term on navigation, so this isn't needed
+		dTnav 				= (float)(millis() - nav_loopTimer)/ 1000.0;
+		nav_loopTimer 		= millis();
+
+		// prevent runup from bad GPS
+		dTnav = min(dTnav, 1.0);
+
 		if(ground_start_count > 1){
 			ground_start_count--;
 
@@ -935,11 +938,14 @@ static void update_GPS(void)
 		if (g.log_bitmask & MASK_LOG_GPS){
 			Log_Write_GPS();
 		}
-	}
-	#if HIL_MODE == HIL_MODE_ATTITUDE					// don't execute in HIL mode
-		update_altitude();
-	#endif
 
+		#if HIL_MODE == HIL_MODE_ATTITUDE					// only execute in HIL mode
+			update_altitude();
+		#endif
+
+	} else {
+		g_gps->new_data = false;
+	}
 }
 
 
@@ -1226,12 +1232,12 @@ static void update_altitude()
 	#if HIL_MODE == HIL_MODE_ATTITUDE
 		// we are in the SIM, fake out the baro and Sonar
 		int fake_relative_alt = g_gps->altitude - gps_base_alt;
-		int temp_baro_alt	= fake_relative_alt;
-		baro_rate 			= (temp_baro_alt - old_baro_alt) * 10;
-		old_baro_alt		= temp_baro_alt;
 		baro_alt			= fake_relative_alt;
 		sonar_alt			= fake_relative_alt;
 
+		baro_rate 			= (baro_alt - old_baro_alt) * 5; // 5hz
+		old_baro_alt		= baro_alt;
+
 	#else
 		// This is real life
 		// calc the vertical accel rate