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