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