diff options
author | Vegard Storheil Eriksen <zyp@jvnv.net> | 2011-10-10 01:13:52 +0200 |
---|---|---|
committer | Vegard Storheil Eriksen <zyp@jvnv.net> | 2011-10-10 01:13:52 +0200 |
commit | 261e6be038754388212e7a7b69da1318d6a171d2 (patch) | |
tree | 8788bffbb8248aeab9ed07c326445bac491633b7 | |
parent | f8e706f8bc5a69f5b8134624516a3597d335685e (diff) |
Add GPS code.
-rw-r--r-- | gps.cpp | 40 | ||||
-rw-r--r-- | gps.h | 60 | ||||
-rw-r--r-- | main.cpp | 19 |
3 files changed, 119 insertions, 0 deletions
@@ -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; + } +} @@ -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 @@ -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. |