arducontrol/interface.c

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