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 uint8_t dispatch_command(const uint8_t *, uint8_t);
  10
  11 void interface_init(void)
  12 {
  13         DDRD = (DDRD&0xFC)|0x02;
  14
  15         serial_init(9600);
  16 }
  17
  18 void interface_check(void)
  19 {
  20         if(serial_read_overrun())
  21                 interface_send1(RECEIVE_OVERRUN);
  22
  23         while(serial_read_available())
  24         {
  25                 uint8_t length = ~serial_read();
  26
  27                 if(length==0)
  28                         serial_write(0xFF);
  29                 else if(length>=0x10)
  30                         interface_send1(FRAMING_ERROR);
  31                 else
  32                 {
  33                         uint8_t cmd[15];
  34                         uint8_t i;
  35
  36                         for(i=0; i<length; ++i)
  37                                 cmd[i] = serial_read();
  38
  39                         uint8_t result = dispatch_command(cmd, length);
  40                         interface_send1(result);
  41                 }
  42         }
  43 }
  44
  45 uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  46 {
  47         uint8_t type = cmd[0]>>4;
  48         if(type==0)
  49         {
  50                 uint8_t subtype = (cmd[0]>>3)&1;
  51                 if(subtype==0)
  52                         return output_command(cmd, length);
  53                 else
  54                         return monitor_command(cmd, length);
  55         }
  56         else if(type==1)
  57                 return motorola_command(cmd, length);
  58         else if(type==3)
  59                 return s88_command(cmd, length);
  60         else
  61                 return INVALID_COMMAND;
  62 }
  63
  64 void interface_send(const uint8_t *cmd, uint8_t length)
  65 {
  66         uint8_t i;
  67
  68         serial_write(~length);
  69         for(i=0; i<length; ++i)
  70                 serial_write(cmd[i]);
  71 }
  72
  73 void interface_send1(uint8_t cmd)
  74 {
  75         serial_write(0xFE);
  76         serial_write(cmd);
  77 }