Add GPS code.

1 files changed, 19 insertions, 0 deletions

diff --git a/main.cpp b/main.cpp
index 471e40e..c95731f 100644
--- a/main.cpp
+++ b/main.cpp
@@ -11,6 +11,7 @@
 
 #include "usart.h"
 #include "xbee.h"
+#include "gps.h"
 
 template<class T>
 inline void saturate(T& var, T absmax) {
@@ -82,6 +83,22 @@ uint32_t thstack[1024];
 
 Thread thread(thstack, sizeof(thstack), threadmain);
 
+GPS gps;
+
+void gps_thread_main() {
+	while(1) {
+		P<GPSMsg> msg = gps.read();
+		
+		if(msg->n < 128) {
+			xbee_send(2, msg->n, msg->buf);
+		}
+	}
+}
+
+uint32_t gps_stack[256];
+
+Thread gps_thread(gps_stack, sizeof(gps_stack), gps_thread_main);
+
 int main() {
 	// Initialize system timer.
 	Time::init();
@@ -124,8 +141,10 @@ int main() {
 	PID pid_yaw(6000, 0, 0);
 	
 	usart_enable();
+	gps.enable();
 	
 	thread.start();
+	gps_thread.start();
 	
 	while(1) {
 		// Wait for a new update.