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