Hard&Soft 2013, Timișoara 2, Mbed Camera Code

common.hpp

#define PIN_CAMERA_XCLK p21
#define PIN_XBEE_TX p28
#define PIN_XBEE_RX p27
#define PIN_CAMERA_SIOD p22
#define PIN_CAMERA_SIOC p23
#define PIN_CAMERA_PCLK p8
#define PIN_CAMERA_P2 p11
#define PIN_CAMERA_P3 p12
#define PIN_CAMERA_P4 p13
#define PIN_CAMERA_P5 p14
#define PIN_CAMERA_P6 p15
#define PIN_CAMERA_P7 p16
#define PIN_CAMERA_P8 p17
#define PIN_CAMERA_P9 p18
#define PIN_CAMERA_HREF p5
#define PIN_CAMERA_VSYNC p6

FastPWM.cpp

#include "FastPWM.h"

FastPWM::FastPWM(PinName pin) : PWMObject(pin){
    //Set clock source
    LPC_SC->PCLKSEL0|=1<<12;
    
    _duty=0;
    _period=0.02;
    if (pin==p26||pin==LED1) {
        PWMUnit=1;
        MR=&LPC_PWM1->MR1;
        }
    else if (pin==p25||pin==LED2){
        PWMUnit=2;
        MR=&LPC_PWM1->MR2;
        }
    else if (pin==p24||pin==LED3){
        PWMUnit=3;
        MR=&LPC_PWM1->MR3;
        }
    else if (pin==p23||pin==LED4){
        PWMUnit=4;
        MR=&LPC_PWM1->MR4;
        }
    else if (pin==p22){
        PWMUnit=5;
        MR=&LPC_PWM1->MR5;
        }
    else if (pin==p21){
        PWMUnit=6;
        MR=&LPC_PWM1->MR6;
        }
    else
        error("No hardware PWM pin\n\r");
    
    period(_period);
}

void FastPWM::period(double seconds) {
    LPC_PWM1->MR0 = (unsigned int) (seconds * (double)F_CLK);
    LPC_PWM1->LER |= 1;
    _period = seconds;
    pulsewidth(_duty*_period);
}

void FastPWM::period_ms(int ms) {
    period((double)ms/1000.0);
}

void FastPWM::period_us(int us) {
    period((double)us/1000000.0);
}

void FastPWM::period_us(double us) {
    period(us/1000000.0);
}

void FastPWM::pulsewidth(double seconds) {
    *MR=(unsigned int) (seconds * (double)F_CLK);
    LPC_PWM1->LER |= 1<<PWMUnit;
    _duty=seconds/_period;
}

void FastPWM::pulsewidth_ms(int ms) {
    pulsewidth((double)ms/1000.0);
}

void FastPWM::pulsewidth_us(int us) {
    pulsewidth((double)us/1000000.0);
}

void FastPWM::pulsewidth_us(double us) {
    pulsewidth(us/1000000.0);
}

void FastPWM::write(double duty) {
    _duty=duty;
    pulsewidth(duty*_period);
}

double FastPWM::read( void ) {
    return _duty;
    }
    
FastPWM & FastPWM::operator= (double value) {
    write(value);
    return(*this);
    }
    
FastPWM::operator double() {
    return _duty;
}

FastPWM.h

/*
    .---.           _....._
   /  p  `\     .-""`:     :`"-.
   |__   - |  ,'     .     '    ',
    ._>    \ /:      :     ;      :,
     '-.    '\`.     .     :     '  \
        `.   | .'._.' '._.' '._.'.  |
          `;-\.   :     :     '   '/,__,
          .-'`'._ '     .     : _.'.__.'
         ((((-'/ `";--..:..--;"` \
             .'   /           \   \
       jgs  ((((-'           ((((-'
       
Yeah ASCII art turtle more fun than copyright stuff
*/


#include "mbed.h"


#ifndef FASTPWM_H
#define FASTPWM_H

#ifndef F_CLK
#define F_CLK   96000000
#endif

/** Library that allows faster and/or higher resolution PWM output
  *
  * Library can directly replace standard mbed PWM library. Only limitation is that the maximum PWM period is four times shorter
  * The maximum achievable period is roughly 40 seconds, I dont think that should be a problem.
  * Do take into account all PWM objects will run four times faster than default.
  *
  * Contrary to the default mbed library, this library takes doubles instead of floats. The compiler will autocast if needed,
  * but do take into account it is done for a reason, your accuracy will otherwise be limitted by the floating point precision.
  *
  * In your program you can define F_CLK if you use a different clock frequency than the default one. 
  *
  * Only works on LPC1768 for now. If you want support for the other one, send a PM and I will have a look, but I cannot even compile for it.
  */
class FastPWM {
public:
    /**
    * Create a FastPWM object connected to the specified pin
    *
    * @param pin - PWM pin to connect to
    */
    FastPWM(PinName pin);
    
    /**
    * Set the PWM period, specified in seconds (double), keeping the duty cycle the same.
    */
    void period(double seconds);
    
    /**
    * Set the PWM period, specified in milli-seconds (int), keeping the duty cycle the same.
    */
    void period_ms(int ms);
    
    /**
    * Set the PWM period, specified in micro-seconds (int), keeping the duty cycle the same.
    */
    void period_us(int us);
    
    /**
    * Set the PWM period, specified in micro-seconds (double), keeping the duty cycle the same.
    */
    void period_us(double us);
    
    /**
    * Set the PWM pulsewidth, specified in seconds (double), keeping the period the same.
    */
    void pulsewidth(double seconds);
    
    /**
    * Set the PWM pulsewidth, specified in milli-seconds (int), keeping the period the same.
    */
    void pulsewidth_ms(int ms);
    
    /**
    * Set the PWM pulsewidth, specified in micro-seconds (int), keeping the period the same.
    */
    void pulsewidth_us(int us);
    
    /**
    * Set the PWM pulsewidth, specified in micro-seconds (double), keeping the period the same.
    */
    void pulsewidth_us(double us);
    
    /**
    * Set the ouput duty-cycle, specified as a percentage (double)
    *
    * @param duty - A double value representing the output duty-cycle, specified as a percentage.  The value should lie between 0.0 (representing on 0%) and 1.0 (representing on 100%).
    */
    void write(double duty);
    
    /**
    * Return the ouput duty-cycle, specified as a percentage (double)
    *
    * @param return - A double value representing the output duty-cycle, specified as a percentage.
    */
    double read( void );
    
    /**
    * An operator shorthand for write()
    */
    FastPWM& operator= (double value);
    
    /**
    * An operator shorthand for read()
    */
    operator double();
    
private:
    PwmOut PWMObject;
    double _duty;
    double _period;
    unsigned int PWMUnit;
    
    __IO uint32_t *MR;

};
#endif

main.cpp

#include "mbed.h"
#include "FastPWM.h"
#include "OV9655.hpp"
#include "common.hpp"

DigitalOut myled(LED1);

DigitalOut testpin(p22);
FastPWM cameraClock(PIN_CAMERA_XCLK);
Serial pc(PIN_XBEE_TX, PIN_XBEE_RX);
OV9655Cam myCam(PIN_CAMERA_SIOD, PIN_CAMERA_SIOC);
InterruptIn camPCLK(PIN_CAMERA_PCLK);
InterruptIn camHREF(PIN_CAMERA_HREF);
InterruptIn camVSYNC(PIN_CAMERA_VSYNC);

//PortIn cameraData(Port0,0x07878000);
BusIn cameraData(PIN_CAMERA_P2,PIN_CAMERA_P3,PIN_CAMERA_P4,PIN_CAMERA_P5,PIN_CAMERA_P6,PIN_CAMERA_P7,PIN_CAMERA_P8,PIN_CAMERA_P9);

#define LINES_STORED 80
#define LINE_SIZE 320
#define BUFFER_SIZE 25601


char dataStorage[BUFFER_SIZE];
int linesLength[LINES_STORED];
int u8LineStoreIndex = 0;
int u8LineSendIndex = 0;
int u32StoreIndex = 0;
int u32SendIndex = 0;
int startOfLine = 3;
int endOfLine = 1;

bool isPair = true;
int endOfLastImage = 0;
bool endOfImage = false;
void cameraReceiveInterrupt(void);

void vsyncCallbackRise(void)
{
 //  pc.printf("%d", u8LineSendIndex);
   endOfLastImage = u8LineStoreIndex;
   endOfImage = true;
   u8LineStoreIndex = 0;
   u32StoreIndex = 0;
}
void hrefCallbackRise(void)
{
if (isPair){
    isPair = false;
      linesLength[(u8LineStoreIndex%LINES_STORED)]= u32StoreIndex;
     u32StoreIndex = 0;
    u8LineStoreIndex++;
   camPCLK.rise(&cameraReceiveInterrupt);
 
   }
   else
   {
    isPair = true;
   }
   
   
}
void hrefCallbackFall(void)
{
camPCLK.rise(0);
 
  
  
}

void cameraReceiveInterrupt(void)
{
  dataStorage[(u8LineStoreIndex%LINES_STORED)*LINE_SIZE + u32StoreIndex] = (char) cameraData.read() ;//receiveByte();
 // pc.putc(cameraData.read());
  u32StoreIndex++;   
}

void init ()
{
    pc.baud(115200);
    //initPLL();
    //  Start OminiVision XCLK driver
    cameraClock.period(1.0/10000000);
    cameraClock.write(0.5);
   // pc.printf("Merge legatura!\r\n");
    if(myCam.WriteRegister(COM7, SCCB_REG_RESET) == true) {
       
        //pc.printf("Am resetat cu succes camera.\r\n");
        wait_ms(10);
        myCam.QQVGASizeSetup();
      //  pc.printf("%d",ret);
         
        if(myCam.WriteRegister(CLKRC, 0x1f) != true) {
            pc.printf("error writing clock");
        }
      //  pc.printf("Val. registru %x: %x\r\n", i, myCam.ReadRegister(COM11));
    }
    camVSYNC.rise(&vsyncCallbackRise);
    camHREF.rise(&hrefCallbackRise);
    camHREF.fall(&hrefCallbackFall);
}
int main() {
   char byteToSend;
   int tmpShort;
   int red,green,blue;
    
   init();
    while(1) {
        if (u8LineStoreIndex == u8LineSendIndex)
        {
            
        }
        else if (u32SendIndex ==-1)
            {
               if (pc.writeable())
               {
                    switch (startOfLine)
                    {
                       case 5: pc.putc(0); break;
                       case 4: pc.putc(255); break;
                       case 1: pc.putc(0);break;
                       case 2: pc.putc(255);break;
                       case 3: if (u8LineSendIndex == 0 | u8LineSendIndex == 1)
                                pc.putc(1);
                               else
                                pc.putc(0);
                                break;
                    }
                    startOfLine--;
                    if (startOfLine <=0)
                    {
                       startOfLine =5;
                       u32SendIndex = 0;
                    }
                    
                }
            }
        else if (linesLength[u8LineSendIndex%LINES_STORED]-1 > u32SendIndex)
            {
               //prepare next byte to send
               tmpShort = (dataStorage[(u8LineSendIndex%LINES_STORED)*LINE_SIZE+u32SendIndex]<<8)|dataStorage[(u8LineSendIndex%LINES_STORED)*LINE_SIZE+u32SendIndex+1];
               red=  (tmpShort& 0xf800)>>11;
               green =  (tmpShort&0x07D0)>>5;
               blue =  tmpShort&0x001F;
                   byteToSend =(char) ((((int)red)*3.0/10+((int)green)*6.0/20+((int)blue)/10.0)*255/31);
               // byteToSend = (char) ( red*2 +green+blue*2)*255/187;
                if (pc.writeable())
               {
                    pc.putc(byteToSend);
                    u32SendIndex+=2;
               }            
            }
         else 
         {
            u32SendIndex = -1;
            linesLength[u8LineSendIndex%LINES_STORED] = 0;
            u8LineSendIndex++;  
         }
         //next line
        if ( (endOfImage == true)&&(u8LineSendIndex == endOfLastImage) )
        {
            endOfImage =false;
            endOfLastImage =0;
            u8LineSendIndex = 0;
            u32SendIndex = -1;
        }
    }
}

OV9655.cpp

#include "OV9655.hpp"

OV9655Cam::OV9655Cam(const PinName &siod, const PinName &sioc) 
  : m_ipWriteAddress(0x60), m_ipReadAddress(0x61) {
  m_sioc = new DigitalOut(sioc);
  m_siod = new DigitalInOut(siod);
  
  // Initialize the lines
  m_siod->output();
  m_siod->write(1);
  m_sioc->write(1);
  wait_us(5);
}
bool OV9655Cam::QVGASetup(){

  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x03, 0x02);//diff
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x09, 0x03);//diff
 
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x0f, 0xc0);//diff bun
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x10, 0x50);//diff
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x12, 0x63);//diff bun
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x13, 0xef);//diff
  
//  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x14, 0x0A);//automatic gain skilling diff
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x15, 0x10); //No PCLK output, HREF reverse 18
  
//  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x16, 0x24);//gain
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x17, 0x18);//HSTART diferit
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x18, 0x04);//HSTOP dif
  
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x1a, 0x81);//vstop dif
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x1e, 0x00); /*0x20*/ //pentru mirror, flip
                       
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x24, 0x3c);//AGC
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x25, 0x36);
                           
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x26, 0x72);
                               
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x27, 0x08);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x28, 0x08);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x29, 0x15);
 
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x2c, 0x08);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x32, 0x12);
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x36, 0x3a);//aref3 default f9
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x38, 0x72);//adc ref cont
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x39, 0x57);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x3a, 0xca);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x3b, 0x04);
  
  
  //wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x3e, 0x02); zoom function on
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x3f, 0xc1);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x40, 0xd0);//rgb 565
  
  //wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x41, 0x41);scaling down 
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x42, 0xc0);
 /* wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x4f, 0x98);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x50, 0x98);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x51, 0x00);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x52, 0x28);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x53, 0x70);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x54, 0x98);
  */
  //wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x58, 0x1a);
 /* 
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x59, 0x85);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x5a, 0xa9);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x5b, 0x64);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x5c, 0x84);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x5d, 0x53);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x5e, 0x0e);*///AWB
  
  /*wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x62, 0x00);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x63, 0x00);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x64, 0x02);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x65, 0x20);*///lens correction
  //wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x6b, 0x0a);
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x70, 0x21);
  //wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x72, 0x11); 
  //important
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x74, 0x10); 
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x75, 0x10); 
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0x8c, 0x80);
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0xa1, 0x1f);
  
//  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0xa4, 0x50);
 wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0xa8, 0xc1);
  
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0xa9, 0xef);
  wait_ms(10);this->WriteRegister((OV9655RegisterAddresses)0xc7, 0x81);
  return true;
  }
  
  void OV9655Cam::QQVGASizeSetup(void)
{

   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x04, 0x03);//AEC low 11
   
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x0f, 0x40);//manually update VSTART si restul
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x12, 0x64);//YUV 62
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x13, 0xc7);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x14, 0x3a);
  
 // this->WriteRegister((OV9655RegisterAddresses)0x16, 0x24);//dummy frame
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x17, 0x18);//HSTART
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x18, 0x04);//HSTOP
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x19, 0x01);//VSTRT
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x1a, 0x81);//VSTOP
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x1e, 0x20);//mirror flip
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x24, 0x3c);//operating region
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x25, 0x36);//operating region
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x26, 0x72);//fast mode op reg
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x27, 0x08);//fast mode op reg
   

  this->WriteRegister((OV9655RegisterAddresses)0x36, 0x3a);//aref3
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x38, 0x72);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x39, 0x57);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x3a, 0xcc);
   wait_ms(10);
  
  this->WriteRegister((OV9655RegisterAddresses)0x3e, 0x0e); //white pixel corr black pixel corr
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x3f, 0xc1);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x40, 0xd0);//rgb565
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x41, 0x41);//scaling on
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x42, 0xc8);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x43, 0x0a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x44, 0xf0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x45, 0x46);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x46, 0x62);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x47, 0x2a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x48, 0x3c);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4a, 0xfc);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4b, 0xfc);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4c, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4d, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4e, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x4f, 0x98);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x50, 0x98);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x51, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x52, 0x28);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x53, 0x70);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x54, 0x98);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x58, 0x1a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x59, 0x85);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5a, 0xa9);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5b, 0x64);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5c, 0x84);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5d, 0x53);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5e, 0x0e);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x5f, 0xf0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x60, 0xf0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x61, 0xf0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x62, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x63, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x64, 0x02);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x65, 0x20);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x66, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x69, 0x0a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x6b, 0x1a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x6c, 0x04);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x6d, 0x55);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x6e, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x6f, 0x9d);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x70, 0x21);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x71, 0x78);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x72, 0x22); 
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x73, 0x01);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x74, 0x10);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x75, 0x10); 
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x76, 0x01);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x77, 0x02);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7A, 0x12);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7B, 0x08);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7C, 0x16);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7D, 0x30);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7E, 0x5e);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x7F, 0x72);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x80, 0x82);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x81, 0x8e);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x82, 0x9a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x83, 0xa4);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x84, 0xac);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x85, 0xb8);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x86, 0xc3);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x87, 0xd6);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x88, 0xe6);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x89, 0xf2);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x8a, 0x24);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x8c, 0x80);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x90, 0x7d);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x91, 0x7b);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x9d, 0x02);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x9e, 0x02);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0x9f, 0x7a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa0, 0x79);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa1, 0x40);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa4, 0x50);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa5, 0x68);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa6, 0x4a);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa8, 0xc1);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xa9, 0xef);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xaa, 0x92);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xab, 0x04);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xac, 0x80);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xad, 0x80);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xae, 0x80);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xaf, 0x80);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb2, 0xf2);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb3, 0x20);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb4, 0x20);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb5, 0x00);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb6, 0xaf);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xb6, 0xaf);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbb, 0xae);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbc, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbd, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbe, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbf, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xbf, 0x7f);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc0, 0xaa);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc1, 0xc0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc2, 0x01);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc3, 0x4e);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc6, 0x05);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc7, 0x82);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xc9, 0xe0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xca, 0xe8);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xcb, 0xf0);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xcc, 0xd8);
   wait_ms(10);
  this->WriteRegister((OV9655RegisterAddresses)0xcd, 0x93);
   wait_ms(10);
}
OV9655Cam::~OV9655Cam() { }

bool OV9655Cam::WriteRegister(OV9655RegisterAddresses regAddress,uint8_t regValue) {
  bool stOperatie = true;
  
  GenerateStartCondition();
  
  if (GenerateTransmitPhase(m_ipWriteAddress) == false) {
    // First phase - IP Address
    stOperatie = false;
  } else if (GenerateTransmitPhase((uint8_t)(regAddress)) == false) {
    // Second phase - Register Address
    stOperatie = false;
  } else if (GenerateTransmitPhase(regValue) == false) {
    // Third phase - Register Value
    stOperatie = false;
  }
  
  GenerateStopCondition();
  return stOperatie;
}

uint8_t OV9655Cam::ReadRegister(OV9655RegisterAddresses regAddress) {
  uint8_t regValue;
  
  // First we generate a 2-Phase write transmission cycle (3.2.1.2)
  GenerateStartCondition();
  // Phase 1 - IP Address
  GenerateTransmitPhase(m_ipWriteAddress);
  // Phase 2 - register address
  GenerateTransmitPhase((uint8_t)(regAddress));
  GenerateStopCondition();
  
  // Then we issue a 2-Phase read transmission cycle (3.2.1.3)
  GenerateStartCondition();
  // Phase 1 - IP Address
  GenerateTransmitPhase(m_ipReadAddress);
  // Phase 2 - read data
  regValue = GenerateReceivePhase();
  GenerateStopCondition();
  
  return regValue;
}

bool OV9655Cam::GenerateTransmitPhase(uint8_t data) {
  uint8_t mask = 0x80;
  bool stareOp = true;
  int i;
  
  // Send the actual data in a MSB manner
  m_siod->output();
  for (i = 0; i < 8; i++) {
    if((mask & data) != 0) {
      m_siod->write(1);
    } else {
      m_siod->write(0);
    }
    data = data << 1;
    wait_us(1);
    m_sioc->write(1);
    wait_us(1);
    m_sioc->write(0);
    wait_us(1);
  }
  
  // send "Don't Care" (DC) bit 
  m_siod->input();
  
  if(m_siod->read() != 0) {
    stareOp = false;
  } else {
    stareOp = true;
  }
  
  m_sioc->write(1);
  wait_us(1);
  m_sioc->write(0);
  wait_us(1);
  m_siod->output();
  
  return stareOp;
}

uint8_t OV9655Cam::GenerateReceivePhase() {
  uint8_t readVal = 0x00;
  int i;
  
  // Send the actual data in a MSB manner
  m_siod->input();
  wait_us(3);
  for (i = 0; i < 8; i++) {
    m_sioc->write(1);
    wait_us(1);
    readVal = readVal << 1;
    if(m_siod->read() != 0) {
      readVal |= 0x01;
    } else {
      readVal |= 0x00;
    }
    wait_us(1);
    m_sioc->write(0);
    wait_us(1);
  }
  
  // send "Don't Care" (DC) bit
  m_siod->output();
  m_siod->write(1);
  wait_us(1);
  m_sioc->write(1);
  wait_us(1);
  m_sioc->write(0);

  return readVal;
}

void OV9655Cam::GenerateStartCondition() {
  m_siod->output();
  
  m_siod->write(1);
  m_sioc->write(1);
  wait_us(1);
  
  m_siod->write(0);
  wait_us(1);
  m_sioc->write(0);
  wait_us(1);
}

void OV9655Cam::GenerateStopCondition() {
  m_siod->output();        
  m_sioc->write(1);
  wait_us(1);
  m_siod->write(1);
}

OV9655.hpp

#ifndef __OV9655_h__
#define __OV9655_h__

#include <cstdint>
#include "mbed.h"

#define DCMI_DR_ADDRESS     0x50050028
#define FSMC_LCD_ADDRESS    /*0x40011000 | 0x04*/0x6F010000

/* Exported types ------------------------------------------------------------*/
typedef struct
{
  uint8_t Manufacturer_ID1;
  uint8_t Manufacturer_ID2;
  uint8_t Version;
  uint8_t PID; 
} OV9655_IDTypeDef;  

/* Exported constants --------------------------------------------------------*/

// Camera resolution
//#define QQVGA_SIZE  /*160x120 */
#define QVGA_SIZE  /* 320x240 */
                                 
// OV9655 Registers definition
enum OV9655RegisterAddresses {
  GAIN = 0x00,
  BLUE = 0x01,
  RED = 0x02,
  VREF = 0x03,
  COM1 = 0x04,
  BAVE = 0x05,
  GbAVE = 0x06,
  GrAVE = 0x07,
  RAVE = 0x08,
  COM2 = 0x09,
  PID = 0x0A,
  VER = 0x0B,
  COM3 = 0x0C,
  COM4 = 0x0D,
  COM5 = 0x0E,
  COM6 = 0x0F,
  AEC = 0x10,
  CLKRC = 0x11, 
  COM7 = 0x12, 
  COM8 = 0x13,
  COM9 = 0x14, 
  COM10 = 0x15, 
  REG16 = 0x16, 
  HSTART = 0x17, 
  HSTOP = 0x18, 
  VSTART = 0x19, 
  VSTOP = 0x1A, 
  PSHFT = 0x1B,
  MIDH = 0x1C, 
  MIDL = 0x1D, 
  MVFP = 0x1E,
  BOS = 0x20,
  GBOS = 0x21,
  GROS = 0x22,
  ROS = 0x23,
  AEW = 0x24,
  AEB = 0x25,
  VPT = 0x26,
  BBIAS = 0x27,
  GbBIAS = 0x28,
  PREGAIN = 0x29,
  EXHCH = 0x2A,
  EXHCL = 0x2B, 
  RBIAS = 0x2C,
  ADVFL = 0x2D,
  ADVFH = 0x2E,
  YAVE = 0x2F,
  HSYST = 0x30,
  HSYEN = 0x31,
  HREF = 0x32,
  CHLF = 0x33,
  AREF1 = 0x34,
  AREF2 = 0x35,
  AREF3 = 0x36,
  ADC1 = 0x37,
  ADC2 = 0x38,
  AREF4 = 0x39,
  TSLB = 0x3A,
  COM11 = 0x3B,  
  COM12 = 0x3C, 
  COM13 = 0x3D, 
  COM14 = 0x3E, 
  EDGE = 0x3F,
  COM15 = 0x40, 
  COM16 = 0x41,
  COM17 = 0x42,
  MTX1 = 0x4F,
  MTX2 = 0x50,
  MTX3 = 0x51,
  MTX4 = 0x52,
  MTX5 = 0x53,
  MTX6 = 0x54,
  BRTN = 0x55,
  CNST1 = 0x56,
  CNST2 = 0x57,
  MTXS = 0x58,
  AWBOP1 = 0x59,
  AWBOP2 = 0x5A,
  AWBOP3 = 0x5B,
  AWBOP4 = 0x5C,
  AWBOP5 = 0x5D,
  AWBOP6 = 0x5E,
  BLMT = 0x5F,
  RLMT = 0x60,
  GLMT = 0x61,
  LCC1 = 0x62,
  LCC2 = 0x63,
  LCC3 = 0x64,
  LCC4 = 0x65,
  MANU = 0x66,
  MANV = 0x67,
  MANY = 0x68,
  VARO = 0x69,
  BD50MAX = 0x6A,
  DBLV =  0x6B,
  DNSTH = 0x70,
  POIDX = 0x72,
  PCKDV = 0x73,
  XINDX = 0x74,
  YINDX = 0x75,
  SLOP = 0x7A,
  GAM1 = 0x7B,
  GAM2 = 0x7C,
  GAM3 = 0x7D,
  GAM4 = 0x7E,
  GAM5 = 0x7F,
  GAM6 = 0x80,
  GAM7 = 0x81,
  GAM8 = 0x82,
  GAM9 = 0x83,
  GAM10 = 0x84,
  GAM11 = 0x85,
  GAM12 = 0x86,
  GAM13 = 0x87,
  GAM14 = 0x88,
  GAM15 = 0x89,
  COM18 = 0x8B,
  COM19 = 0x8C,
  COM20 = 0x8D,
  DMLNL = 0x92,
  DMLNH = 0x93,
  LCC6 = 0x9D,
  LCC7 = 0x9E,
  AECH = 0xA1,
  BD50 = 0xA2,
  BD60 = 0xA3,
  COM21 = 0xA4, 
  GREEN = 0xA6,
  VZST = 0xA7,
  REFA8 = 0xA8,
  REFA9 = 0xA9,
  BLC1 = 0xAC,
  BLC2 = 0xAD,
  BLC3 = 0xAE,
  BLC4 = 0xAF,
  BLC5 = 0xB0,
  BLC6 = 0xB1,
  BLC7 = 0xB2,
  BLC8 = 0xB3,
  CTRLB4 = 0xB4,
  FRSTL = 0xB7,
  FRSTH = 0xB8,
  ADBOFF = 0xBC,
  ADROFF = 0xBD,
  ADGbOFF = 0xBE,
  ADGrOFF = 0xBF,
  COM23 = 0xC4,
  BD60MAX = 0xC5, 
  COM24 = 0xC7
};

/* Registers bit definition */
/* COM1 Register */
#define CCIR656_FORMAT  0x40
#define HREF_SKIP_0     0x00
#define HREF_SKIP_1     0x04
#define HREF_SKIP_3     0x08

/* COM2 Register */
#define SOFT_SLEEP_MODE  0x10   
#define ODCAP_1x         0x00   
#define ODCAP_2x         0x01   
#define ODCAP_3x         0x02   
#define ODCAP_4x         0x03
    
/* COM3 Register */
#define COLOR_BAR_OUTPUT         0x80
#define OUTPUT_MSB_LAS_SWAP      0x40
#define PIN_REMAP_RESETB_EXPST   0x08 
#define RGB565_FORMAT            0x00 
#define RGB_OUTPUT_AVERAGE       0x04 
#define SINGLE_FRAME             0x01

/* COM5 Register */
#define SLAM_MODE_ENABLE      0x40
#define EXPOSURE_NORMAL_MODE  0x01

/* COM7 Register */
#define SCCB_REG_RESET                       0x80
#define FORMAT_CTRL_15fpsVGA                 0x00
#define FORMAT_CTRL_30fpsVGA_NoVArioPixel    0x50
#define FORMAT_CTRL_30fpsVGA_VArioPixel      0x60
#define OUTPUT_FORMAT_RAWRGB                 0x00
#define OUTPUT_FORMAT_RAWRGB_DATA            0x00
#define OUTPUT_FORMAT_RAWRGB_INTERP          0x01
#define OUTPUT_FORMAT_YUV                    0x02
#define OUTPUT_FORMAT_RGB                    0x03

/* COM9 Register */
#define GAIN_2x         0x00    
#define GAIN_4x         0x10    
#define GAIN_8x         0x20    
#define GAIN_16x        0x30    
#define GAIN_32x        0x40    
#define GAIN_64x        0x50    
#define GAIN_128x       0x60    
#define DROP_VSYNC      0x04    
#define DROP_HREF       0x02
    
/* COM10 Register */
#define RESETb_REMAP_SLHS    0x80
#define HREF_CHANGE_HSYNC    0x40
#define PCLK_ON              0x00
#define PCLK_OFF             0x20
#define PCLK_POLARITY_REV    0x10
#define HREF_POLARITY_REV    0x08
#define RESET_ENDPOINT       0x04
#define VSYNC_NEG            0x02
#define HSYNC_NEG            0x01

/* TSLB Register */
#define PCLK_DELAY_0         0x00
#define PCLK_DELAY_2         0x40
#define PCLK_DELAY_4         0x80
#define PCLK_DELAY_6         0xC0
#define OUTPUT_BITWISE_REV   0x20
#define UV_NORMAL            0x00
#define UV_FIXED             0x10
#define YUV_SEQ_YUYV         0x00
#define YUV_SEQ_YVYU         0x02
#define YUV_SEQ_VYUY         0x04
#define YUV_SEQ_UYVY         0x06
#define BANDING_FREQ_50      0x02

#define RGB_NORMAL   0x00 
#define RGB_565      0x10 
#define RGB_555      0x30 

class OV9655Cam {
  public:
    OV9655Cam(const PinName &siod, const PinName &sioc);
    ~OV9655Cam();
    bool QVGASetup();
    void QQVGASizeSetup(void);
    bool WriteRegister(OV9655RegisterAddresses regAddress,uint8_t regValue);
    uint8_t ReadRegister(OV9655RegisterAddresses regAddress);
  private:
    uint8_t m_ipWriteAddress;
    uint8_t m_ipReadAddress;
    DigitalOut *m_sioc;
    DigitalInOut *m_siod;
    
    void GenerateStartCondition();
    bool GenerateTransmitPhase(uint8_t data);
    uint8_t GenerateReceivePhase();
    void GenerateStopCondition();
};

#endif