Insert dummy zero at end of packet if last eight bits are ones

diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
index a39ce1fbc25792ef4ca1440847478f814449fe34..95de44523110d0e1a83805cb4ee6f91b83ce85a9 100644 (file)
--- a/arducontrol/mfx.c
+++ b/arducontrol/mfx.c
@@ -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;
 }