arducontrol/mfx.c

   1 #include "interface.h"
   2 #include "mfx.h"
   3 #include "monitor.h"
   4 #include "output.h"
   5
   6 typedef struct
   7 {
   8         uint8_t ones_count;
   9         uint8_t crc8;
  10 } MfxEncodingState;
  11
  12 static uint32_t station_id = 0x12345678;
  13 static uint8_t feedback_threshold = 30;
  14
  15 void mfx_set_station_id(uint32_t id)
  16 {
  17         station_id = id;
  18 }
  19
  20 static OutputPacket *mfx_create_packet(MfxEncodingState *state)
  21 {
  22         OutputPacket *packet = output_create_packet();
  23         packet->bit_duration = 4;
  24         packet->repeat_count = 1;
  25         packet->final_delay = 128;
  26         packet->length = 10;
  27         packet->data[0] = 0x9B;
  28         packet->data[1] = 0;
  29         state->ones_count = 0;
  30         state->crc8 = 0x7A;
  31         return packet;
  32 }
  33
  34 static inline void mfx_update_crc8(MfxEncodingState *state, uint8_t bits, uint8_t length)
  35 {
  36         for(uint8_t i=length; i--; )
  37         {
  38                 uint8_t bit = (bits>>i)&1;
  39                 if(bit^(state->crc8>>7))
  40                         state->crc8 = (state->crc8<<1)^7;
  41                 else
  42                         state->crc8 <<= 1;
  43         }
  44 }
  45
  46 static inline uint8_t mfx_get_output_level(const OutputPacket *packet)
  47 {
  48         uint8_t b = packet->length-1;
  49         return (packet->data[b>>3]>>(b&7))&1;
  50 }
  51
  52 static inline void mfx_ensure_low_level(OutputPacket *packet)
  53 {
  54         if(mfx_get_output_level(packet))
  55         {
  56                 // Motorola decoders require a low logic level when idle.
  57                 uint8_t out_bit = packet->length;
  58                 if((out_bit&7)==0)
  59                         packet->data[out_bit>>3] = 0;
  60                 ++packet->length;
  61         }
  62 }
  63
  64 static inline void mfx_encode_flag_pairs(OutputPacket *packet, uint8_t count)
  65 {
  66         static const uint8_t flag_bits[5] = { 0x9B, 0x6C, 0xB2, 0xC9, 0x26 };
  67
  68         uint8_t invert = mfx_get_output_level(packet)*0xFF;
  69         uint8_t shift = packet->length&7;
  70         uint8_t total_bits = count*10;
  71         uint8_t tail_bits = (total_bits+shift)&7;
  72         uint8_t out_byte = packet->length>>3;
  73         if(shift)
  74         {
  75                 uint8_t head_bits = 8-shift;
  76                 packet->data[out_byte++] |= (flag_bits[0]^invert)<<shift;
  77
  78                 uint8_t i = 1;
  79                 uint8_t bytes = (total_bits-head_bits)>>3;
  80                 uint8_t carry = flag_bits[0]>>head_bits;
  81                 while(bytes--)
  82                 {
  83                         packet->data[out_byte++] = (carry|(flag_bits[i]<<shift))^invert;
  84                         carry = flag_bits[i]>>head_bits;
  85
  86                         if(++i>=sizeof(flag_bits))
  87                                 i -= sizeof(flag_bits);
  88                 }
  89
  90                 if(tail_bits>0)
  91                         packet->data[out_byte] = ((carry|(flag_bits[i]<<shift))^invert)&((1<<tail_bits)-1);
  92         }
  93         else
  94         {
  95                 uint8_t i = 0;
  96                 uint8_t bytes = total_bits>>3;
  97                 while(bytes--)
  98                 {
  99                         packet->data[out_byte++] = flag_bits[i]^invert;
 100
 101                         if(++i>=sizeof(flag_bits))
 102                                 i -= sizeof(flag_bits);
 103                 }
 104
 105                 uint8_t tail_bits = (total_bits+shift)&7;
 106                 if(tail_bits>0)
 107                         packet->data[out_byte] = (flag_bits[i]^invert)&((1<<tail_bits)-1);
 108         }
 109
 110         packet->length += total_bits;
 111 }
 112
 113 static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length)
 114 {
 115         uint8_t out_bit = packet->length;
 116         uint8_t out_level = mfx_get_output_level(packet);
 117         uint8_t ones_count = state->ones_count;
 118
 119         for(uint8_t i=length; i--; )
 120         {
 121                 uint8_t bit = (bits>>i)&1;
 122
 123                 if(bit)
 124                         ++ones_count;
 125                 else
 126                         ones_count = 0;
 127
 128                 if(out_level==bit)
 129                         bit ^= 3;
 130
 131                 // out_bit is always even at this point
 132                 if((out_bit&7)==0)
 133                         packet->data[out_bit>>3] = bit;
 134                 else
 135                         packet->data[out_bit>>3] |= bit<<(out_bit&7);
 136                 out_bit += 2;
 137                 out_level = bit>>1;
 138
 139                 if(ones_count>=8)
 140                 {
 141                         bits &= (1<<i)-1;
 142                         ++i;
 143                 }
 144         }
 145
 146         state->ones_count = ones_count;
 147         packet->length = out_bit;
 148 }
 149
 150 static inline void mfx_encode_bits8(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length)
 151 {
 152         mfx_encode_bits8_raw(packet, state, bits, length);
 153         mfx_update_crc8(state, bits, length);
 154 }
 155
 156 static inline void mfx_encode_bits16(OutputPacket *packet, MfxEncodingState *state, uint16_t bits, uint8_t length)
 157 {
 158         mfx_encode_bits8(packet, state, bits>>8, length-8);
 159         mfx_encode_bits8(packet, state, bits, 8);
 160 }
 161
 162 static inline void mfx_encode_bits32(OutputPacket *packet, MfxEncodingState *state, uint32_t bits, uint8_t length)
 163 {
 164         if(length>24)
 165         {
 166                 mfx_encode_bits8(packet, state, bits>>24, length-24);
 167                 mfx_encode_bits8(packet, state, bits>>16, 8);
 168         }
 169         else
 170                 mfx_encode_bits8(packet, state, bits>>16, length-16);
 171         mfx_encode_bits8(packet, state, bits>>8, 8);
 172         mfx_encode_bits8(packet, state, bits, 8);
 173 }
 174
 175 static inline void mfx_address_field(OutputPacket *packet, MfxEncodingState *state, uint16_t address)
 176 {
 177         if(address<0x80)
 178                 mfx_encode_bits16(packet, state, 0x100|address, 9);
 179         else if(address<0x200)
 180                 mfx_encode_bits16(packet, state, 0xC00|address, 12);
 181         else if(address<0x800)
 182                 mfx_encode_bits16(packet, state, 0x7000|address, 15);
 183         else
 184         {
 185                 mfx_encode_bits8(packet, state, 0xF, 4);
 186                 mfx_encode_bits16(packet, state, address, 14);
 187         }
 188 }
 189
 190 void mfx_speed_field(OutputPacket *packet, MfxEncodingState *state, uint8_t speed_dir)
 191 {
 192         if(speed_dir&0x0F)
 193         {
 194                 // Field type: 001
 195                 mfx_encode_bits8(packet, state, 0x1, 3);
 196                 mfx_encode_bits8(packet, state, speed_dir, 8);
 197         }
 198         else
 199                 // Field type: 000
 200                 mfx_encode_bits8(packet, state, speed_dir>>4, 7);
 201 }
 202
 203 static void mfx_finish_packet(OutputPacket *packet, MfxEncodingState *state)
 204 {
 205         mfx_encode_bits8_raw(packet, state, state->crc8, 8);
 206         mfx_encode_flag_pairs(packet, 2);
 207         mfx_ensure_low_level(packet);
 208 }
 209
 210 static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *state)
 211 {
 212         mfx_encode_bits8_raw(packet, state, state->crc8, 8);
 213         mfx_encode_flag_pairs(packet, 11);
 214         mfx_encode_bits8_raw(packet, state, 0x3, 4);
 215         packet->final_delay = 0;
 216         uint8_t fill = (1-mfx_get_output_level(packet))*0xFF;
 217
 218         for(uint8_t i=0; i<2; ++i)
 219         {
 220                 packet = output_create_chained_packet();
 221                 packet->bit_duration = 4;
 222                 packet->repeat_count = 1;
 223                 packet->final_delay = 0;
 224                 packet->trigger_position = 112;
 225                 packet->trigger_value = i+1;
 226
 227                 for(uint8_t j=0; j<16; ++j)
 228                         packet->data[j] = fill;
 229                 packet->length = 128;
 230                 mfx_encode_flag_pairs(packet, 1+i);
 231                 fill ^= 0xFF;
 232         }
 233
 234         mfx_ensure_low_level(packet);
 235         packet->final_delay = 128;
 236 }
 237
 238 static void mfx_receive_feedback(uint8_t type)
 239 {
 240         /* The decoder should activate a 52.6 kHz carrier to indicate positive
 241         acknowledgement, but so far I've been unable to build a circuit that detects
 242         it.  The increased current draw can nevertheless be measured. */
 243         uint8_t state = 0;
 244         uint16_t current[3];
 245         current[0] = monitor_track_current();
 246         while(state<2)
 247         {
 248                 uint8_t trig = output_get_trigger();
 249                 if(trig!=state)
 250                 {
 251                         current[trig] = monitor_track_current();
 252                         state = trig;
 253                 }
 254                 monitor_check();
 255         }
 256
 257         uint8_t reply[2];
 258         reply[0] = type;
 259         if(current[1]>feedback_threshold)
 260         {
 261                 current[1] -= feedback_threshold;
 262                 reply[1] = (current[1]>current[0] && current[1]>current[2]);
 263         }
 264         else
 265                 reply[1] = 0;
 266         interface_send(reply, 2);
 267 }
 268
 269 void mfx_announce_packet(uint16_t serial)
 270 {
 271         MfxEncodingState state;
 272         OutputPacket *packet = mfx_create_packet(&state);
 273         mfx_address_field(packet, &state, 0);
 274         // Packet type: 111101
 275         mfx_encode_bits8(packet, &state, 0x3D, 6);
 276         mfx_encode_bits32(packet, &state, station_id, 32);
 277         mfx_encode_bits16(packet, &state, serial, 16);
 278         mfx_finish_packet(packet, &state);
 279         output_send_packet();
 280 }
 281
 282 void mfx_search_packet(uint32_t mask_bits, uint8_t mask_size)
 283 {
 284         MfxEncodingState state;
 285         OutputPacket *packet = mfx_create_packet(&state);
 286         mfx_address_field(packet, &state, 0);
 287         // Packet type: 111010
 288         mfx_encode_bits8(packet, &state, 0x3A, 6);
 289         mfx_encode_bits8(packet, &state, mask_size, 6);
 290         mfx_encode_bits32(packet, &state, mask_bits, 32);
 291         mfx_finish_packet_feedback(packet, &state);
 292         output_send_packet();
 293 }
 294
 295 void mfx_assign_address_packet(uint16_t addr, uint32_t id)
 296 {
 297         MfxEncodingState state;
 298         OutputPacket *packet = mfx_create_packet(&state);
 299         mfx_address_field(packet, &state, 0);
 300         // Packet type: 111011
 301         mfx_encode_bits8(packet, &state, 0x3B, 6);
 302         mfx_encode_bits16(packet, &state, addr, 14);
 303         mfx_encode_bits32(packet, &state, id, 32);
 304         mfx_finish_packet(packet, &state);
 305         output_send_packet();
 306 }
 307
 308 void mfx_ping_packet(uint16_t addr, uint32_t id)
 309 {
 310         MfxEncodingState state;
 311         OutputPacket *packet = mfx_create_packet(&state);
 312         mfx_address_field(packet, &state, addr);
 313         // Packet type: 111100
 314         mfx_encode_bits8(packet, &state, 0x3C, 6);
 315         mfx_encode_bits32(packet, &state, id, 32);
 316         mfx_finish_packet_feedback(packet, &state);
 317         output_send_packet();
 318 }
 319
 320 void mfx_speed_packet(uint16_t addr, uint8_t speed_dir)
 321 {
 322         MfxEncodingState state;
 323         OutputPacket *packet = mfx_create_packet(&state);
 324         mfx_address_field(packet, &state, addr);
 325         mfx_speed_field(packet, &state, speed_dir);
 326         mfx_finish_packet(packet, &state);
 327         output_send_packet();
 328 }
 329
 330 void mfx_speed_funcs8_packet(uint16_t addr, uint8_t speed_dir, uint8_t funcs)
 331 {
 332         MfxEncodingState state;
 333         OutputPacket *packet = mfx_create_packet(&state);
 334         mfx_address_field(packet, &state, addr);
 335         mfx_speed_field(packet, &state, speed_dir);
 336         if(funcs&0xF0)
 337         {
 338                 // Field type: 0110
 339                 mfx_encode_bits8(packet, &state, 0x6, 4);
 340                 mfx_encode_bits8(packet, &state, funcs, 8);
 341         }
 342         else
 343                 // Field type: 010
 344                 mfx_encode_bits8(packet, &state, 0x20|funcs, 7);
 345         mfx_finish_packet(packet, &state);
 346         output_send_packet();
 347 }
 348
 349 void mfx_speed_funcs16_packet(uint16_t addr, uint8_t speed_dir, uint16_t funcs)
 350 {
 351         MfxEncodingState state;
 352         OutputPacket *packet = mfx_create_packet(&state);
 353         mfx_address_field(packet, &state, addr);
 354         mfx_speed_field(packet, &state, speed_dir);
 355         // Field type: 0111
 356         mfx_encode_bits8(packet, &state, 0x7, 4);
 357         mfx_encode_bits16(packet, &state, funcs, 16);
 358         mfx_finish_packet(packet, &state);
 359         output_send_packet();
 360 }
 361
 362 uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
 363 {
 364         if(cmd[0]==MFX_SET_STATION_ID)
 365         {
 366                 if(length!=5)
 367                         return LENGTH_ERROR;
 368
 369                 uint32_t id = (cmd[1]<<8)|cmd[2];
 370                 id <<= 16;
 371                 id |= (uint16_t)(cmd[3]<<8)|cmd[4];
 372                 mfx_set_station_id(id);
 373         }
 374         else if(cmd[0]==MFX_ANNOUNCE)
 375         {
 376                 if(length!=3)
 377                         return LENGTH_ERROR;
 378
 379                 mfx_announce_packet((cmd[1]<<8)|cmd[2]);
 380         }
 381         else if(cmd[0]==MFX_SEARCH)
 382         {
 383                 if(length!=6)
 384                         return LENGTH_ERROR;
 385
 386                 uint8_t mask_size = cmd[5];
 387                 if(mask_size>0x20)
 388                         return INVALID_VALUE;
 389
 390                 uint32_t mask_bits = (cmd[1]<<8)|cmd[2];
 391                 mask_bits <<= 16;
 392                 mask_bits |= (uint16_t)(cmd[3]<<8)|cmd[4];
 393                 mfx_search_packet(mask_bits, mask_size);
 394                 mfx_receive_feedback(MFX_SEARCH_FEEDBACK);
 395         }
 396         else if(cmd[0]==MFX_ASSIGN_ADDRESS || cmd[0]==MFX_PING)
 397         {
 398                 if(length!=7)
 399                         return LENGTH_ERROR;
 400
 401                 uint16_t addr = (cmd[1]<<8)|cmd[2];
 402                 if(addr==0 || addr>0x3FFF)
 403                         return INVALID_VALUE;
 404
 405                 uint32_t id = (cmd[3]<<8)|cmd[4];
 406                 id <<= 16;
 407                 id |= (uint16_t)(cmd[5]<<8)|cmd[6];
 408                 if(cmd[0]==MFX_ASSIGN_ADDRESS)
 409                         mfx_assign_address_packet(addr, id);
 410                 else
 411                 {
 412                         mfx_ping_packet(addr, id);
 413                         mfx_receive_feedback(MFX_PING_FEEDBACK);
 414                 }
 415         }
 416         else if(cmd[0]==MFX_SPEED || cmd[0]==MFX_SPEED_FUNCS8 || cmd[0]==MFX_SPEED_FUNCS16)
 417         {
 418                 if(cmd[0]==MFX_SPEED && length!=4)
 419                         return LENGTH_ERROR;
 420                 else if(cmd[0]==MFX_SPEED_FUNCS8 && length!=5)
 421                         return LENGTH_ERROR;
 422                 else if(cmd[0]==MFX_SPEED_FUNCS16 && length!=6)
 423                         return LENGTH_ERROR;
 424
 425                 uint16_t addr = (cmd[1]<<8)|cmd[2];
 426                 if(addr==0 || addr>0x3FFF)
 427                         return INVALID_VALUE;
 428
 429                 uint8_t speed_dir = cmd[3];
 430                 if((speed_dir&0x7F)==0x7F)
 431                         return INVALID_VALUE;
 432                 // Skip emergency stop
 433                 if(speed_dir>0)
 434                         ++speed_dir;
 435
 436                 if(cmd[0]==MFX_SPEED)
 437                         mfx_speed_packet(addr, speed_dir);
 438                 else if(cmd[0]==MFX_SPEED_FUNCS8)
 439                         mfx_speed_funcs8_packet(addr, speed_dir, cmd[4]);
 440                 else if(cmd[0]==MFX_SPEED_FUNCS16)
 441                         mfx_speed_funcs8_packet(addr, speed_dir, (cmd[4]<<8)|cmd[5]);
 442         }
 443         else
 444                 return INVALID_COMMAND;
 445
 446         return COMMAND_OK;
 447 }