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         DDRB |= 0x01;
  18         DDRD = (DDRD&0xFC)|0x02;
  19
  20         serial_init(9600);
  21 }
  22
  23 void interface_check(void)
  24 {
  25         if(serial_read_overrun())
  26                 interface_send1(RECEIVE_OVERRUN);
  27
  28         uint8_t count = serial_read_available();
  29         if(count>0)
  30         {
  31                 PORTB |= 0x01;
  32                 if(cmd_length==0)
  33                 {
  34                         uint8_t l = ~serial_read();
  35                         if(l==0)
  36                                 serial_write(0xFF);
  37                         else if(l>=0x10)
  38                                 interface_send1(FRAMING_ERROR);
  39                         else
  40                         {
  41                                 cmd_length = l;
  42                                 --count;
  43                                 cmd_read_pos = 0;
  44                         }
  45                 }
  46
  47                 if(cmd_read_pos<cmd_length)
  48                 {
  49                         if(cmd_read_pos+count>cmd_length)
  50                                 count = cmd_length-cmd_read_pos;
  51                         for(uint8_t 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                 PORTB &= ~0x01;
  62         }
  63 }
  64
  65 static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  66 {
  67         uint8_t type = cmd[0]>>4;
  68         if(type==0)
  69         {
  70                 uint8_t subtype = (cmd[0]>>3)&1;
  71                 if(subtype==0)
  72                         return output_command(cmd, length);
  73                 else
  74                         return monitor_command(cmd, length);
  75         }
  76         else if(type==1)
  77                 return motorola_command(cmd, length);
  78         else if(type==3)
  79                 return s88_command(cmd, length);
  80         else
  81                 return INVALID_COMMAND;
  82 }
  83
  84 void interface_send(const uint8_t *cmd, uint8_t length)
  85 {
  86         serial_write(~length);
  87         for(uint8_t i=0; i<length; ++i)
  88                 serial_write(cmd[i]);
  89 }
  90
  91 void interface_send1(uint8_t cmd)
  92 {
  93         serial_write(0xFE);
  94         serial_write(cmd);
  95 }