src/edu/berkeley/cs/obits/device/atmel/AvrDrone.c

   1 //\r
   2 // YOU MUST COMPILE THIS WITH -O3 OR THE AVR WILL NOT BE ABLE TO KEEP UP!!!!\r
   3 //\r
   4 \r
   5 #define F_CPU 3960000\r
   6 #define __AVR_AT94K__\r
   7 #include <avr/wdt.h>\r
   8 #include <avr/delay.h>\r
   9 #include <avr/io.h>\r
  10 #include <avr/signal.h>\r
  11 #include <avr/interrupt.h>\r
  12 \r
  13 unsigned char val = 0;\r
  14 \r
  15 volatile unsigned char dataReady = 0;\r
  16 volatile unsigned char data = 0;\r
  17 \r
  18 void initUART1(unsigned int baudRate, unsigned int doubleRate) {\r
  19   UBRRHI = (((baudRate) >> 8) & 0x000F); \r
  20   UBRR1  = ((baudRate) & 0x00FF); \r
  21   UCSR1B |= ((1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1)); \r
  22   /*\r
  23   if (doubleRate)\r
  24     UCSR1A |= (1 << U2X1);\r
  25   else\r
  26     UCSR1A &= ~(1 << U2X1);\r
  27   */\r
  28 }\r
  29 \r
  30 #define BUFSIZE (1024 *2)\r
  31 static char buf[BUFSIZE];\r
  32 volatile static int head = 0;\r
  33 volatile static int tail = 0;\r
  34 \r
  35 void send(char c) {\r
  36   while(!(UCSR1A & (1 << UDRE1)));           /* Wait for data Regiester to be empty */\r
  37   UDR1 = (int)c;\r
  38 }\r
  39 \r
  40 inline void portd(int bit, int on) {\r
  41   if (on) {\r
  42     PORTD &= ~(1<<bit);\r
  43   } else {\r
  44     PORTD |= (1<<bit);\r
  45   }\r
  46 }\r
  47 inline void cts(int c) {\r
  48   if (c) {\r
  49     PORTE &= ~(1 << 4);\r
  50     portd(0, 1);\r
  51   } else {\r
  52     PORTE |= (1 << 4);\r
  53     portd(0, 0);\r
  54   }\r
  55 }\r
  56 \r
  57 inline int inc(int x) { x++; if (x>=BUFSIZE) x=0; return x; }\r
  58 inline int full() { return inc(tail)==head; }\r
  59 inline int nearlyFull() {\r
  60   if (tail==head) return 0;\r
  61   if (tail < head) return (head-tail) < (BUFSIZE/2);\r
  62   return (tail-head) > (BUFSIZE/2);\r
  63 }\r
  64 inline int empty() { return head==tail; }\r
  65 \r
  66 inline char recv() {\r
  67   int q;\r
  68   char ret;\r
  69   while(empty()) cts(1);\r
  70   ret = buf[head];\r
  71   head = inc(head);\r
  72   if (!nearlyFull()) cts(0);\r
  73   return ret;\r
  74 }\r
  75 \r
  76 void init() {\r
  77   EIMF  = 0xFF;                          /* Enalbe External Interrrupt*/  \r
  78   DDRD = 0xFF;                           /* Configure PORTD as Output */\r
  79   DDRE = 1 << 4;                         /* ability to write to E */\r
  80   initUART1(1, 0);\r
  81   SREG |= 0x80;\r
  82   sei();\r
  83 }\r
  84 \r
  85 void conf(int z, int y, int x, int d) {\r
  86   FPGAX = x;\r
  87   FPGAY = y;\r
  88   FPGAZ = z;\r
  89   FPGAD = d;\r
  90 }\r
  91 \r
  92 void doreset() {\r
  93   int i;\r
  94   for(i=0; i<5; i++) {\r
  95     PORTD = ~0x01;\r
  96     _delay_ms(50);\r
  97     PORTD = ~0x02;\r
  98     _delay_ms(50);\r
  99     PORTD = ~0x04;\r
 100     _delay_ms(50);\r
 101     PORTD = ~0x08;\r
 102     _delay_ms(50);\r
 103   }\r
 104   PORTD = ~0x00;\r
 105   wdt_enable(WDTO_250MS);\r
 106   while(1) { }\r
 107 }\r
 108 \r
 109 SIGNAL(SIG_INTERRUPT1) {\r
 110   doreset();\r
 111 }\r
 112 \r
 113 void die() { cli(); cts(0); _delay_ms(2000); while(1) { } }\r
 114 SIGNAL(SIG_UART1_RECV) {\r
 115   if (UCSR1A & (1 << FE1))   { portd(2,0); portd(3,1); die(); }  // framing error, lock up with LED=01\r
 116   if ((UCSR1A & (1 << OR1))) { portd(2,1); portd(3,0); die(); }  // overflow; lock up with LED=10\r
 117   if (full())                { portd(2,1); portd(3,1); die(); }  // buffer overrun\r
 118   buf[tail] = UDR1;\r
 119   tail = inc(tail);\r
 120   SREG |= 0x80;\r
 121   sei();\r
 122   if (nearlyFull()) cts(0);\r
 123 }\r
 124 \r
 125 inline int hex(char c) {\r
 126   if (c >= '0' && c <= '9') return (c - '0');\r
 127   if (c >= 'a' && c <= 'f') return ((c - 'a') + 0xa);\r
 128   if (c >= 'A' && c <= 'F') return ((c - 'A') + 0xa);\r
 129   return -1;\r
 130 }\r
 131 \r
 132 int main() {\r
 133   int count;\r
 134   init();\r
 135   cts(0);\r
 136   send('O');\r
 137   send('B');\r
 138   send('I');\r
 139   send('T');\r
 140   send('S');\r
 141   send('\n');\r
 142   cts(1);\r
 143   for(;;) {\r
 144     int i, x=0, y=0, z=0, d=0;\r
 145     switch(recv()) {\r
 146       case 1:\r
 147         z = recv();\r
 148         y = recv();\r
 149         x = recv();\r
 150         d = recv();\r
 151         portd(1,1);\r
 152         conf(z, y, x, d);\r
 153         portd(1,0);\r
 154         break;\r
 155       default: die();\r
 156     }\r
 157   }\r
 158   return 0;\r
 159 }  \r
 160 \r