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