src/edu/berkeley/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 \r
   7 #if !defined(__AVR_AT94K__)\r
   8 #error you forgot to put -mmcu=at94k on the command line\r
   9 #endif\r
  10 \r
  11 #include <avr/wdt.h>\r
  12 #include <util/delay.h>\r
  13 #include <avr/io.h>\r
  14 #include <avr/interrupt.h>\r
  15 \r
  16 void initUART1(unsigned int baudRate, unsigned int doubleRate) {\r
  17   UBRRHI = (((baudRate) >> 8) & 0x000F); \r
  18   UBRR1  = ((baudRate) & 0x00FF); \r
  19   UCSR1B |= ((1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1)); \r
  20   /*\r
  21   if (doubleRate)\r
  22     UCSR1A |= (1 << U2X1);\r
  23   else\r
  24     UCSR1A &= ~(1 << U2X1);\r
  25   */\r
  26 }\r
  27 \r
  28 #define BUFSIZE (1024)\r
  29 \r
  30 inline void portd(int bit, int on) {\r
  31   if (on) {\r
  32     PORTD &= ~(1<<bit);\r
  33   } else {\r
  34     PORTD |= (1<<bit);\r
  35   }\r
  36 }\r
  37 inline void cts(int c) {\r
  38   if (c) {\r
  39     PORTE &= ~(1 << 4);\r
  40     portd(0, 1);\r
  41   } else {\r
  42     PORTE |= (1 << 4);\r
  43     portd(0, 0);\r
  44   }\r
  45 }\r
  46 \r
  47 \r
  48 // RECV //////////////////////////////////////////////////////////////////////////////\r
  49 \r
  50 char read_buf[BUFSIZE];\r
  51 volatile int read_buf_head;\r
  52 volatile int read_buf_tail;\r
  53 char write_buf[BUFSIZE];\r
  54 volatile int write_buf_head;\r
  55 volatile int write_buf_tail;\r
  56 \r
  57 inline int inc(int x) { x++; if (x>=BUFSIZE) x=0; return x; }\r
  58 inline int read_full() { return inc(read_buf_tail)==read_buf_head; }\r
  59 inline int read_empty() { return read_buf_head==read_buf_tail; }\r
  60 inline int read_nearlyFull() {\r
  61   if (read_buf_tail==read_buf_head) return 0;\r
  62   if (read_buf_tail < read_buf_head) return (read_buf_head-read_buf_tail) < (BUFSIZE/2);\r
  63   return (read_buf_tail-read_buf_head) > (BUFSIZE/2);\r
  64 }\r
  65 \r
  66 inline int write_full() { return inc(write_buf_tail)==write_buf_head; }\r
  67 inline int write_empty() { return write_buf_head==write_buf_tail; }\r
  68 inline int write_nearlyFull() {\r
  69   if (write_buf_tail==write_buf_head) return 0;\r
  70   if (write_buf_tail < write_buf_head) return (write_buf_head-write_buf_tail) < (BUFSIZE/2);\r
  71   return (write_buf_tail-write_buf_head) > (BUFSIZE/2);\r
  72 }\r
  73 \r
  74 inline char recv() {\r
  75   int q;\r
  76   char ret;\r
  77   while(read_empty()) cts(1);\r
  78   ret = read_buf[read_buf_head];\r
  79   read_buf_head = inc(read_buf_head);\r
  80   if (!read_nearlyFull()) cts(0);\r
  81   return ret;\r
  82 }\r
  83 \r
  84 ISR(SIG_UART1_DATA) {\r
  85   //if (write_empty()) return;\r
  86   portd(1, 0);\r
  87   _delay_ms(10);\r
  88   portd(1, 1);\r
  89   _delay_ms(10);\r
  90   UCSR1B &= ~(1 << UDRIE1);\r
  91   sei();\r
  92 }\r
  93 \r
  94 void send(char c) {\r
  95 \r
  96   write_buf[write_buf_tail] = c;\r
  97   write_buf_tail = inc(write_buf_tail);\r
  98 \r
  99   UCSR1B |= (1 << UDRIE1);\r
 100 \r
 101   while(!(UCSR1A & (1 << UDRE1)));           /* Wait for data Regiester to be empty */\r
 102   char ret = write_buf[write_buf_head];\r
 103   write_buf_head = inc(write_buf_head);\r
 104   UDR1 = (int)ret;\r
 105 }\r
 106 \r
 107 \r
 108 void init() {\r
 109   read_buf_head = 0;\r
 110   read_buf_tail = 0;\r
 111   write_buf_head = 0;\r
 112   write_buf_tail = 0;\r
 113   EIMF  = 0xFF;                          /* Enalbe External Interrrupt*/  \r
 114   DDRD = 0xFF;                           /* Configure PORTD as Output */\r
 115   DDRE = 1 << 4;                         /* ability to write to E */\r
 116   initUART1(1, 0);\r
 117   SREG |= 0x80;\r
 118   sei();\r
 119 }\r
 120 \r
 121 \r
 122 \r
 123 void conf(int z, int y, int x, int d) {\r
 124   FPGAX = x;\r
 125   FPGAY = y;\r
 126   FPGAZ = z;\r
 127   FPGAD = d;\r
 128 }\r
 129 \r
 130 void doreset() {\r
 131   int i;\r
 132   for(i=0; i<5; i++) {\r
 133     PORTD = ~0x01;\r
 134     _delay_ms(50);\r
 135     PORTD = ~0x02;\r
 136     _delay_ms(50);\r
 137     PORTD = ~0x04;\r
 138     _delay_ms(50);\r
 139     PORTD = ~0x08;\r
 140     _delay_ms(50);\r
 141   }\r
 142   PORTD = ~0x00;\r
 143   wdt_enable(WDTO_250MS);\r
 144   while(1) { }\r
 145 }\r
 146 \r
 147 #define TIMERVAL 100\r
 148 ISR(SIG_OVERFLOW0) { \r
 149   PORTD = ~FISUA;\r
 150   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 151   TIMSK |= (1<<TOIE0);        //enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 152   sei();\r
 153 } \r
 154 void init_timer()  { \r
 155   TCCR0 |= (1<<CS00);         // set the timer0 prescaler to CK\r
 156   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 157   TIMSK |= (1<<TOIE0);        //enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 158 } \r
 159 \r
 160 ISR(SIG_INTERRUPT1) {   // use interrupt1 since interrupt0 is sent by the watchdog (I think)\r
 161   doreset();\r
 162 }\r
 163 \r
 164 void die() { cli(); cts(0); _delay_ms(2000); while(1) { portd(2,0); portd(2,1); } }\r
 165 ISR(SIG_UART1_RECV) {\r
 166   if (UCSR1A & (1 << FE1))   { portd(2,0); portd(3,1); die(); }  // framing error, lock up with LED=01\r
 167   if ((UCSR1A & (1 << OR1))) { portd(2,1); portd(3,0); die(); }  // overflow; lock up with LED=10\r
 168   if (read_full())           { portd(2,1); portd(3,1); die(); }  // buffer overrun\r
 169   read_buf[read_buf_tail] = UDR1;\r
 170   read_buf_tail = inc(read_buf_tail);\r
 171   SREG |= 0x80;\r
 172   sei();\r
 173   if (read_nearlyFull()) cts(0);\r
 174 }\r
 175 \r
 176 inline int hex(char c) {\r
 177   if (c >= '0' && c <= '9') return (c - '0');\r
 178   if (c >= 'a' && c <= 'f') return ((c - 'a') + 0xa);\r
 179   if (c >= 'A' && c <= 'F') return ((c - 'A') + 0xa);\r
 180   return -1;\r
 181 }\r
 182 \r
 183 int main() {\r
 184   int count;\r
 185   init();\r
 186   cts(0);\r
 187   send('O');\r
 188   send('B');\r
 189   send('I');\r
 190   send('T');\r
 191   send('S');\r
 192   send('\n');\r
 193   cts(1);\r
 194   for(;;) {\r
 195     int i, x=0, y=0, z=0, d=0;\r
 196     switch(recv()) {\r
 197       case 1:\r
 198         z = recv();\r
 199         y = recv();\r
 200         x = recv();\r
 201         d = recv();\r
 202         portd(1,1);\r
 203         conf(z, y, x, d);\r
 204         portd(1,0);\r
 205         break;\r
 206       case 2:\r
 207         portd(1,1);\r
 208         send(FISUA);\r
 209         portd(1,0);\r
 210         break;\r
 211       case 3:\r
 212         init_timer();\r
 213         break;\r
 214       default: die();\r
 215     }\r
 216   }\r
 217   return 0;\r
 218 }  \r
 219 \r