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 void send(char c) {\r
  85 \r
  86   write_buf[write_buf_tail] = c;\r
  87   write_buf_tail = inc(write_buf_tail);\r
  88 \r
  89   char ret = write_buf[write_buf_head];\r
  90   write_buf_head = inc(write_buf_head);\r
  91 \r
  92   while(!(UCSR1A & (1 << UDRE1)));           /* Wait for data Regiester to be empty */\r
  93   UDR1 = (int)ret;\r
  94 }\r
  95 \r
  96 \r
  97 void init() {\r
  98   read_buf_head = 0;\r
  99   read_buf_tail = 0;\r
 100   write_buf_head = 0;\r
 101   write_buf_tail = 0;\r
 102   EIMF  = 0xFF;                          /* Enalbe External Interrrupt*/  \r
 103   DDRD = 0xFF;                           /* Configure PORTD as Output */\r
 104   DDRE = 1 << 4;                         /* ability to write to E */\r
 105   initUART1(1, 0);\r
 106   SREG |= 0x80;\r
 107   sei();\r
 108 }\r
 109 \r
 110 \r
 111 \r
 112 void conf(int z, int y, int x, int d) {\r
 113   FPGAX = x;\r
 114   FPGAY = y;\r
 115   FPGAZ = z;\r
 116   FPGAD = d;\r
 117 }\r
 118 \r
 119 void doreset() {\r
 120   int i;\r
 121   for(i=0; i<5; i++) {\r
 122     PORTD = ~0x01;\r
 123     _delay_ms(50);\r
 124     PORTD = ~0x02;\r
 125     _delay_ms(50);\r
 126     PORTD = ~0x04;\r
 127     _delay_ms(50);\r
 128     PORTD = ~0x08;\r
 129     _delay_ms(50);\r
 130   }\r
 131   PORTD = ~0x00;\r
 132   wdt_enable(WDTO_250MS);\r
 133   while(1) { }\r
 134 }\r
 135 \r
 136 #define TIMERVAL 100\r
 137 ISR(SIG_OVERFLOW0) { \r
 138   PORTD = ~FISUA;\r
 139   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 140   TIMSK |= (1<<TOIE0);        //enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 141   sei();\r
 142 } \r
 143 void init_timer()  { \r
 144   TCCR0 |= (1<<CS00);         // set the timer0 prescaler to CK\r
 145   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 146   TIMSK |= (1<<TOIE0);        //enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 147 } \r
 148 \r
 149 ISR(SIG_INTERRUPT1) {   // use interrupt1 since interrupt0 is sent by the watchdog (I think)\r
 150   doreset();\r
 151 }\r
 152 \r
 153 void die() { cli(); cts(0); _delay_ms(2000); while(1) { portd(2,0); portd(2,1); } }\r
 154 ISR(SIG_UART1_RECV) {\r
 155   if (UCSR1A & (1 << FE1))   { portd(2,0); portd(3,1); die(); }  // framing error, lock up with LED=01\r
 156   if ((UCSR1A & (1 << OR1))) { portd(2,1); portd(3,0); die(); }  // overflow; lock up with LED=10\r
 157   if (read_full())           { portd(2,1); portd(3,1); die(); }  // buffer overrun\r
 158   read_buf[read_buf_tail] = UDR1;\r
 159   read_buf_tail = inc(read_buf_tail);\r
 160   SREG |= 0x80;\r
 161   sei();\r
 162   if (read_nearlyFull()) cts(0);\r
 163 }\r
 164 \r
 165 inline int hex(char c) {\r
 166   if (c >= '0' && c <= '9') return (c - '0');\r
 167   if (c >= 'a' && c <= 'f') return ((c - 'a') + 0xa);\r
 168   if (c >= 'A' && c <= 'F') return ((c - 'A') + 0xa);\r
 169   return -1;\r
 170 }\r
 171 \r
 172 int main() {\r
 173   int count;\r
 174   init();\r
 175   cts(0);\r
 176   send('O');\r
 177   send('B');\r
 178   send('I');\r
 179   send('T');\r
 180   send('S');\r
 181   send('\n');\r
 182   cts(1);\r
 183   for(;;) {\r
 184     int i, x=0, y=0, z=0, d=0;\r
 185     switch(recv()) {\r
 186       case 1:\r
 187         z = recv();\r
 188         y = recv();\r
 189         x = recv();\r
 190         d = recv();\r
 191         portd(1,1);\r
 192         conf(z, y, x, d);\r
 193         portd(1,0);\r
 194         break;\r
 195       case 2:\r
 196         portd(1,1);\r
 197         send(FISUA);\r
 198         portd(1,0);\r
 199         break;\r
 200       case 3:\r
 201         init_timer();\r
 202         break;\r
 203       default: die();\r
 204     }\r
 205   }\r
 206   return 0;\r
 207 }  \r
 208 \r