Fixes #1925

radio/src/bmp.cpp

@@ -203,3 +203,54 @@ const pm_char * bmpLoad(uint8_t *bmp, const char *filename, const unsigned int w
   f_close(&bmpFile);
   return 0;
 }
+
+const uint8_t bmpHeader[] = {
+  0x42, 0x4d, 0xF8, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x76, 0x00, 0x00, 0x00, 0x28, 0x00,
+  0x00, 0x00, 212,  0x00, 0x00, 0x00, 64,   0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0xbc, 0x38, 0x00, 0x00, 0xbc, 0x38, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, 0x11, 0x11, 0x00, 0x22, 0x22,
+  0x22, 0x00, 0x33, 0x33, 0x33, 0x00, 0x44, 0x44, 0x44, 0x00, 0x55, 0x55, 0x55, 0x00, 0x66, 0x66,
+  0x66, 0x00, 0x77, 0x77, 0x77, 0x00, 0x88, 0x88, 0x88, 0x00, 0x99, 0x99, 0x99, 0x00, 0xaa, 0xaa,
+  0xaa, 0x00, 0xbb, 0xbb, 0xbb, 0x00, 0xcc, 0xcc, 0xcc, 0x00, 0xdd, 0xdd, 0xdd, 0x00, 0xee, 0xee,
+  0xee, 0x00, 0xff, 0xff, 0xff, 0x00
+};
+
+inline display_t getPixel(int x, int y)
+{
+  if (x>=LCD_W || y>=LCD_H)
+    return 0;
+  display_t * p = &displayBuf[y / 2 * LCD_W + x];
+  return (y & 1) ? (*p >> 4) : (*p & 0x0F);
+}
+
+const char *writeScreenshot(const char *filename)
+{
+  FIL bmpFile;
+  UINT written;
+
+  FRESULT result = f_open(&bmpFile, filename, FA_CREATE_ALWAYS | FA_WRITE);
+  if (result != FR_OK) {
+    return SDCARD_ERROR(result);
+  }
+
+  result = f_write(&bmpFile, bmpHeader, sizeof(bmpHeader), &written);
+  if (result != FR_OK || written != sizeof(bmpHeader)) {
+    f_close(&bmpFile);
+    return SDCARD_ERROR(result);
+  }
+
+  for (int y=LCD_H-1; y>=0; y-=1) {
+    for (int x=0; x<8*((LCD_W+7)/8); x+=2) {
+      uint8_t byte = getPixel(x+1, y) + (getPixel(x, y) << 4);
+      f_write(&bmpFile, &byte, 1, &written);
+      if (result != FR_OK || written != 1) {
+        f_close(&bmpFile);
+        return SDCARD_ERROR(result);
+      }
+    }
+  }
+
+  f_close(&bmpFile);
+
+  return NULL;
+}