arducontrol/interface.c

   1 #include <avr/io.h>
   2 #include "interface.h"
   3 #include "monitor.h"
   4 #include "motorola.h"
   5 #include "output.h"
   6 #include "serial.h"
   7 #include "s88.h"
   8
   9 static uint8_t cmd_buffer[15];
  10 static uint8_t cmd_length = 0;
  11 static uint8_t cmd_read_pos = 0;
  12
  13 static uint8_t dispatch_command(const uint8_t *, uint8_t);
  14
  15 void interface_init(void)
  16 {
  17         DDRD = (DDRD&0xFC)|0x02;
  18
  19         serial_init(9600);
  20 }
  21
  22 void interface_check(void)
  23 {
  24         uint8_t count;
  25         if(serial_read_overrun())
  26                 interface_send1(RECEIVE_OVERRUN);
  27
  28         count = serial_read_available();
  29         if(count>0)
  30         {
  31                 if(cmd_length==0)
  32                 {
  33                         uint8_t l = ~serial_read();
  34                         if(l==0)
  35                                 serial_write(0xFF);
  36                         else if(l>=0x10)
  37                                 interface_send1(FRAMING_ERROR);
  38                         else
  39                         {
  40                                 cmd_length = l;
  41                                 --count;
  42                                 cmd_read_pos = 0;
  43                         }
  44                 }
  45
  46                 if(cmd_read_pos<cmd_length)
  47                 {
  48                         uint8_t i;
  49                         if(cmd_read_pos+count>cmd_length)
  50                                 count = cmd_length-cmd_read_pos;
  51                         for(i=0; i<count; ++i)
  52                                 cmd_buffer[cmd_read_pos++] = serial_read();
  53
  54                         if(cmd_read_pos>=cmd_length)
  55                         {
  56                                 uint8_t result = dispatch_command(cmd_buffer, cmd_length);
  57                                 interface_send1(result);
  58                                 cmd_length = 0;
  59                         }
  60                 }
  61         }
  62 }
  63
  64 static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  65 {
  66         uint8_t type = cmd[0]>>4;
  67         if(type==0)
  68         {
  69                 uint8_t subtype = (cmd[0]>>3)&1;
  70                 if(subtype==0)
  71                         return output_command(cmd, length);
  72                 else
  73                         return monitor_command(cmd, length);
  74         }
  75         else if(type==1)
  76                 return motorola_command(cmd, length);
  77         else if(type==3)
  78                 return s88_command(cmd, length);
  79         else
  80                 return INVALID_COMMAND;
  81 }
  82
  83 void interface_send(const uint8_t *cmd, uint8_t length)
  84 {
  85         uint8_t i;
  86
  87         serial_write(~length);
  88         for(i=0; i<length; ++i)
  89                 serial_write(cmd[i]);
  90 }
  91
  92 void interface_send1(uint8_t cmd)
  93 {
  94         serial_write(0xFE);
  95         serial_write(cmd);
  96 }