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
  13 void process_commands(void);
  14 uint8_t dispatch_command(const uint8_t *, uint8_t);
  15
  16 void interface_init(void)
  17 {
  18         DDRD = (DDRD&0xFC)|0x02;
  19
  20         serial_init(9600);
  21 }
  22
  23 void interface_check(void)
  24 {
  25         if(recv_overrun)
  26         {
  27                 serial_write(0xFE);
  28                 serial_write(RECEIVE_OVERRUN);
  29                 recv_overrun = 0;
  30         }
  31         if(recv_fill>0)
  32                 process_commands();
  33 }
  34
  35 void process_commands(void)
  36 {
  37         while(recv_fill>0)
  38         {
  39                 uint8_t cmd[15];
  40                 uint8_t length = ~recv_buf[recv_tail];
  41
  42                 if(length<0x10)
  43                 {
  44                         if(recv_fill<=length)
  45                                 break;
  46
  47                         uint8_t i, j;
  48                         for(i=0, j=recv_tail+1; i<length; ++i, ++j)
  49                         {
  50                                 if(j>=sizeof(recv_buf))
  51                                         j = 0;
  52                                 cmd[i] = recv_buf[j];
  53                         }
  54                 }
  55                 else
  56                 {
  57                         length = 0;
  58                         serial_write(0xFE);
  59                         serial_write(FRAMING_ERROR);
  60                 }
  61
  62                 recv_tail += length+1;
  63                 if(recv_tail>=sizeof(recv_buf))
  64                         recv_tail -= sizeof(recv_buf);
  65                 recv_fill -= length+1;
  66
  67                 if(length>0)
  68                 {
  69                         uint8_t result = dispatch_command(cmd, length);
  70                         serial_write(0xFE);
  71                         serial_write(result);
  72                 }
  73         }
  74 }
  75
  76 uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  77 {
  78         uint8_t type = cmd[0]>>4;
  79         if(type==0)
  80         {
  81                 uint8_t subtype = (cmd[0]>>3)&1;
  82                 if(subtype==0)
  83                         return output_command(cmd, length);
  84                 else
  85                         return monitor_command(cmd, length);
  86         }
  87         else if(type==1)
  88                 return motorola_command(cmd, length);
  89         else
  90                 return INVALID_COMMAND;
  91 }
  92
  93 static inline void receive(uint8_t c)
  94 {
  95         if(recv_fill>=sizeof(recv_buf))
  96         {
  97                 recv_overrun = 1;
  98                 return;
  99         }
 100
 101         recv_buf[recv_head++] = c;
 102         if(recv_head>=sizeof(recv_buf))
 103                 recv_head = 0;
 104         ++recv_fill;
 105 }
 106
 107 SERIAL_SET_CALLBACK(receive)