arducontrol/motorola.c

   1 #include "commands.h"
   2 #include "motorola.h"
   3 #include "output.h"
   4
   5 static uint8_t motorola_speed_to_value(uint8_t speed)
   6 {
   7         if(speed>14)
   8                 return 15;
   9         else if(speed)
  10                 return speed+1;
  11         else
  12                 return 0;
  13 }
  14
  15 static void motorola_common_packet(uint8_t addr, uint8_t aux, uint8_t value)
  16 {
  17         uint8_t i;
  18
  19         clear_packet();
  20
  21         packet.bit_duration = 2;
  22         packet.length = 18*8;
  23
  24         for(i=0; i<4; ++i)
  25         {
  26                 uint8_t d = addr%3;
  27                 addr /= 3;
  28
  29                 packet.data[i*2] = (d==0 ? 0x01 : 0x7F);
  30                 packet.data[i*2+1] = (d==1 ? 0x7F : 0x01);
  31         }
  32
  33         packet.data[8] = (aux ? 0x7F : 0x01);
  34         packet.data[9] = packet.data[8];
  35
  36         for(i=0; i<4; ++i)
  37         {
  38                 packet.data[10+i*2] = ((value&1) ? 0x7F : 0x01);
  39                 value >>= 1;
  40         }
  41
  42         packet.repeat_count = 2;
  43         // Duration of three trits
  44         packet.repeat_delay = 96;
  45         packet.final_delay = 224;
  46 }
  47
  48 static void motorola_old_packet(uint8_t addr, uint8_t aux, uint8_t value)
  49 {
  50         uint8_t i;
  51
  52         motorola_common_packet(addr, aux, value);
  53
  54         for(i=0; i<4; ++i)
  55                 packet.data[11+i*2] = packet.data[10+i*2];
  56 }
  57
  58 void motorola_locomotive_speed_packet(uint8_t addr, uint8_t aux, uint8_t speed)
  59 {
  60         motorola_old_packet(addr, aux, motorola_speed_to_value(speed));
  61
  62         packet.ready = 1;
  63 }
  64
  65 void motorola_locomotive_reverse_packet(uint8_t addr, uint8_t aux)
  66 {
  67         motorola_old_packet(addr, aux, 1);
  68
  69         packet.ready = 1;
  70 }
  71
  72 void motorola_locomotive_speed_direction_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t dir)
  73 {
  74         motorola_common_packet(addr, aux, motorola_speed_to_value(speed));
  75
  76         packet.data[11] = (dir ? 0x01 : 0x7F);
  77         packet.data[13] = (dir ? 0x7F : 0x01);
  78         packet.data[15] = (dir ? 0x01 : 0x7F);
  79         packet.data[17] = 0x80-packet.data[16];
  80
  81         packet.ready = 1;
  82 }
  83
  84 void motorola_locomotive_speed_function_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t func, uint8_t state)
  85 {
  86         uint8_t i;
  87         uint8_t value;
  88
  89         value = motorola_speed_to_value(speed);
  90         motorola_common_packet(addr, aux, value);
  91
  92         /*
  93         001 -> 011
  94         010 -> 100
  95         011 -> 110
  96         100 -> 111
  97         */
  98         func += 2;
  99         if(func>=5)
 100                 ++func;
 101         if(state)
 102                 func |= 8;
 103         if(func==value)
 104                 func = ((value&8) ? 2 : 5) | (func&8);
 105
 106         for(i=0; i<4; ++i)
 107         {
 108                 packet.data[11+i*2] = ((func&1) ? 0x7F : 0x01);
 109                 func >>= 1;
 110         }
 111
 112         packet.ready = 1;
 113 }
 114
 115 void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state)
 116 {
 117         uint8_t value = output;
 118         if(state)
 119                 value |= 8;
 120
 121         motorola_old_packet(addr, 0, value);
 122         packet.repeat_delay >>= 1;
 123         packet.bit_duration = 1;
 124
 125         packet.ready = 1;
 126 }
 127
 128 uint8_t motorola_command(const uint8_t *cmd_buf, uint8_t cmd_length)
 129 {
 130         if(cmd_buf[0]==MOTOROLA_SPEED || cmd_buf[0]==MOTOROLA_SPEED_DIRECTION || cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
 131         {
 132                 if(cmd_length!=4)
 133                         return LENGTH_ERROR;
 134
 135                 uint8_t addr = cmd_buf[1];
 136                 if(addr>80)
 137                         return INVALID_VALUE;
 138
 139                 if(cmd_buf[2]&0x0C)
 140                         return INVALID_VALUE;
 141                 uint8_t aux = cmd_buf[2]&0x01;
 142
 143                 uint8_t func = (cmd_buf[2]&0xF0)>>4;
 144                 if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
 145                 {
 146                         if(func<1 || func>4)
 147                                 return INVALID_VALUE;
 148                 }
 149                 else if(cmd_buf[2]&0xFE)
 150                         return INVALID_VALUE;
 151                 uint8_t state = cmd_buf[2]&0x02;
 152
 153                 uint8_t speed = cmd_buf[3]&0x7F;
 154                 if(speed>14)
 155                         return INVALID_VALUE;
 156
 157                 uint8_t dir = !(cmd_buf[3]&0x80);
 158
 159                 while(packet.ready && !packet.done) ;
 160
 161                 if(cmd_buf[0]==MOTOROLA_SPEED)
 162                         motorola_locomotive_speed_packet(addr, aux, speed);
 163                 else if(cmd_buf[0]==MOTOROLA_SPEED_DIRECTION)
 164                         motorola_locomotive_speed_direction_packet(addr, aux, speed, dir);
 165                 else if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION)
 166                         motorola_locomotive_speed_function_packet(addr, aux, speed, func, state);
 167         }
 168         else if(cmd_buf[0]==MOTOROLA_REVERSE)
 169         {
 170                 if(cmd_length!=3)
 171                         return LENGTH_ERROR;
 172
 173                 uint8_t addr = cmd_buf[1];
 174                 if(addr>80)
 175                         return INVALID_VALUE;
 176
 177                 if(cmd_buf[2]&0xFE)
 178                         return INVALID_VALUE;
 179                 uint8_t aux = cmd_buf[2]&0x01;
 180
 181                 while(packet.ready && !packet.done) ;
 182
 183                 motorola_locomotive_reverse_packet(addr, aux);
 184         }
 185         else if(cmd_buf[0]==MOTOROLA_SOLENOID)
 186         {
 187                 if(cmd_length!=3)
 188                         return LENGTH_ERROR;
 189
 190                 uint8_t addr = cmd_buf[1];
 191                 if(addr>80)
 192                         return INVALID_VALUE;
 193
 194                 if(cmd_buf[2]&0x8E)
 195                         return INVALID_VALUE;
 196                 uint8_t output = (cmd_buf[2]&0x70)>>4;
 197                 uint8_t state = cmd_buf[2]&1;
 198
 199                 while(packet.ready && !packet.done) ;
 200
 201                 motorola_solenoid_packet(addr, output, state);
 202         }
 203         else
 204                 return INVALID_COMMAND;
 205
 206         return COMMAND_OK;
 207 }