From 55f60a37bbeeaa62796e1be8bef062eeb1522a71 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Sun, 3 Nov 2013 19:41:15 +0200 Subject: [PATCH] =?utf8?q?Add=20support=20for=20M=C3=A4rklin=20MFX=20proto?= =?utf8?q?col=20in=20arducontrol?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit It hasn't been exhaustively tested yet, but decoder registration and function control at least works. --- arducontrol/Makefile | 2 +- arducontrol/commands.h | 11 +- arducontrol/interface.c | 3 + arducontrol/mfx.c | 441 ++++++++++++++++++++++++++++++++++++++++ arducontrol/mfx.h | 16 ++ 5 files changed, 471 insertions(+), 2 deletions(-) create mode 100644 arducontrol/mfx.c create mode 100644 arducontrol/mfx.h diff --git a/arducontrol/Makefile b/arducontrol/Makefile index be2ca32..73dfb0f 100644 --- a/arducontrol/Makefile +++ b/arducontrol/Makefile @@ -1,4 +1,4 @@ FEATURES := SERIAL_ASYNC include ../common/build.mk -arducontrol.elf: adc.o interface.o monitor.o motorola.o output.o serial.o s88.o timer.o +arducontrol.elf: adc.o interface.o mfx.o monitor.o motorola.o output.o serial.o s88.o timer.o diff --git a/arducontrol/commands.h b/arducontrol/commands.h index 7e2370a..1fd027c 100644 --- a/arducontrol/commands.h +++ b/arducontrol/commands.h @@ -14,6 +14,14 @@ enum Command MOTOROLA_SPEED_DIRECTION = 0x13, MOTOROLA_SPEED_FUNCTION = 0x14, MOTOROLA_SOLENOID = 0x15, + MFX_SET_STATION_ID = 0x21, + MFX_ANNOUNCE = 0x22, + MFX_SEARCH = 0x23, + MFX_ASSIGN_ADDRESS = 0x24, + MFX_PING = 0x25, + MFX_SPEED = 0x28, + MFX_SPEED_FUNCS8 = 0x29, + MFX_SPEED_FUNCS16 = 0x2A, S88_READ = 0x30, COMMAND_OK = 0x80, RECEIVE_OVERRUN = 0x81, @@ -25,7 +33,8 @@ enum Command TRACK_CURRENT = 0xC0, INPUT_VOLTAGE = 0xC1, POWER_STATE = 0xC2, - S88_DATA = 0xD0 + S88_DATA = 0xD0, + MFX_FEEDBACK = 0xD1 }; #endif diff --git a/arducontrol/interface.c b/arducontrol/interface.c index 014e8e9..63b1ebd 100644 --- a/arducontrol/interface.c +++ b/arducontrol/interface.c @@ -1,5 +1,6 @@ #include #include "interface.h" +#include "mfx.h" #include "monitor.h" #include "motorola.h" #include "output.h" @@ -75,6 +76,8 @@ static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length) } else if(type==1) return motorola_command(cmd, length); + else if(type==2) + return mfx_command(cmd, length); else if(type==3) return s88_command(cmd, length); else diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c new file mode 100644 index 0000000..a39ce1f --- /dev/null +++ b/arducontrol/mfx.c @@ -0,0 +1,441 @@ +#include "interface.h" +#include "mfx.h" +#include "monitor.h" +#include "output.h" + +typedef struct +{ + uint8_t ones_count; + uint8_t crc8; +} MfxEncodingState; + +static uint32_t station_id = 0x12345678; +static uint8_t feedback_threshold = 30; + +void mfx_set_station_id(uint32_t id) +{ + station_id = id; +} + +static OutputPacket *mfx_create_packet(MfxEncodingState *state) +{ + OutputPacket *packet = output_create_packet(); + packet->bit_duration = 4; + packet->repeat_count = 1; + packet->final_delay = 0; + packet->length = 10; + packet->data[0] = 0x9B; + packet->data[1] = 0; + state->ones_count = 0; + state->crc8 = 0x7A; + return packet; +} + +static inline void mfx_update_crc8(MfxEncodingState *state, uint8_t bits, uint8_t length) +{ + for(uint8_t i=length; i--; ) + { + uint8_t bit = (bits>>i)&1; + if(bit^(state->crc8>>7)) + state->crc8 = (state->crc8<<1)^7; + else + state->crc8 <<= 1; + } +} + +static inline uint8_t mfx_get_output_level(const OutputPacket *packet) +{ + uint8_t b = packet->length-1; + return (packet->data[b>>3]>>(b&7))&1; +} + +static inline void mfx_ensure_low_level(OutputPacket *packet) +{ + if(mfx_get_output_level(packet)) + { + // Motorola decoders require a low logic level when idle. + uint8_t out_bit = packet->length; + if((out_bit&7)==0) + packet->data[out_bit>>3] = 0; + ++packet->length; + } +} + +static inline void mfx_encode_flag_pairs(OutputPacket *packet, uint8_t count) +{ + static const uint8_t flag_bits[5] = { 0x9B, 0x6C, 0xB2, 0xC9, 0x26 }; + + uint8_t invert = mfx_get_output_level(packet)*0xFF; + uint8_t shift = packet->length&7; + uint8_t total_bits = count*10; + uint8_t tail_bits = (total_bits+shift)&7; + uint8_t out_byte = packet->length>>3; + if(shift) + { + uint8_t head_bits = 8-shift; + packet->data[out_byte++] |= (flag_bits[0]^invert)<>3; + uint8_t carry = flag_bits[0]>>head_bits; + while(bytes--) + { + packet->data[out_byte++] = (carry|(flag_bits[i]<>head_bits; + + if(++i>=sizeof(flag_bits)) + i -= sizeof(flag_bits); + } + + if(tail_bits>0) + packet->data[out_byte] = ((carry|(flag_bits[i]<>3; + while(bytes--) + { + packet->data[out_byte++] = flag_bits[i]^invert; + + if(++i>=sizeof(flag_bits)) + i -= sizeof(flag_bits); + } + + uint8_t tail_bits = (total_bits+shift)&7; + if(tail_bits>0) + packet->data[out_byte] = (flag_bits[i]^invert)&((1<length += total_bits; +} + +static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length) +{ + uint8_t out_bit = packet->length; + uint8_t out_level = mfx_get_output_level(packet); + + for(uint8_t i=length; i--; ) + { + uint8_t bit = (bits>>i)&1; + + if(state->ones_count>=8) + { + ++i; + bit = 0; + state->ones_count = 0; + } + else if(bit) + ++state->ones_count; + else + state->ones_count = 0; + + if(out_level==bit) + bit ^= 3; + + // out_bit is always even at this point + if((out_bit&7)==0) + packet->data[out_bit>>3] = bit; + else + packet->data[out_bit>>3] |= bit<<(out_bit&7); + out_bit += 2; + out_level = bit>>1; + } + + packet->length = out_bit; +} + +static inline void mfx_encode_bits8(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length) +{ + mfx_encode_bits8_raw(packet, state, bits, length); + mfx_update_crc8(state, bits, length); +} + +static inline void mfx_encode_bits16(OutputPacket *packet, MfxEncodingState *state, uint16_t bits, uint8_t length) +{ + mfx_encode_bits8(packet, state, bits>>8, length-8); + mfx_encode_bits8(packet, state, bits, 8); +} + +static inline void mfx_encode_bits32(OutputPacket *packet, MfxEncodingState *state, uint32_t bits, uint8_t length) +{ + if(length>24) + { + mfx_encode_bits8(packet, state, bits>>24, length-24); + mfx_encode_bits8(packet, state, bits>>16, 8); + } + else + mfx_encode_bits8(packet, state, bits>>16, length-16); + mfx_encode_bits8(packet, state, bits>>8, 8); + mfx_encode_bits8(packet, state, bits, 8); +} + +static inline void mfx_address_field(OutputPacket *packet, MfxEncodingState *state, uint16_t address) +{ + if(address<0x80) + mfx_encode_bits16(packet, state, 0x100|address, 9); + else if(address<0x200) + mfx_encode_bits16(packet, state, 0xC00|address, 12); + else if(address<0x800) + mfx_encode_bits16(packet, state, 0x7000|address, 15); + else + { + mfx_encode_bits8(packet, state, 0xF, 4); + mfx_encode_bits16(packet, state, address, 14); + } +} + +void mfx_speed_field(OutputPacket *packet, MfxEncodingState *state, uint8_t speed_dir) +{ + if(speed_dir&0x0F) + { + // Field type: 001 + mfx_encode_bits8(packet, state, 0x1, 3); + mfx_encode_bits8(packet, state, speed_dir, 8); + } + else + // Field type: 000 + mfx_encode_bits8(packet, state, speed_dir>>4, 7); +} + +static void mfx_finish_packet(OutputPacket *packet, MfxEncodingState *state) +{ + mfx_encode_bits8_raw(packet, state, state->crc8, 8); + mfx_encode_flag_pairs(packet, 2); + mfx_ensure_low_level(packet); +} + +static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *state) +{ + mfx_encode_bits8_raw(packet, state, state->crc8, 8); + mfx_encode_flag_pairs(packet, 11); + mfx_encode_bits8_raw(packet, state, 0x3, 4); + uint8_t fill = (1-mfx_get_output_level(packet))*0xFF; + + for(uint8_t i=0; i<2; ++i) + { + packet = output_create_chained_packet(); + packet->bit_duration = 4; + packet->repeat_count = 1; + packet->final_delay = 0; + packet->trigger_position = 112; + packet->trigger_value = i+1; + + for(uint8_t j=0; j<16; ++j) + packet->data[j] = fill; + packet->length = 128; + mfx_encode_flag_pairs(packet, 1+i); + fill ^= 0xFF; + } + + mfx_ensure_low_level(packet); +} + +static void mfx_receive_feedback() +{ + /* The decoder should activate a 52.6 kHz carrier to indicate positive + acknowledgement, but so far I've been unable to build a circuit that detects + it. The increased current draw can nevertheless be measured. */ + uint8_t state = 0; + uint16_t current[3]; + current[0] = monitor_track_current(); + while(state<2) + { + uint8_t trig = output_get_trigger(); + if(trig!=state) + { + current[trig] = monitor_track_current(); + state = trig; + } + monitor_check(); + } + + uint8_t reply[2]; + reply[0] = MFX_FEEDBACK; + if(current[1]>feedback_threshold) + { + current[1] -= feedback_threshold; + reply[1] = (current[1]>current[0] && current[1]>current[2]); + } + else + reply[1] = 0; + interface_send(reply, 2); +} + +void mfx_announce_packet(uint16_t serial) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, 0); + // Packet type: 111101 + mfx_encode_bits8(packet, &state, 0x3D, 6); + mfx_encode_bits32(packet, &state, station_id, 32); + mfx_encode_bits16(packet, &state, serial, 16); + mfx_finish_packet(packet, &state); + output_send_packet(); +} + +void mfx_search_packet(uint32_t mask_bits, uint8_t mask_size) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, 0); + // Packet type: 111010 + mfx_encode_bits8(packet, &state, 0x3A, 6); + mfx_encode_bits8(packet, &state, mask_size, 6); + mfx_encode_bits32(packet, &state, mask_bits, 32); + mfx_finish_packet_feedback(packet, &state); + output_send_packet(); +} + +void mfx_assign_address_packet(uint16_t addr, uint32_t id) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, 0); + // Packet type: 111011 + mfx_encode_bits8(packet, &state, 0x3B, 6); + mfx_encode_bits16(packet, &state, addr, 14); + mfx_encode_bits32(packet, &state, id, 32); + mfx_finish_packet(packet, &state); + output_send_packet(); +} + +void mfx_ping_packet(uint16_t addr, uint32_t id) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, addr); + // Packet type: 111100 + mfx_encode_bits8(packet, &state, 0x3C, 6); + mfx_encode_bits32(packet, &state, id, 32); + mfx_finish_packet_feedback(packet, &state); + output_send_packet(); +} + +void mfx_speed_packet(uint16_t addr, uint8_t speed_dir) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, addr); + mfx_speed_field(packet, &state, speed_dir); + mfx_finish_packet(packet, &state); + output_send_packet(); +} + +void mfx_speed_funcs8_packet(uint16_t addr, uint8_t speed_dir, uint8_t funcs) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, addr); + mfx_speed_field(packet, &state, speed_dir); + if(funcs&0xF0) + { + // Field type: 0110 + mfx_encode_bits8(packet, &state, 0x6, 4); + mfx_encode_bits8(packet, &state, funcs, 8); + } + else + // Field type: 010 + mfx_encode_bits8(packet, &state, 0x20|funcs, 7); + mfx_finish_packet(packet, &state); + output_send_packet(); +} + +void mfx_speed_funcs16_packet(uint16_t addr, uint8_t speed_dir, uint16_t funcs) +{ + MfxEncodingState state; + OutputPacket *packet = mfx_create_packet(&state); + mfx_address_field(packet, &state, addr); + mfx_speed_field(packet, &state, speed_dir); + // Field type: 0111 + mfx_encode_bits8(packet, &state, 0x7, 4); + mfx_encode_bits16(packet, &state, funcs, 16); + mfx_finish_packet(packet, &state); + output_send_packet(); +} + +uint8_t mfx_command(const uint8_t *cmd, uint8_t length) +{ + if(cmd[0]==MFX_SET_STATION_ID) + { + if(length!=5) + return LENGTH_ERROR; + + uint32_t id = (cmd[1]<<8)|cmd[2]; + id <<= 16; + id |= (uint16_t)(cmd[3]<<8)|cmd[4]; + mfx_set_station_id(id); + } + else if(cmd[0]==MFX_ANNOUNCE) + { + if(length!=3) + return LENGTH_ERROR; + + mfx_announce_packet((cmd[1]<<8)|cmd[2]); + } + else if(cmd[0]==MFX_SEARCH) + { + if(length!=6) + return LENGTH_ERROR; + + uint8_t mask_size = cmd[5]; + if(mask_size>0x20) + return INVALID_VALUE; + + uint32_t mask_bits = (cmd[1]<<8)|cmd[2]; + mask_bits <<= 16; + mask_bits |= (uint16_t)(cmd[3]<<8)|cmd[4]; + mfx_search_packet(mask_bits, mask_size); + mfx_receive_feedback(); + } + else if(cmd[0]==MFX_ASSIGN_ADDRESS || cmd[0]==MFX_PING) + { + if(length!=7) + return LENGTH_ERROR; + + uint16_t addr = (cmd[1]<<8)|cmd[2]; + if(addr==0 || addr>0x3FFF) + return INVALID_VALUE; + + uint32_t id = (cmd[3]<<8)|cmd[4]; + id <<= 16; + id |= (uint16_t)(cmd[5]<<8)|cmd[6]; + if(cmd[0]==MFX_ASSIGN_ADDRESS) + mfx_assign_address_packet(addr, id); + else + { + mfx_ping_packet(addr, id); + mfx_receive_feedback(); + } + } + else if(cmd[0]==MFX_SPEED || cmd[0]==MFX_SPEED_FUNCS8 || cmd[0]==MFX_SPEED_FUNCS16) + { + if(cmd[0]==MFX_SPEED && length!=4) + return LENGTH_ERROR; + else if(cmd[0]==MFX_SPEED_FUNCS8 && length!=5) + return LENGTH_ERROR; + else if(cmd[0]==MFX_SPEED_FUNCS16 && length!=6) + return LENGTH_ERROR; + + uint16_t addr = (cmd[1]<<8)|cmd[2]; + if(addr==0 || addr>0x3FFF) + return INVALID_VALUE; + + uint8_t speed_dir = cmd[3]; + if((speed_dir&0x7F)==0x7F) + return INVALID_VALUE; + ++speed_dir; + + if(cmd[0]==MFX_SPEED) + mfx_speed_packet(addr, speed_dir); + else if(cmd[0]==MFX_SPEED_FUNCS8) + mfx_speed_funcs8_packet(addr, speed_dir, cmd[4]); + else if(cmd[0]==MFX_SPEED_FUNCS16) + mfx_speed_funcs8_packet(addr, speed_dir, (cmd[4]<<8)|cmd[5]); + } + else + return INVALID_COMMAND; + + return COMMAND_OK; +} diff --git a/arducontrol/mfx.h b/arducontrol/mfx.h new file mode 100644 index 0000000..d540184 --- /dev/null +++ b/arducontrol/mfx.h @@ -0,0 +1,16 @@ +#ifndef MFX_H_ +#define MFX_H_ + +#include + +void mfx_set_station_id(uint32_t); +void mfx_announce_packet(uint16_t); +void mfx_search_packet(uint32_t, uint8_t); +void mfx_assign_address_packet(uint16_t, uint32_t); +void mfx_ping_packet(uint16_t, uint32_t); +void mfx_speed_packet(uint16_t, uint8_t); +void mfx_speed_funcs8_packet(uint16_t, uint8_t, uint8_t); +void mfx_speed_funcs16_packet(uint16_t, uint8_t, uint16_t); +uint8_t mfx_command(const uint8_t *, uint8_t); + +#endif -- 2.43.0