arducontrol/interface.c

   1 #include "interface.h"
   2 #include "monitor.h"
   3 #include "motorola.h"
   4 #include "output.h"
   5 #include "serial.h"
   6
   7 volatile uint8_t recv_buf[32];
   8 uint8_t recv_head = 0;
   9 uint8_t recv_tail = 0;
  10 volatile uint8_t recv_fill = 0;
  11 volatile uint8_t recv_overrun = 0;
  12 uint8_t cmd_buf[15];
  13 uint8_t cmd_length;
  14
  15 void process_commands();
  16 uint8_t process_command();
  17
  18 void interface_init()
  19 {
  20         DDRD = (DDRD&0xFC)|0x02;
  21
  22         serial_init(9600);
  23 }
  24
  25 void interface_check()
  26 {
  27         if(recv_overrun)
  28         {
  29                 serial_write(0xFE);
  30                 serial_write(RECEIVE_OVERRUN);
  31                 recv_overrun = 0;
  32         }
  33         if(recv_fill>0)
  34                 process_commands();
  35 }
  36
  37 void process_commands()
  38 {
  39         while(recv_fill>0)
  40         {
  41                 uint8_t consumed;
  42                 uint8_t c = recv_buf[recv_tail];
  43
  44                 cmd_length = 0;
  45
  46                 if(c>=0xF0)
  47                 {
  48                         cmd_length = ~c;
  49                         if(recv_fill<=cmd_length)
  50                                 break;
  51
  52                         uint8_t i, j;
  53                         for(i=0, j=recv_tail+1; i<cmd_length; ++i, ++j)
  54                         {
  55                                 if(j>=sizeof(recv_buf))
  56                                         j = 0;
  57                                 cmd_buf[i] = recv_buf[j];
  58                         }
  59
  60                         consumed = 1+cmd_length;
  61                 }
  62                 else
  63                 {
  64                         serial_write(0xFE);
  65                         serial_write(FRAMING_ERROR);
  66                         consumed = 1;
  67                 }
  68
  69                 recv_tail += consumed;
  70                 if(recv_tail>=sizeof(recv_buf))
  71                         recv_tail -= sizeof(recv_buf);
  72                 recv_fill -= consumed;
  73
  74                 if(cmd_length>0)
  75                 {
  76                         uint8_t result = process_command();
  77                         serial_write(0xFE);
  78                         serial_write(result);
  79                 }
  80         }
  81 }
  82
  83 uint8_t process_command()
  84 {
  85         uint8_t type = cmd_buf[0]>>4;
  86         if(type==0)
  87         {
  88                 uint8_t subtype = (cmd_buf[0]>>3)&1;
  89                 if(subtype==0)
  90                         return output_command();
  91                 else
  92                         return monitor_command();
  93         }
  94         else if(type==1)
  95                 return motorola_command();
  96         else
  97                 return INVALID_COMMAND;
  98 }
  99
 100 static inline void receive(uint8_t c)
 101 {
 102         if(recv_fill>=sizeof(recv_buf))
 103         {
 104                 recv_overrun = 1;
 105                 return;
 106         }
 107
 108         recv_buf[recv_head++] = c;
 109         if(recv_head>=sizeof(recv_buf))
 110                 recv_head = 0;
 111         ++recv_fill;
 112 }
 113
 114 SERIAL_SET_CALLBACK(receive)