M5 Stack のbitmapの拡大表示

元は24bitのbitmapを16bitに変換して表示してるっぽい。

padding部分はとりあえずそのまま。

void M5Display::drawBmpFile2(fs::FS &fs, const char *path, uint16_t x, uint16_t y) {
  if ((x >= width()) || (y >= height())) return;

  // Open requested file on SD card
  File bmpFS = fs.open(path, "r");

  if (!bmpFS) {
    Serial.print("File not found");
    return;
  }

  uint32_t seekOffset;
  uint16_t w, h, row, col;
  uint8_t  r, g, b;

  uint32_t startTime = millis();

  if (read16(bmpFS) == 0x4D42) {
    read32(bmpFS);
    read32(bmpFS);
    seekOffset = read32(bmpFS);
    read32(bmpFS);
    w = read32(bmpFS);
    h = read32(bmpFS);

    if ((read16(bmpFS) == 1) && (read16(bmpFS) == 24) && (read32(bmpFS) == 0)) {
      y += h*SCALE - 1;

      setSwapBytes(true);
      bmpFS.seek(seekOffset);

      uint16_t padding = (4 - ((w * 3) & 3)) & 3;
      uint8_t lineBuffer[w * 3 + padding];
      uint8_t lineBuffer2[w * 3 * SCALE + padding];

      for (row = 0; row < h; row++) {
        bmpFS.read(lineBuffer, sizeof(lineBuffer));
        uint8_t*  bptr = lineBuffer;
        uint16_t* tptr = (uint16_t*)lineBuffer2;
        // Convert 24 to 16 bit colours
        for (col = 0; col < w; col++) {
          b = *bptr++;
          g = *bptr++;
          r = *bptr++;
        	for(int i = 0; i < SCALE; i++){
        		*tptr++ = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
        	}
        }

        // Push the pixel row to screen, pushImage will crop the line if needed
        // y is decremented as the BMP image is drawn bottom up
        	for(int i = 0; i < SCALE; i++){
      			pushImage(x, y--, w*SCALE, 1, (uint16_t*)lineBuffer2);
      		}
      }
      Serial.print("Loaded in "); Serial.print(millis() - startTime);
      Serial.println(" ms");
    }
    else Serial.println("BMP format not recognized.");
  }
  bmpFS.close();
}