ゆっくりラズパイ活用講座

#include <WiFi.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>

#define  SPI_MISO      47  // D12
#define  SPI_MOSI      38  // D11
#define  SPI_SCK       48  // D13
#define  SPI_DC        21  // D10
#define  SPI_TFT_CS    18  // D9
#define  SPI_TFT_RST   17  // D8
#define  SPI_TFT_BL    10  // D7 Backlight
#define  SERIAL2_RX     9  // Serial2 D6 ->QWIIC(RX)
#define  SERIAL2_TX     8  //         D5 ->QWIIC(TX)
#define  SERIAL1_RX     6  // Serial1 D2
#define  SERIAL1_TX     5  //         D3
#define  SERIAL0_RX    44  // Serial0 D0(RX0)
#define  SERIAL0_TX    43  //         D1(TX1)
SPIClass SPI2(HSPI);
#define  SPI2_MISO      3  // microSD
#define  SPI2_MOSI      2  // 
#define  SPI2_SCK       4  // 
#define  SPI2_CS        7  // 

#define PIC_BLK_LEN    128      // data length of each read, dont set this too big because ram is limited
#define PIC_FMT_VGA    7        // 640x480
#define PIC_FMT_CIF    5        // 320x240
#define PIC_FMT_OCIF   3        // 160x128
#define CAM_SERIAL     Serial

#define SHUTTER_PIN   1
#define SDSPEED 40000000

//#define PIC_FMT        PIC_FMT_VGA
#define PIC_FMT        PIC_FMT_CIF

unsigned long picTotalLen = 0;            // picture length

#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
Adafruit_ST7789  tft = Adafruit_ST7789(SPI_TFT_CS, SPI_DC, SPI_MOSI, SPI_SCK, SPI_TFT_RST);

#define TFT_BLACK 0x0000
#define TFT_RED   0xF800
#define TFT_BLUE  0x001F
#define TFT_GREEN 0x07E0
#define TFT_WHITE 0xFFFF

#include "picojpeg.h"

char filenames[255];
uint8_t *jpeg = NULL;

typedef struct {
	// Header
	uint8_t Signature[2];   // 'BM'
	uint8_t FileSize[4];
	uint8_t reserved1[4];   // unused (=0)
	uint8_t DataOffset[4];
	// InfoHeader
	uint8_t Size[4];        // Size of InfoHeader =40 
	uint8_t Width[4];
	uint8_t Height[4];
	uint8_t Planes[2];      // always 1
	uint8_t BitPerPixel[2];
	uint8_t Compression[4];
	uint8_t ImageSize[4];
	uint8_t XpixelsPerM[4];
	uint8_t YpixelsPerM[4];
	uint8_t ColorsUsed[4];
	uint8_t ImportantColors[4];
} BMP_FORMAT;

BMP_FORMAT Bmp;

typedef struct {
	uint8_t  R;
	uint8_t  G;
	uint8_t  B;
} PIXEL_INFO;

PIXEL_INFO *RGBs    = NULL;
uint16_t *color565s = NULL;

typedef struct {
	uint32_t Width;
	uint32_t Height;
	uint16_t BitPerPixel;
	uint32_t FileSize;
	uint32_t DataOffset;
	uint32_t ImageSize;
	uint32_t ColorsUsed;
} IMAGE_INFO;

IMAGE_INFO img;

#define local_min(a,b)  (((a) < (b)) ? (a) : (b))

FILE *stream;
pjpeg_image_info_t image_info;
int  jpeg_is_available;
int  jpeg_mcu_x;
int  jpeg_mcu_y;
uint jpeg_read_offset;
uint jpeg_size;

// for JPEGDec - public
uint8_t *jpeg_pImage;

int jpeg_MCUSPerRow;
int jpeg_MCUSPerCol;
int jpeg_MCUWidth;
int jpeg_MCUHeight;
int jpeg_MCUx;
int jpeg_MCUy;

void clearRxBuf()
{
	while (Serial2.available()) Serial2.read();
}

void sendCmd(char cmd[], int cmd_len)
{
	for (char i = 0; i < cmd_len; i++) Serial2.write(cmd[i]);
}

void initialize()
{
	char cmd[] = {0xaa,0x0d,0x00,0x00,0x00,0x00} ; // SNYC
	char cmd_reset[] = {0xaa,0x08,0x00,0x00,0x00,0x00} ; // Reset & Reboot
	unsigned char resp[6];
	uint8_t len;

	Serial2.setTimeout(500);
	while (1) {
		//clearRxBuf();
		sendCmd(cmd,6); // SNYC

		// Receive ACK from OV528
		len = Serial2.readBytes((char *)resp, 6);
		// aa:0e:0d:00:00:00
		if (len == 6) {
			if (   resp[0] == 0xaa
					&& resp[1] == 0x0e
					&& resp[2] == 0x0d
					&& resp[4] == 0
					&& resp[5] == 0) {

				// Receive SNYC from OV528
				len = Serial2.readBytes((char *)resp, 6);
				// aa:0d:00:00:00:00
				if (len == 6) {
					if (   resp[0] == 0xaa
							&& resp[1] == 0x0d
							&& resp[2] == 0
							&& resp[3] == 0
							&& resp[4] == 0
							&& resp[5] == 0)
						break;
				}
			} else {
				// fd:3f:08:00:00:00
				sendCmd(cmd_reset,6); // Reset & Reboot
			}
		}
	}
	cmd[1] = 0x0e;  // ACK
	cmd[2] = 0x0d;
	sendCmd(cmd, 6);

	// Baud Rate 115200bps
	cmd[1] = 0x07;
	cmd[2] = 0x00;
	cmd[3] = 0x01;
	cmd[4] = 0x00;
	cmd[5] = 0x00;
	sendCmd(cmd, 6);
	len = Serial2.readBytes((char *)resp, 6);
}

void preCapture()
{
	uint8_t len;

	char cmd[] = { 0xaa, 0x01, 0x00, 0x07, 0x00, PIC_FMT };
	unsigned char resp[6];

	Serial2.setTimeout(100);
	while (1) {
		clearRxBuf();
		sendCmd(cmd, 6);
		len = Serial2.readBytes((char *)resp, 6);
		if (len == 6) {
			if (   resp[0] == 0xaa
				  && resp[1] == 0x0e
				  && resp[2] == 0x01
				  && resp[4] == 0
				  && resp[5] == 0)
				break;
		}
		initialize();
	}
}

unsigned long Capture()
{
	char cmd[] = { 0xaa, 0x06, 0x08, PIC_BLK_LEN & 0xff, (PIC_BLK_LEN>>8) & 0xff ,0};
	unsigned char resp[6];
	uint8_t len;

	picTotalLen = 0;

	Serial2.setTimeout(100);
	while (1) {
		clearRxBuf();
		sendCmd(cmd, 6);
		len = Serial2.readBytes((char *)resp, 6);
		if (len == 6) {
			if (   resp[0] == 0xaa
			    && resp[1] == 0x0e
			    && resp[2] == 0x06
			    && resp[4] == 0
			    && resp[5] == 0)
			{
				cmd[1] = 0x05;
				cmd[2] = 0;
				cmd[3] = 0;
				cmd[4] = 0;
				cmd[5] = 0;
				while (1) {
					clearRxBuf();
					sendCmd(cmd, 6);
					len = Serial2.readBytes((char *)resp, 6);
					if (len == 6) {
						if (   resp[0] == 0xaa
						    && resp[1] == 0x0e
						    && resp[2] == 0x05
						    && resp[4] == 0
						    && resp[5] == 0)
							break;
						}
					}
				}
				cmd[1] = 0x04;
				cmd[2] = 0x1;
				while (1) {
				clearRxBuf();
				sendCmd(cmd, 6);
				len = Serial2.readBytes((char *)resp, 6);
				if (len == 6) {
					if (   resp[0] == 0xaa
					    && resp[1] == 0x0e
					    && resp[2] == 0x04
					    && resp[4] == 0
					    && resp[5] == 0) {
						Serial2.setTimeout(1000);
						len = Serial2.readBytes((char *)resp, 6);
						if (len == 6) {
							if (   resp[0] == 0xaa
							    && resp[1] == 0x0a
							    && resp[2] == 0x01) {
								picTotalLen = (resp[3]) | (resp[4] << 8) | (resp[5] << 16);
								return picTotalLen;
							}
						}
					}
				}
			}
		}
		initialize();
		preCapture();
	}
	return picTotalLen;
}

boolean GetData()
{
	uint8_t *imgPt = jpeg;
	uint16_t cnt = 0;
	int retry_cnt = 0;

	unsigned int pktCnt = (picTotalLen) / (PIC_BLK_LEN - 6);
	if ((picTotalLen % (PIC_BLK_LEN-6)) != 0) pktCnt += 1;

	char cmd[] = { 0xaa, 0x0e, 0x00, 0x00, 0x00, 0x00 };
	unsigned char pkt[PIC_BLK_LEN];

	Serial2.setTimeout(1000);
	for (unsigned int i = 0; i < pktCnt; i++) {
		cmd[4] = i & 0xff;
		cmd[5] = (i >> 8) & 0xff;

		int retry_cnt = 0;
		retry:
		delay(10);
		clearRxBuf();
		sendCmd(cmd, 6);

		retry_cnt = 0;
		while (1) {
			cnt = Serial2.readBytes((char *)pkt, PIC_BLK_LEN);
			if (cnt > 0) break;
			if (++retry_cnt > 10) return false;
		}

		retry_cnt = 0;
		unsigned char sum = 0;
		for (int y = 0; y < cnt - 2; y++) sum += pkt[y];
		if (sum != pkt[cnt-2]) {
			if (++retry_cnt < 100) goto retry; else return false;
		}

		memcpy(imgPt, &pkt[4], cnt-6);
		imgPt += (cnt-6);
	}
	cmd[4] = 0xf0;
	cmd[5] = 0xf0;
	sendCmd(cmd, 6);
	return true;
}

boolean  saveBMP(uint32_t width, uint32_t height)
{
	File  bmpFile;
	PIXEL_INFO *rgb = RGBs;
	int   x,y;
	int   from_x, from_y, to_y;
	uint32_t pos;
	static int picNameNum = 1;
	char picName[] = "/ov528/pic00.bmp";

	while (picNameNum <= 100) {
		picName[10] = picNameNum/10 + '0';
		picName[11] = picNameNum%10 + '0';
		if (!SD.exists(picName)) break;
		picNameNum++;
	}
	if (picNameNum > 100) return false;

	img.BitPerPixel = 24;
	img.ColorsUsed  = 0;
	if (width  > 0) img.Height = height;
	if (height > 0) img.Width  = width;
	img.ImageSize  = img.Width * img.Height * 3;
	img.DataOffset = sizeof(BMP_FORMAT);
	img.FileSize   = img.DataOffset + img.ImageSize;

	memset(&Bmp, 0x00, sizeof(BMP_FORMAT));
	memcpy(&Bmp.Signature,(uint8_t*)"BM",2);
	long2byte(Bmp.FileSize,     img.FileSize);
	long2byte(Bmp.DataOffset,   img.DataOffset);
	long2byte(Bmp.Size,         40);
	long2byte(Bmp.Width,        img.Width);
	long2byte(Bmp.Height,       img.Height);
	short2byte(Bmp.Planes,      1);
	short2byte(Bmp.BitPerPixel, img.BitPerPixel);
	long2byte(Bmp.ImageSize,    img.ImageSize);
	long2byte(Bmp.ColorsUsed,   img.ColorsUsed);

	bmpFile = SD.open(picName, FILE_WRITE);
	if(!bmpFile) {
		SD.end();
		return false;
	}

	bmpFile.write((uint8_t*)&Bmp, sizeof(BMP_FORMAT));
	bmpFile.write((const uint8_t *)rgb, img.ImageSize);

	bmpFile.close();
	SD.end();

	return true;
}

static uint8_t pjpeg_need_bytes_callback(uint8_t* pBuf, uint8_t buf_size, uint8_t *pBytes_actually_read, void *pCallback_data) {
	pjpeg_callback(pBuf, buf_size, pBytes_actually_read, pCallback_data);
	return 0;
}

uint8_t pjpeg_callback(uint8_t* pBuf, uint8_t buf_size, uint8_t *pBytes_actually_read, void *pCallback_data) {
	uint n;

	n = local_min(jpeg_size - jpeg_read_offset, buf_size);

	fread(pBuf,n,1,stream);

	*pBytes_actually_read = (uint8_t)(n);
	jpeg_read_offset += n;
	return 0;
}

int JpegDec_decode(unsigned long len)
{
	jpeg_mcu_x = 0;
	jpeg_mcu_y = 0;
	jpeg_is_available = 0;
	jpeg_read_offset = 0;
	jpeg_size = len;

	uint8_t status = pjpeg_decode_init(&image_info, pjpeg_need_bytes_callback, NULL, 0);

	if (status) return -1;

	// In reduce mode output 1 pixel per 8x8 block.
	jpeg_MCUSPerRow = image_info.m_MCUSPerRow;
	jpeg_MCUSPerCol = image_info.m_MCUSPerCol;
	jpeg_MCUWidth   = image_info.m_MCUWidth;
	jpeg_MCUHeight  = image_info.m_MCUHeight;

	jpeg_pImage = new uint8_t[image_info.m_MCUWidth * image_info.m_MCUHeight * 3];
//	jpeg_pImage = (uint8_t *)malloc(jpeg_MCUWidth * jpeg_MCUHeight * 3);
	if (!jpeg_pImage) return -1;
	memset(jpeg_pImage, 0, sizeof(jpeg_pImage));

	jpeg_is_available = 1 ;

	return JpegDec_decode_mcu();
}

int JpegDec_decode_mcu(void) {

	uint8_t status = pjpeg_decode_mcu();

	if (status) {
		jpeg_is_available = 0 ;

		if (status != PJPG_NO_MORE_BLOCKS) return -1;
	}
	return 1;
}

int JpegDec_read(void)
{
	int y, x;
	uint8_t *pDst_row;

	if(jpeg_is_available == 0 || jpeg_mcu_y >= image_info.m_MCUSPerCol) return 0;

	// Copy MCU's pixel blocks into the destination RGBs.
	pDst_row = jpeg_pImage;
	for (y = 0; y < image_info.m_MCUHeight; y += 8) {
		const int by_limit = local_min(8, image_info.m_height - (jpeg_mcu_y * image_info.m_MCUHeight + y));

		for (x = 0; x < image_info.m_MCUWidth; x += 8) {
			uint8_t *pDst_block = pDst_row + x * 3;

			// Compute source byte offset of the block in the decoder's MCU buffer.
			uint src_ofs = (x * 8U) + (y * 16U);
			const uint8_t *pSrcR = image_info.m_pMCUBufR + src_ofs;
			const uint8_t *pSrcG = image_info.m_pMCUBufG + src_ofs;
			const uint8_t *pSrcB = image_info.m_pMCUBufB + src_ofs;

			const int bx_limit = local_min(8, image_info.m_width - (jpeg_mcu_x * image_info.m_MCUWidth + x));

			int bx, by;
			for (by = 0; by < by_limit; by++) {
				uint8_t *pDst = pDst_block;

				for (bx = 0; bx < bx_limit; bx++) {
					pDst[0] = *pSrcR++;
					pDst[1] = *pSrcG++;
					pDst[2] = *pSrcB++;
					pDst += 3;
				}

				pSrcR += (8 - bx_limit);
				pSrcG += (8 - bx_limit);
				pSrcB += (8 - bx_limit);
				pDst_block += jpeg_MCUWidth * 3;
			}
		}
		pDst_row += (jpeg_MCUWidth * 8 *3 );
	}

	jpeg_MCUx = jpeg_mcu_x;
	jpeg_MCUy = jpeg_mcu_y;

	jpeg_mcu_x++;
	if (jpeg_mcu_x == image_info.m_MCUSPerRow) {
		jpeg_mcu_x = 0;
		jpeg_mcu_y++;
	}

	if(JpegDec_decode_mcu() < 0) jpeg_is_available = 0 ;

	return 1;
}

void short2byte(uint8_t *pt, uint16_t val) {
	*pt     = (uint8_t)val      &0x00ff;
	*(pt+1) = (uint8_t)((val>>8)&0x00ff);
}

void long2byte(uint8_t *pt, uint32_t val) {
	*pt     = (uint8_t)val       &0x000000ff;
	*(pt+1) = (uint8_t)((val>> 8)&0x000000ff);
	*(pt+2) = (uint8_t)((val>>16)&0x000000ff);
	*(pt+3) = (uint8_t)((val>>24)&0x000000ff);
}

boolean Jpeg2RGBs(uint8_t *data, uint32_t len, uint32_t *width, uint32_t *height)
{
	uint8_t  *pImg;
	uint32_t pos;
	int      x,y,bx,by;

	if((stream = fmemopen((void*)data, len, "rb"))==NULL) return false;

	// Decoding start
	if ( JpegDec_decode(len) == -1) return false;
	*width  = image_info.m_width;
	*height = image_info.m_height;

	while(JpegDec_read()){
		pImg = jpeg_pImage;

		for(by=0; by<jpeg_MCUHeight; by++){
			for(bx=0; bx<jpeg_MCUWidth; bx++){
				x = jpeg_MCUx * jpeg_MCUWidth + bx;
				y = jpeg_MCUy * jpeg_MCUHeight + by;
				if(x<image_info.m_width && y<image_info.m_height){
					pos = x + (y * image_info.m_width);
					RGBs[pos].B = pImg[0];
					RGBs[pos].G = pImg[1];
					RGBs[pos].R = pImg[2];
					color565s[pos] = ((RGBs[pos].B >> 3)<<11) | ((RGBs[pos].G >> 2)<<5) | (RGBs[pos].R >> 3);
				}
				pImg += 3;
			}
		}
	}
	delete[] jpeg_pImage;

	return true;
}

void setup()
{
	char filename[64], *pt;
	uint32_t pos, x, y;
	uint32_t bmp_width, bmp_height;

	pinMode(SPI2_CS, OUTPUT);
	SPI2.begin(SPI2_SCK, SPI2_MISO, SPI2_MOSI, -1);
	while(!SD.begin(SPI2_CS, SPI2, SDSPEED)) delay(100);

	pinMode(SHUTTER_PIN, INPUT);    // initialize the pushbutton pin as an input
	Serial2.begin(115200, SERIAL_8N1, SERIAL2_RX, SERIAL2_TX);

	initialize();
	preCapture();

	if (!(color565s = (uint16_t*)malloc(320*240*2))) while(1);
	if (!(RGBs = (PIXEL_INFO *)malloc(320 * 240 * sizeof(PIXEL_INFO)))) while(1);

  tft.init(240, 320);  // Init ST7789
	tft.setRotation(1);

	while(1) {
		if (Capture() > 0) {
			if (jpeg) free(jpeg);
			if ((jpeg = (uint8_t*)malloc(picTotalLen))) {
				if (GetData()) {
					Jpeg2RGBs(jpeg, picTotalLen, &bmp_width, &bmp_height);
					tft.drawRGBBitmap(0, 0, color565s, bmp_width, bmp_height);
					if (digitalRead(SHUTTER_PIN)) {
						tft.setCursor(10, 10);
						tft.setTextColor(TFT_RED, TFT_BLACK);
						tft.setTextSize(1);
						tft.println("[REC]");
						saveBMP(bmp_width, bmp_height);
					}
				}
			}
		}
	}
}

void loop() {}