arducontrol/interface.c

   1 #include "interface.h"
   2 #include "monitor.h"
   3 #include "motorola.h"
   4 #include "output.h"
   5 #include "serial.h"
   6 #include "s88.h"
   7
   8 volatile uint8_t recv_buf[32];
   9 uint8_t recv_head = 0;
  10 uint8_t recv_tail = 0;
  11 volatile uint8_t recv_fill = 0;
  12 volatile uint8_t recv_overrun = 0;
  13
  14 void process_commands(void);
  15 uint8_t dispatch_command(const uint8_t *, uint8_t);
  16
  17 void interface_init(void)
  18 {
  19         DDRD = (DDRD&0xFC)|0x02;
  20
  21         serial_init(9600);
  22 }
  23
  24 void interface_check(void)
  25 {
  26         if(recv_overrun)
  27         {
  28                 interface_send1(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                         interface_send1(FRAMING_ERROR);
  59                 }
  60
  61                 recv_tail += length+1;
  62                 if(recv_tail>=sizeof(recv_buf))
  63                         recv_tail -= sizeof(recv_buf);
  64                 recv_fill -= length+1;
  65
  66                 if(length>0)
  67                 {
  68                         uint8_t result = dispatch_command(cmd, length);
  69                         interface_send1(result);
  70                 }
  71         }
  72 }
  73
  74 uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  75 {
  76         uint8_t type = cmd[0]>>4;
  77         if(type==0)
  78         {
  79                 uint8_t subtype = (cmd[0]>>3)&1;
  80                 if(subtype==0)
  81                         return output_command(cmd, length);
  82                 else
  83                         return monitor_command(cmd, length);
  84         }
  85         else if(type==1)
  86                 return motorola_command(cmd, length);
  87         else if(type==3)
  88                 return s88_command(cmd, length);
  89         else
  90                 return INVALID_COMMAND;
  91 }
  92
  93 void interface_send(const uint8_t *cmd, uint8_t length)
  94 {
  95         uint8_t i;
  96
  97         serial_write(~length);
  98         for(i=0; i<length; ++i)
  99                 serial_write(cmd[i]);
 100 }
 101
 102 void interface_send1(uint8_t cmd)
 103 {
 104         serial_write(0xFE);
 105         serial_write(cmd);
 106 }
 107
 108 static inline void receive(uint8_t c)
 109 {
 110         if(recv_fill>=sizeof(recv_buf))
 111         {
 112                 recv_overrun = 1;
 113                 return;
 114         }
 115
 116         recv_buf[recv_head++] = c;
 117         if(recv_head>=sizeof(recv_buf))
 118                 recv_head = 0;
 119         ++recv_fill;
 120 }
 121
 122 SERIAL_SET_CALLBACK(receive)