arducontrol/interface.c

   1 #include <avr/io.h>
   2 #include "clock.h"
   3 #include "interface.h"
   4 #include "mfx.h"
   5 #include "monitor.h"
   6 #include "motorola.h"
   7 #include "output.h"
   8 #include "serial.h"
   9 #include "s88.h"
  10
  11 static uint8_t cmd_buffer[15];
  12 static uint8_t cmd_length = 0;
  13 static uint8_t cmd_read_pos = 0;
  14 static uint16_t baud_rate = 9600;
  15 static uint16_t baud_change = 0;
  16 static uint32_t baud_changed_at = 0;
  17
  18 static uint8_t dispatch_command(const uint8_t *, uint8_t);
  19 uint8_t interface_command(const uint8_t *, uint8_t);
  20
  21 void interface_init(void)
  22 {
  23         DDRB |= 0x01;
  24         DDRD = (DDRD&0xFC)|0x02;
  25
  26         serial_init(baud_rate);
  27 }
  28
  29 void interface_check(void)
  30 {
  31         if(serial_read_overrun())
  32                 interface_send1(RECEIVE_OVERRUN);
  33
  34         uint8_t count = serial_read_available();
  35         if(count>0)
  36         {
  37                 PORTB |= 0x01;
  38                 if(cmd_length==0)
  39                 {
  40                         uint8_t l = ~serial_read();
  41                         if(l==0)
  42                                 serial_write(0xFF);
  43                         else if(l>=0x10)
  44                                 interface_send1(FRAMING_ERROR);
  45                         else
  46                         {
  47                                 cmd_length = l;
  48                                 --count;
  49                                 cmd_read_pos = 0;
  50                         }
  51                 }
  52
  53                 if(cmd_read_pos<cmd_length)
  54                 {
  55                         if(cmd_read_pos+count>cmd_length)
  56                                 count = cmd_length-cmd_read_pos;
  57                         for(uint8_t i=0; i<count; ++i)
  58                                 cmd_buffer[cmd_read_pos++] = serial_read();
  59
  60                         if(cmd_read_pos>=cmd_length)
  61                         {
  62                                 uint8_t result = dispatch_command(cmd_buffer, cmd_length);
  63                                 interface_send1(result);
  64                                 cmd_length = 0;
  65                         }
  66                 }
  67                 PORTB &= ~0x01;
  68         }
  69
  70         if(baud_change)
  71         {
  72                 if(!baud_changed_at)
  73                 {
  74                         baud_changed_at = clock_get();
  75                         serial_set_baud(baud_change);
  76                 }
  77                 else if(clock_get()-baud_changed_at>3*CLOCK_RATE)
  78                 {
  79                         baud_change = 0;
  80                         serial_set_baud(baud_rate);
  81                         interface_send1(BAUD_CHANGE_FAILED);
  82                 }
  83         }
  84 }
  85
  86 static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
  87 {
  88         uint8_t type = cmd[0]>>4;
  89         if(type==0)
  90         {
  91                 uint8_t subtype = (cmd[0]>>3)&1;
  92                 if(subtype==0)
  93                         return output_command(cmd, length);
  94                 else
  95                         return monitor_command(cmd, length);
  96         }
  97         else if(type==1)
  98                 return motorola_command(cmd, length);
  99         else if(type==2)
 100                 return mfx_command(cmd, length);
 101         else if(type==3)
 102                 return s88_command(cmd, length);
 103         else if(type==7)
 104                 return interface_command(cmd, length);
 105         else
 106                 return INVALID_COMMAND;
 107 }
 108
 109 void interface_send(const uint8_t *cmd, uint8_t length)
 110 {
 111         serial_write(~length);
 112         for(uint8_t i=0; i<length; ++i)
 113                 serial_write(cmd[i]);
 114 }
 115
 116 void interface_send1(uint8_t cmd)
 117 {
 118         serial_write(0xFE);
 119         serial_write(cmd);
 120 }
 121
 122 uint8_t interface_command(const uint8_t *cmd, uint8_t length)
 123 {
 124         if(cmd[0]==SET_BAUD_RATE)
 125         {
 126                 if(length!=3)
 127                         return LENGTH_ERROR;
 128
 129                 uint16_t baud = (cmd[1]<<8)|cmd[2];
 130                 if(baud!=baud_rate)
 131                 {
 132                         if(baud!=baud_change)
 133                         {
 134                                 baud_change = baud;
 135                                 baud_changed_at = 0;
 136                         }
 137                         else
 138                         {
 139                                 baud_rate = baud_change;
 140                                 baud_change = 0;
 141                         }
 142                 }
 143         }
 144         else
 145                 return INVALID_COMMAND;
 146
 147         return COMMAND_OK;
 148 }