From 52d77407d481055381cfe55562c197453adbd049 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 23 Mar 2012 16:27:37 +1100
Subject: [PATCH] AHRS: added GPS support to AHRS test

---
 .../AP_AHRS/examples/AHRS_Test/AHRS_Test.pde  | 22 +++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
index 5864d32d61..09d6a12af7 100644
--- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
+++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
@@ -24,9 +24,12 @@
 #include <Filter.h>
 
 // uncomment this for a APM2 board
-#define APM2_HARDWARE
+//#define APM2_HARDWARE
 
-FastSerialPort(Serial, 0);
+#define WITH_GPS 0
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
 
 Arduino_Mega_ISR_Registry isr_registry;
 AP_TimerProcess  scheduler;
@@ -46,11 +49,13 @@ AP_Compass_HMC5843 compass;
 
 static GPS         *g_gps;
 
+AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
+
 AP_IMU_INS imu(&ins);
 
 // choose which AHRS system to use
-//AP_AHRS_DCM  ahrs(&imu, g_gps);
-AP_AHRS_Quaternion  ahrs(&imu, g_gps);
+AP_AHRS_DCM  ahrs(&imu, g_gps);
+//AP_AHRS_Quaternion  ahrs(&imu, g_gps);
 
 AP_Baro_BMP085_HIL      barometer;
 
@@ -107,7 +112,13 @@ void setup(void)
 		Serial.printf("Enabling compass\n");
 		compass.null_offsets_enable();
 		ahrs.set_compass(&compass);
+	} else {
+		Serial.printf("No compass detected\n");
 	}
+	g_gps = &g_gps_driver;
+#if WITH_GPS
+	g_gps->init();
+#endif
 }
 
 void loop(void)
@@ -127,6 +138,9 @@ void loop(void)
 		compass.calculate(ahrs.get_dcm_matrix());
 		// read compass at 10Hz
 		last_compass = now;
+#if WITH_GPS
+		g_gps->update();
+#endif
 	}
 
 	ahrs.update();