void convert_pic(uint16_t w, uint16_t h, const uint8_t *data, uint8_t *dest) { uint8_t remainder = 0; uint8_t remainder_size = 0; uint8_t pic_width_bytes = w / 8 + ((w % 8) ? 1 : 0); uint16_t spurious_bits = (pic_width_bytes * 8) - w; uint16_t counter = 0; for(uint16_t row = 0; row < h; row++) { remainder = 0; remainder_size = 0; // MSB is the color indicator for(uint8_t column = 0; column < pic_width_bytes; column++) { uint8_t pix_data = data[(pic_width_bytes * row) + column]; uint8_t conv_data = (pix_data << remainder_size) | remainder; remainder_size++; remainder = (pix_data >> (8 - remainder_size)) & 0x7F; dest[counter++] = conv_data & 0x7F; if(remainder_size == 7) { dest[counter++] = remainder & 0x7F; remainder = 0; remainder_size = 0; } } if(remainder_size > spurious_bits) { uint8_t mask = ~((1 << remainder_size) - 1); dest[counter] = (dest[counter] & mask) | (remainder & 0x7F); counter++; } } }