X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=arducontrol%2Fmfx.c;h=293765fba9d7c8933106e48c6cdbc5ab4923cfa2;hb=57bb90d8019098a9b8fe63093498862ce6551017;hp=a39ce1fbc25792ef4ca1440847478f814449fe34;hpb=55f60a37bbeeaa62796e1be8bef062eeb1522a71;p=model-railway-devices.git

diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
index a39ce1f..293765f 100644
--- 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,6 +232,7 @@ static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *s
 	}
 
 	mfx_ensure_low_level(packet);
+	packet->final_delay = 128;
 }
 
 static void mfx_receive_feedback()
@@ -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);