ov7670 raw bayer capture arduino uno

#include <Wire.h>
#include <SdFat.h>
#include <SdFatUtil.h>
// file system
uint8_t buf2[512];
SdFat sd;
//uint8_t* buf;
// test file
SdFile file;
// SD chip select pin
const uint8_t chipSelect = 9;
uint8_t sensor_addr = 0x42;
#define REG_COM1	0x04	/* Control 1 */
#define REG_COM7	0x12	/* Control 7 */
#define   COM7_RESET	  0x80	  /* Register reset */
#define   COM7_FMT_MASK	  0x38
#define   COM7_FMT_VGA	  0x00
#define	  COM7_FMT_CIF	  0x20	  /* CIF format */
#define   COM7_FMT_QVGA	  0x10	  /* QVGA format */
#define   COM7_FMT_QCIF	  0x08	  /* QCIF format */
#define	  COM7_RGB	  0x04	  /* bits 0 and 2 - RGB format */
#define	  COM7_YUV	  0x00	  /* YUV */
#define	  COM7_BAYER	  0x01	  /* Bayer format */
#define	  COM7_PBAYER	  0x05	  /* "Processed bayer" */
#define REG_RGB444	0x8c	/* RGB 444 control */
#define   R444_ENABLE	  0x02	  /* Turn on RGB444, overrides 5x5 */
#define   R444_RGBX	  0x01	  /* Empty nibble at end */
#define REG_COM9	0x14	/* Control 9  - gain ceiling */
#define REG_COM10	0x15	/* Control 10 */
#define REG_COM13	0x3d	/* Control 13 */
#define   COM13_GAMMA	  0x80	  /* Gamma enable */
#define	  COM13_UVSAT	  0x40	  /* UV saturation auto adjustment */
#define   COM13_UVSWAP	  0x01	  /* V before U - w/TSLB */
#define REG_COM15	0x40	/* Control 15 */
#define   COM15_R10F0	  0x00	  /* Data range 10 to F0 */
#define	  COM15_R01FE	  0x80	  /*            01 to FE */
#define   COM15_R00FF	  0xc0	  /*            00 to FF */
#define   COM15_RGB565	  0x10	  /* RGB565 output */
#define   COM15_RGB555	  0x30	  /* RGB555 output */
#define REG_COM11	0x3b	/* Control 11 */
#define   COM11_NIGHT	  0x80	  /* NIght mode enable */
#define   COM11_NMFR	  0x60	  /* Two bit NM frame rate */
#define   COM11_HZAUTO	  0x10	  /* Auto detect 50/60 Hz */
#define	  COM11_50HZ	  0x08	  /* Manual 50Hz select */
#define   COM11_EXP	  0x02
#define REG_COM16	0x41	/* Control 16 */
#define   COM16_AWBGAIN   0x08	  /* AWB gain enable */
#define REG_COM17       0x42    /* Control 17 */
#define COM17_AECWIN    0xc0    /* AEC window - must match COM4 */
#define COM17_CBAR      0x08    /* DSP Color bar */
#define REG_TSLB	0x3a	/* lots of stuff */
#define   TSLB_YLAST	  0x04	  /* UYVY or VYUY - see com13 */
#define MTX1            0x4f    /* Matrix Coefficient 1 */
#define MTX2            0x50    /* Matrix Coefficient 2 */
#define MTX3            0x51    /* Matrix Coefficient 3 */
#define MTX4            0x52    /* Matrix Coefficient 4 */
#define MTX5            0x53    /* Matrix Coefficient 5 */
#define MTX6            0x54    /* Matrix Coefficient 6 */
#define REG_CONTRAS     0x56    /* Contrast control */
#define MTXS            0x58    /* Matrix Coefficient Sign */
#define AWBC7           0x59    /* AWB Control 7 */
#define AWBC8           0x5a    /* AWB Control 8 */
#define AWBC9           0x5b    /* AWB Control 9 */
#define AWBC10          0x5c    /* AWB Control 10 */
#define AWBC11          0x5d    /* AWB Control 11 */
#define AWBC12          0x5e    /* AWB Control 12 */
#define REG_GFIX        0x69    /* Fix gain control */
#define GGAIN           0x6a    /* G Channel AWB Gain */
#define DBLV            0x6b
#define AWBCTR3         0x6c    /* AWB Control 3 */
#define AWBCTR2         0x6d    /* AWB Control 2 */
#define AWBCTR1         0x6e    /* AWB Control 1 */
#define AWBCTR0         0x6f    /* AWB Control 0 */
#define REG_COM8	0x13	/* Control 8 */
#define   COM8_FASTAEC	  0x80	  /* Enable fast AGC/AEC */
#define   COM8_AECSTEP	  0x40	  /* Unlimited AEC step size */
#define   COM8_BFILT	  0x20	  /* Band filter enable */
#define   COM8_AGC	  0x04	  /* Auto gain enable */
#define   COM8_AWB	  0x02	  /* White balance enable */
#define   COM8_AEC	  0x01	  /* Auto exposure enable */
#define REG_COM3        0x0c    /* Control 3 */
#define COM3_SWAP       0x40    /* Byte swap */
#define COM3_SCALEEN    0x08    /* Enable scaling */
#define COM3_DCWEN      0x04    /* Enable downsamp/crop/window */
#define REG_BRIGHT      0x55    /* Brightness */
// Serial output stream
// store error strings in flash to save RAM
uint32_t bgnBlock, endBlock;
#define error(s) sd.errorHalt_P(PSTR(s))
#define useSdCard  PORTB|=4; PORTB&=~2
#define UseFastWR
#define useLed
void setup()
{
	Wire.begin();
    pinMode(10,OUTPUT);
    digitalWrite(10,HIGH);
	DDRB|=47;//clock as output and SPI pins as output except MISO
	PORTB|=6;//set both CS pins to high
	DDRC&=~15;//low d0-d3 camera
	DDRD&=~252;//d7-d4 and interupt pins
  #ifdef useLed
  DDRD|=1;//pin 0 as output
  PORTD&=~1;//turn off led
  //set pin 1 as input
  DDRD&=~2;
  PORTB|=2;//enable pullup
  #endif
	//DDRB&=~2;//href as input
	//EICRA=11;//int0 rising int1 falling
	//EIMSK=2;//disable int0 and enable int1
	//disable pin change interupt
	//PCICR=0;
	//PCMSK0=2;

	//set up twi for 100khz
	TWSR&=~3;//disable prescaler for TWI
	TWBR=72;//set to 100khz
	//enable serial
	//UBRR0H = (unsigned char)(MYUBRR>>8);
	//UBRR0L = (unsigned char)MYUBRR&255;
  #ifndef useLed
	UBRR0H=0;
	UBRR0L=207;//3 = 0.5M 2M baud rate = 0 7 = 250k 207 is 9600 baud rate
	UCSR0A|=2;//double speed aysnc
	UCSR0B = (1<<RXEN0)|(1<<TXEN0);//Enable receiver and transmitter
	UCSR0C=6;//async 1 stop bit 8bit char no parity bits
         PgmPrintln("Setting up Camera Registers");
         #endif
	//set up camera
	wrReg(0x15,32);//pclk does not toggle on HBLANK COM10 vsync falling
	//wrReg(0x11,32);//using scaler for divider
	wrReg(REG_RGB444, 0x00);             // Disable RGB444
	wrReg(REG_COM11,226);//enable nigh mode 1/8 frame rate COM11*/
	wrReg(REG_TSLB,0x04);                // 0D = UYVY  04 = YUYV
 	wrReg(REG_COM13,0x88);               // connect to REG_TSLB
	//wrReg(REG_COM13,0x8);               // connect to REG_TSLB disable gamma
	wrReg(REG_COM7, 0x01);           // raw
	wrReg(REG_COM15, 0xC0);          // Set Full range
        //wrReg(REG_COM3, 0x04);
	#if defined qqvga || defined qvga
	wrReg(REG_COM3,4);    // REG_COM3
	#else
	wrReg(REG_COM3,0);    // REG_COM3
	#endif
        //wrReg(0x3e,0x00);        //  REG_COM14
     //   wrReg(0x72,0x11);        //
       // wrReg(0x73,0xf0);        //
	#ifdef qqvga
	wrReg(REG_COM14, 0x1a);          // divide by 4
        wrReg(0x72, 0x22);               // downsample by 4
        wrReg(0x73, 0xf2);               // divide by 4
        wrReg(REG_HSTART,0x16);
        wrReg(REG_HSTOP,0x04);
        wrReg(REG_HREF,0xa4);
        wrReg(REG_VSTART,0x02);
        wrReg(REG_VSTOP,0x7a);
        wrReg(REG_VREF,0x0a);
	#endif
	#ifdef qvga
	wrReg(REG_COM14, 0x19);
        wrReg(0x72, 0x11);
        wrReg(0x73, 0xf1);
        wrReg(REG_HSTART,0x16);
        wrReg(REG_HSTOP,0x04);
        wrReg(REG_HREF,0x24);
        wrReg(REG_VSTART,0x02);
        wrReg(REG_VSTOP,0x7a);
        wrReg(REG_VREF,0x0a);
	#else
        wrReg(0x32,0xF6);        // HREF was b6 F6 improves reliabilty
        wrReg(0x17,0x13);        // HSTART
        wrReg(0x18,0x01);        // HSTOP
        wrReg(0x19,0x02);        // VSTART
        wrReg(0x1a,0x7a);        // VSTOP
        wrReg(0x03,0x0a);        // VREF
	#endif
       // wrReg(0x70, 0x3a);       // Scaling Xsc
        //wrReg(0x71, 0x35);       // Scaling Ysc
        //wrReg(0xA2, 0x02);       // pixel clock delay

        wrReg(REG_COM1, 0x00);
  // COLOR SETTING

    wrReg(REG_COM8,0x8F);        // AGC AWB AEC bading filter off
    wrReg(0xAA,0x14);            // Average-based AEC algorithm
    wrReg(REG_BRIGHT,0x00);      // 0x00(Brightness 0) - 0x18(Brightness +1) - 0x98(Brightness -1)
    wrReg(REG_CONTRAS,0x40);     // 0x40(Contrast 0) - 0x50(Contrast +1) - 0x38(Contrast -1)
   // wrReg(0xB1,0xB1);            // Automatic Black level Calibration
    wrReg(0xB1,4);//really enable auto black level calibration
    wrReg(MTX1,0x80);
    wrReg(MTX2,0x80);
    wrReg(MTX3,0x00);
    wrReg(MTX4,0x22);
    wrReg(MTX5,0x5e);
    wrReg(MTX6,0x80);
    wrReg(MTXS,0x9e);
    wrReg(AWBC7,0x88);
    wrReg(AWBC8,0x88);
    wrReg(AWBC9,0x44);
    wrReg(AWBC10,0x67);
    wrReg(AWBC11,0x49);
    wrReg(AWBC12,0x0e);
	wrReg(REG_GFIX,0x00);
	//wrReg(GGAIN,0x40);
    wrReg(AWBCTR3,0x0a);
    wrReg(AWBCTR2,0x55);
    wrReg(AWBCTR1,0x11);
    wrReg(AWBCTR0,0x9f);

	wrReg(0xb0,0x84);//not sure what this does
	wrReg(REG_COM16,COM16_AWBGAIN);//disable auto denoise and edge enhancment
	//wrReg(REG_COM16,0);
	wrReg(0x4C,0);//disable denoise
	wrReg(0x76,0);//disable denoise
	wrReg(0x77,0);//disable denoise
	wrReg(0x7B,4);//brighten up shadows a bit end point 4
	wrReg(0x7C,8);//brighten up shadows a bit end point 8
	//wrReg(0x88,238);//darken highligts end point 176
	//wrReg(0x89,211);//try to get more highlight detail
	//wrReg(0x7A,60);//slope
	//wrReg(0x26,0xB4);//lower maxium stable operating range for AEC
	//hueSatMatrix(0,100);
	//ov7670_store_cmatrix();
	//wrReg(0x20,12);//set ADC range to 1.5x
	wrReg(REG_COM9,0x6A);//max gain to 128x
  wrReg(0x74,16);//disable digital gain
	wrReg(0x11,4);//using scaler for divider
        //cout << pstr("Free RAM: ") << FreeRam() << endl;
        //wrReg(0x31,0xFF);//HSYNC delay
	//wrReg(0x2A, 12);
        delay(100);//i2c uses interupts to write data wait for all bytes to be written
	cli();//disable interupts

        useSdCard;
        #ifndef useLed
    PgmPrintln("About to Start SD card");
    #else
    PORTD|=1;//turn on led
    #endif
        if (!sd.begin(chipSelect, SPI_FULL_SPEED))
	  sd.initErrorHalt("Error starting Sd card");
	//enable spi
	SPCR=80;//spi enable master
	SPSR=1;//double speed
  /*PgmPrintln("------sd1 root-------");

  sd.ls();*/
      /*	//enable spi
	SPCR=80;//spi enable master
	SPSR=1;//double speed*/
	spiCSt();
	spiWrB(1);
	spiWrB(64);//sequental mode
	spiCSt();
	spiWrB(2);//sequental write mode
	spiWrB(0);//24 bit address
	spiWrB(0);
	spiWrB(0);
       // useSDcard();
       #ifndef useLed
          PgmPrintln("Ready to Begin");
      #endif
        //sd.ls();
        //uint8_t* buf = (uint8_t*)sd.vol()->cacheClear();
        //while (1){}
}
void loop()
{
  /* Note: One frame is broken down into 5 smaller captures this allows for a whole frame to be captured at 640x480 which is nice
  however it could mean that there could be slight differences between each frame capture
  I think the improvment of resolution outweights the cons of the slight changes per frame if any
  The image will take about 15 seconds to reach the computer per frame*/
  static uint16_t countFrames;
  #ifndef useLed
  PgmPrintln("Send One key to start image save");
  RdSerial();

  for (uint16_t z=0;z<4;z++)
  {
      #else//do not use loop when using led
      if ((PIND&2)==0)//is the button on pin 1 pressed?
      {
       //if so blink led quickly
       sei();
       while (1)
       {
        PORTD^=1;
        delay(50);
       }
      }
      else
        PORTD&=~1;//led on when writing to files off when taking picture
      #endif
  //memset(buf,0,sizeof(buf));
  sprintf_P((char *)buf2,PSTR("f%d.yuv"),countFrames);
  #ifndef useLed
  PgmPrintln("Saving to:");
  StringRam((char *)buf2);
  serialWrB('\n');
  #else
  PORTD|=1;
  #endif
  sd.remove((char*)buf2);
    // create a contiguous file
  if (!file.createContiguous(sd.vwd(),(char *) buf2, 307200)) {
    error("createContiguous failed");
  }
  #ifndef useLed
  PgmPrintln("File created");
  #endif
  // get the location of the file's blocks

  if (!file.contiguousRange(&bgnBlock, &endBlock)) {
    error("contiguousRange failed");
  }
    // tell card to setup for multiple block write with pre-erase

  if (!sd.card()->erase(bgnBlock, endBlock)) error("card.erase failed");
  if (!sd.card()->writeStart(bgnBlock, 600)) {
    error("writeStart failed");
  }//1200 is the block count filesize/512
    // PgmPrintln("File prepared");
  //PgmPrintln("About to start Image Capture");
  captureImg(0,0,640,160);
  saveImg(200);
 captureImg(640,160,640,160);
  saveImg(200);
   captureImg(640,320,640,160);
  saveImg(200);

  /*file.sync();
  file.rewind();
  readImgSerial(1280,480);*/
  if (!sd.card()->writeStop()) error("writeStop failed");
  if (file.close() == 0) {
    error("File close failed");
  }
  countFrames++;
    #ifndef useLed
      PgmPrintln("File closed");
  }
  #endif

}
void fixPat(void)
{
  	spiCSt();
	spiWrB(2);//sequental write mode
	spiWrB(0);//24 bit address
	spiWrB(0);
	spiWrB(0);
	uint32_t x;
	uint8_t y=0;

	for (x=0;x<122880;x++)
	{
		SPDR=y;
		y++;
		while(!(SPSR & (1<<SPIF))) {}
	}
}
byte wrReg(int regID, int regDat)
{
     //   Serial.println("Started");
	Wire.beginTransmission(sensor_addr >> 1);
     //    Serial.println("Began");
	Wire.write(regID & 0x00FF);
	Wire.write(regDat & 0x00FF);
	if(Wire.endTransmission())
	{
		return 0;
                PORTB|=32;
                while (1) {}
	}
   //     Serial.println("Done!");
	delay(1);
  	return(1);
}

void readImgSerial(uint16_t w,uint16_t h)
{
	//_delay_ms(2000);
	uint16_t wl,hl;
	for (hl=0;hl<h;hl++)
	{
		StringPgm(PSTR("RDY"));
		for (wl=0;wl<w;wl++)
		{
			while ( !( UCSR0A & (1<<UDRE0)) ) {} //wait for byte to transmit
			//SPDR=0;//send dummy value to get byte back
			//while(!(SPSR & (1<<SPIF))) {}
			UDR0=file.read();
		}
	}
	while ( !( UCSR0A & (1<<UDRE0)) ) {} //wait for byte to transmit
}

byte rdSensorReg8_8(uint8_t regID, uint8_t* regDat)
{
	Wire.beginTransmission(sensor_addr >> 1);
	Wire.write(regID & 0x00FF);
	Wire.endTransmission();
	Wire.requestFrom((sensor_addr >> 1),1);
	if(Wire.available())
		*regDat = Wire.read();
	delay(1);
  	return(1);
}
void captureImg(uint16_t ws,uint16_t hs,uint16_t wg,uint8_t hg)
{//TODO: speed up this loop it misses one byte partway through the first line
//right now I am correcting this in the image converter software it is just one byte and not very
//noticable but it would still be nice to fix in my opion I do not have this issue when this loop is compiled with avr-gcc with a setting of -O2
  #ifdef useLed
  PORTD&=~1;//turn off led taking picture
  #endif
  	uint16_t ls2,lg2;
	//skip 1 multiplies skip 2 same with get1 and get2
	//first wait for vsync it is on pin 3 (counting from 0) portD
	//start spi ram
  cli();//make sure interupts are off
	spiCSt();
	spiWrB(2);//sequental write mode
	spiWrB(0);//24 bit address
	spiWrB(0);
	spiWrB(0);
	while (!(PIND&8)) {}//wait for high
	while ((PIND&8)) {}//wait for low
	if (hs != 0)
	{
		//for (ls1=0;ls1<hs;ls1++)
		while (hs--)
		{
			//for (ls2=0;ls2<ws;ls2++)
			ls2=ws;
			while (ls2--)
			{
				while (!(PIND&4)) {}//wait for high
				while ((PIND&4)) {}//wait for low
			}
		}// while (--hs);
	}
	//for (lg1=0;lg1<hg;lg1++)
	while ((uint8_t)hg--)
	{
		//for (lg2=0;lg2<wg;lg2++)
		lg2=wg;
		while (lg2--)
		{
			while (!(PIND&4)) {}//wait for high
			SPDR=(uint8_t)(PINC&15)|(PIND&240);
			while ((PIND&4)) {}//wait for low
		}
	}
}
void sendRam(uint16_t w,uint16_t h)
{
	spiCSt();
	spiWrB(3);//sequental read mode
	spiWrB(0);//24 bit address
	spiWrB(0);
	spiWrB(0);
	//_delay_ms(2000);
	uint16_t wl,hl;
	for (hl=0;hl<h;hl++)
	{
		StringPgm(PSTR("RDY"));
		for (wl=0;wl<w;wl++)
		{
			while ( !( UCSR0A & (1<<UDRE0)) ) {} //wait for byte to transmit
			SPDR=0;//send dummy value to get byte back
			while(!(SPSR & (1<<SPIF))) {}
			UDR0=SPDR;
		}
	}
	while ( !( UCSR0A & (1<<UDRE0)) ) {} //wait for byte to transmit
	//StringPgm(PSTR("RDY"));
	//_delay_ms(10);
	//UDR0=0xFF;
}
void spiCSt(void)
{
	//toggles spi CS used for reseting sram
	PORTB|=6;//cs high
	//_delay_ms(1);
	PORTB&=~4;//cs low
}
void spiWrB(uint8_t dat)
{
	SPDR = dat;
	// Wait for transmission complete
	while(!(SPSR & (1<<SPIF))) {}
}
uint8_t RdSerial(void)
{
	/* Wait for data to be received */
	while ( !(UCSR0A & (1<<RXC0)) );
	/* Get and return received data from buffer */
	return UDR0;
}
void saveImg(uint16_t sizeSect)
{
  //size is size in bytes/512 so for 1280x96 bytes it is 240
  //PgmPrintln("Saving part");
    #ifdef useLed
  PORTD|=1;//turn on led saving picture
  #endif
  uint32_t address=0;
  //PgmPrintln("About to Save Image");
  for (uint16_t x=0;x<sizeSect;x++)
  {
  spiCSt();
  spiWrB(3);//sequental read mode
  sramAddress(address);
  for (uint16_t y=0;y<512;y++)
  {
    SPDR=0;//send dummy value to get byte back
    while(!(SPSR & (1<<SPIF))) {}
    buf2[y]=SPDR;
  }
  useSdCard;
  if (!sd.card()->writeData(buf2)) error("writeData failed");
  address+=sizeof(buf2);
  //PgmPrintln("Done B");
  }
  //PgmPrintln("Done");
}
void sramAddress(uint32_t address)
{
  spiWrB((uint8_t)(address >> 16) & 0xff);
  spiWrB((uint8_t)(address >> 8) & 0xff);
  spiWrB((uint8_t)address);
}
void serialWrB(uint8_t dat)
{
        while ( !( UCSR0A & (1<<UDRE0)) ) {}
	UDR0=dat;
	while ( !( UCSR0A & (1<<UDRE0)) ) {} //wait for byte to transmit
}
void StringPgm(char * str)
{
	do {
		serialWrB(pgm_read_byte_near(str));
	} while(pgm_read_byte_near(++str));
}
void StringRam(char * str)
{
	do {
		serialWrB(*str);
	} while(*++str);
}