1 files changed, 21 insertions, 1 deletions
diff --git a/main.cpp b/main.cpp
index d3c14b3..ea6490d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,11 +1,15 @@
 #include "stm32.h"
 #include "interrupt.h"
+#include "thread.h"
 
 #include "ppmsum.h"
 #include "i2c.h"
 
 #include "itg3200.h"
 
+#include "usart.h"
+#include "xbee.h"
+
 template<class T>
 inline void saturate(T& var, T absmax) {
 	if(var > absmax) {
@@ -52,6 +56,18 @@ class PID {
 		}
 };
 
+void threadmain() {
+	while(1) {
+		GPIOB.ODR ^= 1 << 1;
+		
+		xbee_send(3, (uint8_t*)"hei");
+	}
+}
+
+uint32_t thstack[1024];
+
+Thread thread(thstack, sizeof(thstack), threadmain);
+
 int main() {
 	RCC.enable(RCC.AFIO);
 	RCC.enable(RCC.IOPA);
@@ -86,9 +102,13 @@ int main() {
 	PID pid_roll(6000, 0, 0);
 	PID pid_yaw(6000, 0, 0);
 	
+	usart_enable();
+	
 	while(1) {
 		// Wait for a new update.
-		while(!(TIM2.SR & 0x01));
+		while(!(TIM2.SR & 0x01)) {
+			Thread::yield();
+		}
 		TIM2.SR = 0;
 		
 		// Read sensors.