Add GPS code.

3 files changed, 119 insertions, 0 deletions

diff --git a/gps.cpp b/gps.cpp
new file mode 100644
index 0000000..1ae3684
--- /dev/null
+++ b/gps.cpp
@@ -0,0 +1,40 @@
+#include "gps.h"
+
+GPS* GPS::self;
+
+template<>
+void interrupt<Interrupt::USART3>() {
+	GPS::self->irq();
+}
+
+void GPS::irq() {
+	
+	uint8_t c = USART3.DR;
+	
+	if(!incomplete_msg) {
+		incomplete_msg = msg_pool.create();
+		
+		if(!incomplete_msg) {
+			return;
+		}
+	}
+	
+	if(incomplete_msg->n == 0 && c != '$') {
+		return;
+	}
+	
+	if(incomplete_msg->n >= 128) {
+		incomplete_msg->n = 0;
+		return;
+	}
+	
+	incomplete_msg->buf[incomplete_msg->n++] = c;
+	
+	if(c == '\n') {
+		GPIOB.ODR ^= 1 << 1;
+		
+		const_cast<P<GPSMsg>&>(complete_msg) = incomplete_msg;
+		incomplete_msg.reset();
+		complete = true;
+	}
+}
diff --git a/gps.h b/gps.h
new file mode 100644
index 0000000..c37fc8a
--- /dev/null
+++ b/gps.h
@@ -0,0 +1,60 @@
+#ifndef GPS_H
+#define GPS_H
+
+#include "stm32.h"
+#include "interrupt.h"
+#include "thread.h"
+
+#include "pool.h"
+
+struct GPSMsg {
+	unsigned int n;
+	uint8_t buf[128];
+	
+	GPSMsg() : n(0) {}
+};
+
+class GPS {
+	friend void interrupt<Interrupt::USART3>();
+	
+	private:
+		static GPS* self;
+		
+		void irq();
+		
+		Pool<GPSMsg, 4> msg_pool;
+		
+		P<GPSMsg> incomplete_msg;
+		volatile P<GPSMsg> complete_msg;
+		
+		volatile bool complete;
+	
+	public:
+		GPS() {
+			self = this;
+		}
+		
+		void enable() {
+			RCC.enable(RCC.USART3);
+			USART3.BRR = 7500; // 4800 baud
+			USART3.CR1 = 0x202c;
+			
+			Interrupt::enable(Interrupt::USART3);
+		}
+		
+		P<GPSMsg> read() {
+			while(!complete) {
+				Thread::yield();
+			}
+			
+			complete = false;
+			
+			P<GPSMsg> msg = const_cast<P<GPSMsg>&>(complete_msg);
+			
+			const_cast<P<GPSMsg>&>(complete_msg).reset();
+			
+			return msg;
+		}
+};
+
+#endif
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.