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         uint8_t count;
  26         if(serial_read_overrun())
  27                 interface_send1(RECEIVE_OVERRUN);
  28
  29         count = serial_read_available();
  30         if(count>0)
  31         {
  32                 PORTB |= 0x01;
  33                 if(cmd_length==0)
  34                 {
  35                         uint8_t l = ~serial_read();
  36                         if(l==0)
  37                                 serial_write(0xFF);
  38                         else if(l>=0x10)
  39                                 interface_send1(FRAMING_ERROR);
  40                         else
  41                         {
  42                                 cmd_length = l;
  43                                 --count;
  44                                 cmd_read_pos = 0;
  45                         }
  46                 }
  47
  48                 if(cmd_read_pos<cmd_length)
  49                 {
  50                         uint8_t i;
  51                         if(cmd_read_pos+count>cmd_length)
  52                                 count = cmd_length-cmd_read_pos;
  53                         for(i=0; i<count; ++i)
  54                                 cmd_buffer[cmd_read_pos++] = serial_read();
  55
  56                         if(cmd_read_pos>=cmd_length)
  57                         {
  58                                 uint8_t result = dispatch_command(cmd_buffer, cmd_length);
  59                                 interface_send1(result);
  60                                 cmd_length = 0;
  61                         }
  62                 }
  63                 PORTB &= ~0x01;
  64         }
  65 }
  66
  67 static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  68 {
  69         uint8_t type = cmd[0]>>4;
  70         if(type==0)
  71         {
  72                 uint8_t subtype = (cmd[0]>>3)&1;
  73                 if(subtype==0)
  74                         return output_command(cmd, length);
  75                 else
  76                         return monitor_command(cmd, length);
  77         }
  78         else if(type==1)
  79                 return motorola_command(cmd, length);
  80         else if(type==3)
  81                 return s88_command(cmd, length);
  82         else
  83                 return INVALID_COMMAND;
  84 }
  85
  86 void interface_send(const uint8_t *cmd, uint8_t length)
  87 {
  88         uint8_t i;
  89
  90         serial_write(~length);
  91         for(i=0; i<length; ++i)
  92                 serial_write(cmd[i]);
  93 }
  94
  95 void interface_send1(uint8_t cmd)
  96 {
  97         serial_write(0xFE);
  98         serial_write(cmd);
  99 }