Provide peak current since last read

[model-railway-devices.git] / arducontrol / mfx.c

diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
index a39ce1fbc25792ef4ca1440847478f814449fe34..8c46fc3123cf40d16ac3fc9f56c74eff7989d70e 100644 (file)
--- a/arducontrol/mfx.c
+++ b/arducontrol/mfx.c
@@ -22,7 +22,7 @@ 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->final_delay = 128;
        packet->length = 10;
        packet->data[0] = 0x9B;
        packet->data[1] = 0;
@@ -114,21 +114,16 @@ static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *
 {
        uint8_t out_bit = packet->length;
        uint8_t out_level = mfx_get_output_level(packet);
+       uint8_t ones_count = state->ones_count;
 
        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;
+               if(bit)
+                       ++ones_count;
                else
-                       state->ones_count = 0;
+                       ones_count = 0;
 
                if(out_level==bit)
                        bit ^= 3;
@@ -140,8 +135,15 @@ static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *
                        packet->data[out_bit>>3] |= bit<<(out_bit&7);
                out_bit += 2;
                out_level = bit>>1;
+
+               if(ones_count>=8)
+               {
+                       bits &= (1<<i)-1;
+                       ++i;
+               }
        }
 
+       state->ones_count = ones_count;
        packet->length = out_bit;
 }
 
@@ -210,6 +212,7 @@ static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *s
        mfx_encode_bits8_raw(packet, state, state->crc8, 8);
        mfx_encode_flag_pairs(packet, 11);
        mfx_encode_bits8_raw(packet, state, 0x3, 4);
+       packet->final_delay = 0;
        uint8_t fill = (1-mfx_get_output_level(packet))*0xFF;
 
        for(uint8_t i=0; i<2; ++i)
@@ -229,9 +232,10 @@ static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *s
        }
 
        mfx_ensure_low_level(packet);
+       packet->final_delay = 128;
 }
 
-static void mfx_receive_feedback()
+static void mfx_receive_feedback(uint8_t type)
 {
        /* 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
@@ -251,7 +255,7 @@ static void mfx_receive_feedback()
        }
 
        uint8_t reply[2];
-       reply[0] = MFX_FEEDBACK;
+       reply[0] = type;
        if(current[1]>feedback_threshold)
        {
                current[1] -= feedback_threshold;
@@ -387,7 +391,7 @@ uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
                mask_bits <<= 16;
                mask_bits |= (uint16_t)(cmd[3]<<8)|cmd[4];
                mfx_search_packet(mask_bits, mask_size);
-               mfx_receive_feedback();
+               mfx_receive_feedback(MFX_SEARCH_FEEDBACK);
        }
        else if(cmd[0]==MFX_ASSIGN_ADDRESS || cmd[0]==MFX_PING)
        {
@@ -406,7 +410,7 @@ uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
                else
                {
                        mfx_ping_packet(addr, id);
-                       mfx_receive_feedback();
+                       mfx_receive_feedback(MFX_PING_FEEDBACK);
                }
        }
        else if(cmd[0]==MFX_SPEED || cmd[0]==MFX_SPEED_FUNCS8 || cmd[0]==MFX_SPEED_FUNCS16)
@@ -425,7 +429,9 @@ uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
                uint8_t speed_dir = cmd[3];
                if((speed_dir&0x7F)==0x7F)
                        return INVALID_VALUE;
-               ++speed_dir;
+               // Skip emergency stop
+               if(speed_dir>0)
+                       ++speed_dir;
 
                if(cmd[0]==MFX_SPEED)
                        mfx_speed_packet(addr, speed_dir);