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 = 0;
  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
 118         for(uint8_t i=length; i--; )
 119         {
 120                 uint8_t bit = (bits>>i)&1;
 121
 122                 if(state->ones_count>=8)
 123                 {
 124                         ++i;
 125                         bit = 0;
 126                         state->ones_count = 0;
 127                 }
 128                 else if(bit)
 129                         ++state->ones_count;
 130                 else
 131                         state->ones_count = 0;
 132
 133                 if(out_level==bit)
 134                         bit ^= 3;
 135
 136                 // out_bit is always even at this point
 137                 if((out_bit&7)==0)
 138                         packet->data[out_bit>>3] = bit;
 139                 else
 140                         packet->data[out_bit>>3] |= bit<<(out_bit&7);
 141                 out_bit += 2;
 142                 out_level = bit>>1;
 143         }
 144
 145         packet->length = out_bit;
 146 }
 147
 148 static inline void mfx_encode_bits8(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length)
 149 {
 150         mfx_encode_bits8_raw(packet, state, bits, length);
 151         mfx_update_crc8(state, bits, length);
 152 }
 153
 154 static inline void mfx_encode_bits16(OutputPacket *packet, MfxEncodingState *state, uint16_t bits, uint8_t length)
 155 {
 156         mfx_encode_bits8(packet, state, bits>>8, length-8);
 157         mfx_encode_bits8(packet, state, bits, 8);
 158 }
 159
 160 static inline void mfx_encode_bits32(OutputPacket *packet, MfxEncodingState *state, uint32_t bits, uint8_t length)
 161 {
 162         if(length>24)
 163         {
 164                 mfx_encode_bits8(packet, state, bits>>24, length-24);
 165                 mfx_encode_bits8(packet, state, bits>>16, 8);
 166         }
 167         else
 168                 mfx_encode_bits8(packet, state, bits>>16, length-16);
 169         mfx_encode_bits8(packet, state, bits>>8, 8);
 170         mfx_encode_bits8(packet, state, bits, 8);
 171 }
 172
 173 static inline void mfx_address_field(OutputPacket *packet, MfxEncodingState *state, uint16_t address)
 174 {
 175         if(address<0x80)
 176                 mfx_encode_bits16(packet, state, 0x100|address, 9);
 177         else if(address<0x200)
 178                 mfx_encode_bits16(packet, state, 0xC00|address, 12);
 179         else if(address<0x800)
 180                 mfx_encode_bits16(packet, state, 0x7000|address, 15);
 181         else
 182         {
 183                 mfx_encode_bits8(packet, state, 0xF, 4);
 184                 mfx_encode_bits16(packet, state, address, 14);
 185         }
 186 }
 187
 188 void mfx_speed_field(OutputPacket *packet, MfxEncodingState *state, uint8_t speed_dir)
 189 {
 190         if(speed_dir&0x0F)
 191         {
 192                 // Field type: 001
 193                 mfx_encode_bits8(packet, state, 0x1, 3);
 194                 mfx_encode_bits8(packet, state, speed_dir, 8);
 195         }
 196         else
 197                 // Field type: 000
 198                 mfx_encode_bits8(packet, state, speed_dir>>4, 7);
 199 }
 200
 201 static void mfx_finish_packet(OutputPacket *packet, MfxEncodingState *state)
 202 {
 203         mfx_encode_bits8_raw(packet, state, state->crc8, 8);
 204         mfx_encode_flag_pairs(packet, 2);
 205         mfx_ensure_low_level(packet);
 206 }
 207
 208 static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *state)
 209 {
 210         mfx_encode_bits8_raw(packet, state, state->crc8, 8);
 211         mfx_encode_flag_pairs(packet, 11);
 212         mfx_encode_bits8_raw(packet, state, 0x3, 4);
 213         uint8_t fill = (1-mfx_get_output_level(packet))*0xFF;
 214
 215         for(uint8_t i=0; i<2; ++i)
 216         {
 217                 packet = output_create_chained_packet();
 218                 packet->bit_duration = 4;
 219                 packet->repeat_count = 1;
 220                 packet->final_delay = 0;
 221                 packet->trigger_position = 112;
 222                 packet->trigger_value = i+1;
 223
 224                 for(uint8_t j=0; j<16; ++j)
 225                         packet->data[j] = fill;
 226                 packet->length = 128;
 227                 mfx_encode_flag_pairs(packet, 1+i);
 228                 fill ^= 0xFF;
 229         }
 230
 231         mfx_ensure_low_level(packet);
 232 }
 233
 234 static void mfx_receive_feedback()
 235 {
 236         /* The decoder should activate a 52.6 kHz carrier to indicate positive
 237         acknowledgement, but so far I've been unable to build a circuit that detects
 238         it.  The increased current draw can nevertheless be measured. */
 239         uint8_t state = 0;
 240         uint16_t current[3];
 241         current[0] = monitor_track_current();
 242         while(state<2)
 243         {
 244                 uint8_t trig = output_get_trigger();
 245                 if(trig!=state)
 246                 {
 247                         current[trig] = monitor_track_current();
 248                         state = trig;
 249                 }
 250                 monitor_check();
 251         }
 252
 253         uint8_t reply[2];
 254         reply[0] = MFX_FEEDBACK;
 255         if(current[1]>feedback_threshold)
 256         {
 257                 current[1] -= feedback_threshold;
 258                 reply[1] = (current[1]>current[0] && current[1]>current[2]);
 259         }
 260         else
 261                 reply[1] = 0;
 262         interface_send(reply, 2);
 263 }
 264
 265 void mfx_announce_packet(uint16_t serial)
 266 {
 267         MfxEncodingState state;
 268         OutputPacket *packet = mfx_create_packet(&state);
 269         mfx_address_field(packet, &state, 0);
 270         // Packet type: 111101
 271         mfx_encode_bits8(packet, &state, 0x3D, 6);
 272         mfx_encode_bits32(packet, &state, station_id, 32);
 273         mfx_encode_bits16(packet, &state, serial, 16);
 274         mfx_finish_packet(packet, &state);
 275         output_send_packet();
 276 }
 277
 278 void mfx_search_packet(uint32_t mask_bits, uint8_t mask_size)
 279 {
 280         MfxEncodingState state;
 281         OutputPacket *packet = mfx_create_packet(&state);
 282         mfx_address_field(packet, &state, 0);
 283         // Packet type: 111010
 284         mfx_encode_bits8(packet, &state, 0x3A, 6);
 285         mfx_encode_bits8(packet, &state, mask_size, 6);
 286         mfx_encode_bits32(packet, &state, mask_bits, 32);
 287         mfx_finish_packet_feedback(packet, &state);
 288         output_send_packet();
 289 }
 290
 291 void mfx_assign_address_packet(uint16_t addr, uint32_t id)
 292 {
 293         MfxEncodingState state;
 294         OutputPacket *packet = mfx_create_packet(&state);
 295         mfx_address_field(packet, &state, 0);
 296         // Packet type: 111011
 297         mfx_encode_bits8(packet, &state, 0x3B, 6);
 298         mfx_encode_bits16(packet, &state, addr, 14);
 299         mfx_encode_bits32(packet, &state, id, 32);
 300         mfx_finish_packet(packet, &state);
 301         output_send_packet();
 302 }
 303
 304 void mfx_ping_packet(uint16_t addr, uint32_t id)
 305 {
 306         MfxEncodingState state;
 307         OutputPacket *packet = mfx_create_packet(&state);
 308         mfx_address_field(packet, &state, addr);
 309         // Packet type: 111100
 310         mfx_encode_bits8(packet, &state, 0x3C, 6);
 311         mfx_encode_bits32(packet, &state, id, 32);
 312         mfx_finish_packet_feedback(packet, &state);
 313         output_send_packet();
 314 }
 315
 316 void mfx_speed_packet(uint16_t addr, uint8_t speed_dir)
 317 {
 318         MfxEncodingState state;
 319         OutputPacket *packet = mfx_create_packet(&state);
 320         mfx_address_field(packet, &state, addr);
 321         mfx_speed_field(packet, &state, speed_dir);
 322         mfx_finish_packet(packet, &state);
 323         output_send_packet();
 324 }
 325
 326 void mfx_speed_funcs8_packet(uint16_t addr, uint8_t speed_dir, uint8_t funcs)
 327 {
 328         MfxEncodingState state;
 329         OutputPacket *packet = mfx_create_packet(&state);
 330         mfx_address_field(packet, &state, addr);
 331         mfx_speed_field(packet, &state, speed_dir);
 332         if(funcs&0xF0)
 333         {
 334                 // Field type: 0110
 335                 mfx_encode_bits8(packet, &state, 0x6, 4);
 336                 mfx_encode_bits8(packet, &state, funcs, 8);
 337         }
 338         else
 339                 // Field type: 010
 340                 mfx_encode_bits8(packet, &state, 0x20|funcs, 7);
 341         mfx_finish_packet(packet, &state);
 342         output_send_packet();
 343 }
 344
 345 void mfx_speed_funcs16_packet(uint16_t addr, uint8_t speed_dir, uint16_t funcs)
 346 {
 347         MfxEncodingState state;
 348         OutputPacket *packet = mfx_create_packet(&state);
 349         mfx_address_field(packet, &state, addr);
 350         mfx_speed_field(packet, &state, speed_dir);
 351         // Field type: 0111
 352         mfx_encode_bits8(packet, &state, 0x7, 4);
 353         mfx_encode_bits16(packet, &state, funcs, 16);
 354         mfx_finish_packet(packet, &state);
 355         output_send_packet();
 356 }
 357
 358 uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
 359 {
 360         if(cmd[0]==MFX_SET_STATION_ID)
 361         {
 362                 if(length!=5)
 363                         return LENGTH_ERROR;
 364
 365                 uint32_t id = (cmd[1]<<8)|cmd[2];
 366                 id <<= 16;
 367                 id |= (uint16_t)(cmd[3]<<8)|cmd[4];
 368                 mfx_set_station_id(id);
 369         }
 370         else if(cmd[0]==MFX_ANNOUNCE)
 371         {
 372                 if(length!=3)
 373                         return LENGTH_ERROR;
 374
 375                 mfx_announce_packet((cmd[1]<<8)|cmd[2]);
 376         }
 377         else if(cmd[0]==MFX_SEARCH)
 378         {
 379                 if(length!=6)
 380                         return LENGTH_ERROR;
 381
 382                 uint8_t mask_size = cmd[5];
 383                 if(mask_size>0x20)
 384                         return INVALID_VALUE;
 385
 386                 uint32_t mask_bits = (cmd[1]<<8)|cmd[2];
 387                 mask_bits <<= 16;
 388                 mask_bits |= (uint16_t)(cmd[3]<<8)|cmd[4];
 389                 mfx_search_packet(mask_bits, mask_size);
 390                 mfx_receive_feedback();
 391         }
 392         else if(cmd[0]==MFX_ASSIGN_ADDRESS || cmd[0]==MFX_PING)
 393         {
 394                 if(length!=7)
 395                         return LENGTH_ERROR;
 396
 397                 uint16_t addr = (cmd[1]<<8)|cmd[2];
 398                 if(addr==0 || addr>0x3FFF)
 399                         return INVALID_VALUE;
 400
 401                 uint32_t id = (cmd[3]<<8)|cmd[4];
 402                 id <<= 16;
 403                 id |= (uint16_t)(cmd[5]<<8)|cmd[6];
 404                 if(cmd[0]==MFX_ASSIGN_ADDRESS)
 405                         mfx_assign_address_packet(addr, id);
 406                 else
 407                 {
 408                         mfx_ping_packet(addr, id);
 409                         mfx_receive_feedback();
 410                 }
 411         }
 412         else if(cmd[0]==MFX_SPEED || cmd[0]==MFX_SPEED_FUNCS8 || cmd[0]==MFX_SPEED_FUNCS16)
 413         {
 414                 if(cmd[0]==MFX_SPEED && length!=4)
 415                         return LENGTH_ERROR;
 416                 else if(cmd[0]==MFX_SPEED_FUNCS8 && length!=5)
 417                         return LENGTH_ERROR;
 418                 else if(cmd[0]==MFX_SPEED_FUNCS16 && length!=6)
 419                         return LENGTH_ERROR;
 420
 421                 uint16_t addr = (cmd[1]<<8)|cmd[2];
 422                 if(addr==0 || addr>0x3FFF)
 423                         return INVALID_VALUE;
 424
 425                 uint8_t speed_dir = cmd[3];
 426                 if((speed_dir&0x7F)==0x7F)
 427                         return INVALID_VALUE;
 428                 ++speed_dir;
 429
 430                 if(cmd[0]==MFX_SPEED)
 431                         mfx_speed_packet(addr, speed_dir);
 432                 else if(cmd[0]==MFX_SPEED_FUNCS8)
 433                         mfx_speed_funcs8_packet(addr, speed_dir, cmd[4]);
 434                 else if(cmd[0]==MFX_SPEED_FUNCS16)
 435                         mfx_speed_funcs8_packet(addr, speed_dir, (cmd[4]<<8)|cmd[5]);
 436         }
 437         else
 438                 return INVALID_COMMAND;
 439
 440         return COMMAND_OK;
 441 }