From d9b09d2c93a8a220ec6384785e18e8cf8c8f1a18 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 11 Aug 2012 12:01:08 +1000
Subject: [PATCH] APM: allow for navigation by dead-reckoning

we now ask AHRS if we have a position estimate, and use that if
available
---
 ArduPlane/ArduPlane.pde   | 21 +++++++++++----------
 ArduPlane/GCS_Mavlink.pde |  2 +-
 ArduPlane/navigation.pde  |  4 +---
 3 files changed, 13 insertions(+), 14 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index f62df2a4d9..4eefa3f2f6 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -390,6 +390,9 @@ static int16_t     ground_start_avg;
 // If we do not detect GPS at startup, we stop trying and assume GPS is not connected
 static bool	GPS_enabled 	= false;
 
+// true if we have a position estimate from AHRS
+static bool have_position;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Location & Navigation
 ////////////////////////////////////////////////////////////////////////////////
@@ -854,12 +857,9 @@ Serial.println(tempaccel.z, DEC);
 			// -------------------------------
 			read_control_switch();
 
-			if(g_gps->new_data){
-				g_gps->new_data 	= false;
-				// calculate the plane's desired bearing
-				// -------------------------------------
-				navigate();
-			}
+            // calculate the plane's desired bearing
+            // -------------------------------------
+            navigate();
 
 			break;
 
@@ -985,7 +985,12 @@ static void update_GPS(void)
 	g_gps->update();
 	update_GPS_light();
 
+    // get position from AHRS
+    have_position = ahrs.get_position(&current_loc);
+
 	if (g_gps->new_data && g_gps->fix) {
+        g_gps->new_data = false;
+
 		// for performance
 		// ---------------
 		gps_fix_count++;
@@ -1021,10 +1026,6 @@ static void update_GPS(void)
 			}
 		}
 
-
-		current_loc.lng = g_gps->longitude;    // Lon * 10**7
-		current_loc.lat = g_gps->latitude;     // Lat * 10**7
-
         // see if we've breached the geo-fence
         geofence_check(false);
 	}
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 44a59ab91e..3267ceb1e2 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -242,7 +242,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         g_gps->ground_speed * rot.a.x,  // X speed cm/s
         g_gps->ground_speed * rot.b.x,  // Y speed cm/s
         g_gps->ground_speed * rot.c.x,
-        g_gps->ground_course);          // course in 1/100 degree
+        ahrs.yaw_sensor);
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 6e4ff683e5..71007ba353 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -7,9 +7,7 @@ static void navigate()
 {
 	// do not navigate with corrupt data
 	// ---------------------------------
-	if (g_gps->fix == 0)
-	{
-		g_gps->new_data = false;
+	if (!have_position) {
 		return;
 	}