File content as of revision 4:ed062dc75c52:

/* 
Toragi-CCD-Cam OV7670andFIFO sample use driver

Sadaei Osakabe ( http://mbed.org/users/diasea/ )
  http://mbed.org/users/diasea/code/OV7670_with_AL422B_Color_Size_test/

SDFileSystem
http://mbed.org/users/mbed_official/code/SDFileSystem/docs/tip/

mbed_debug.h
https://github.com/adamgreen/SmoothieTest/blob/master/mbed/src/capi/mbed_debug.h
*/
#include "mbed.h"
#include "SDFileSystem.h"
#include <string.h>
#include <math.h>
#include <algorithm>
#include "ov7670.h"


#define QQVGA
#ifdef QQVGA
# define SIZEX (160)
# define SIZEY (120)
#else
# define SIZEX (320)
# define SIZEY (240)
#endif

#define COLORS 8     // 8bit colors
//#define COLORS 24    // 24bit colors

int create_header(FILE *fp, int width, int height) ;

#define FILEHEADERSIZE 14                   //ファイルヘッダのサイズ
#define INFOHEADERSIZE 40                   //情報ヘッダのサイズ
#define HEADERSIZE (FILEHEADERSIZE+INFOHEADERSIZE)


OV7670 camera(
    p28,p27,       // SDA,SCL(I2C / SCCB)
    p25,p26,p29,   // VSYNC,HREF,WEN(FIFO)
    p24,p23,p22,p21,p20,p19,p18,p17, // D7-D0
    p5,p6,p7) ; // RRST,OE,RCLK

 

SDFileSystem sd(p11, p12, p13, p14, "sd"); // mosi, miso, sclk, cs

LocalFileSystem local("local");
BusOut myleds(LED1, LED2, LED3, LED4);
 
Serial pc(USBTX,USBRX) ;

int sizex = 0;
int sizey = 0;
int FileNo = 0;

#define AREA_X 32
#define AREA_Y 24
#define AREA_DIV_X 5
#define AREA_DIV_Y 5
//#define AREA_THRESH  12000
#define AREA_THRESH  5000


char canvas[SIZEX][SIZEY] ; //18.5KB  [0]gray [1]Median
signed short  AreaDiff[2][AREA_DIV_X][AREA_DIV_Y];   // 5x5 Area = 32x24dot Each Block
int face_bank = 0;
int  Dbmode = 0;    
void MedianFilter(void);
void SaveBMP(int flag);
void SearchNewFileNo();


int main() {
    char color_format = 0;
    char c;
    int d1,d2 ;
    //int mx, pixel , i;
    int pixel ;
    int pixc,  pixelg;
    int sort[3];    
    int ay = 0;
    //char UseMedian = 0;

    memset( AreaDiff, '\0', sizeof(AreaDiff));

    pc.baud(115200);
//RESJMP:
    //pc.printf("Camera resetting..\r\n");
    camera.Reset();

    //pc.printf("Before Init...\r\n");
    //camera.PrintRegister();
    switch( color_format ){
    case '1':   
        camera.InitRGB444(); 
        break;
    case '2':   
        camera.InitRGB555(); 
        break;
    default:
        camera.InitRGB565(); 
        break;
    }
    
    //sizex = 160;
    //sizey = 120;
    camera.InitQQVGA();

    camera.InitForFIFOWriteReset();
    camera.InitDefaultReg(true, true);  // flipv/flipH

    //pc.printf("After Init...\r\n");
    //camera.PrintRegister();
    SearchNewFileNo();


    // CAPTURE and SEND LOOP
    while(1)
    {
        memset( &AreaDiff[face_bank], '\0', sizeof(AreaDiff[face_bank]));
        if ( Dbmode ){
            pc.printf("Debug Mode [d][s]  Any Charactor is Capture Start!  \r\n") ;
            while(!pc.readable()) ;
            c = pc.getc() ;
            /*
            if (( c == '1' )|| ( c == '2' )||( c == '3' )){
                color_format = c;
                goto RESJMP;
            }else if ( ( c== 'm')||( c== 'M')) {
                    UseMedian = 1;
                    continue;
            }else if ( ( c== 'n')||( c== 'N')) {
                    UseMedian = 0;
                    continue;
            }else  
            */
            if (( c == 'd' )|| ( c == 'D' )){
                    Dbmode = 0;     // GO FreeRun
                    continue;
            }else if ( ( c== 's')||( c== 'S')) {
                    SaveBMP(COLORS);
                    continue;
            }
        }else{
            if ( pc.readable()) {
                c = pc.getc() ;
                if (( c == 'd' )|| ( c == 'D' )){
                    Dbmode = 1;     // GO DEBUGmode
                    continue;
                } 
                /*
                else if ( ( c== 'm')||( c== 'M')) {
                    UseMedian = 1;
                    continue;
                }else if ( ( c== 'n')||( c== 'N')) {
                    UseMedian = 0;
                    continue;
                }
                */
            }
            pc.printf(" \r\n") ;
        }
        camera.CaptureNext();   // sample start!
        while(camera.CaptureDone() == false);
        camera.ReadStart();
        pixc = 0;
        ay = 0;
        for (int y = 0;y < SIZEY;y++) {
            ay = y / AREA_Y;    
            for (int x = 0;x < SIZEX;x++) {
                d1 = camera.ReadOneByte() ; // upper nibble is XXX , lower nibble is B
                d2 = camera.ReadOneByte() ; // upper nibble is G   , lower nibble is R
                pixel = ((unsigned int)d2 << 8) + ((unsigned int)d1 & 0xff);
                
                // Make GrayScale
                // make 6bit Code on Each Color
                sort[0] = ((pixel >> 11) & 0x1f)<<1;    // r
                sort[1] = ((pixel >>  5) & 0x3f);       // g
                sort[2] = ((pixel      ) & 0x1f)<<1;    // b
                
                int max = 0, min = 0xffff;
                for (int i=0;i<3;i++){
                    if (sort[i] > max) max = sort[i];
                    if (sort[i] < min) min = sort[i];
                }
                int gray = ((max + min) / 2) ;  
                
                pixelg = ((gray>>1)<<11) + (gray<<5) + (gray>>1);
                pixel = pixelg;
                canvas[x][y] = (char)( gray ) & 0x3f;    // 6bit gray scale
                
                // Make Difference AreaData
                AreaDiff[face_bank][x/AREA_X][ay] += ( gray  );    // 6bit scaler! 
                
                if ( Dbmode ){
                    // send <CRLF> and Count on each 16dot.
                    if ( !(x%16) ){
                        pc.printf("\r\n%04x: ",pixc ) ;
                        pixc +=16;
                    }
                    pc.printf("%04x ",pixel) ;
                }
            }
        }
        camera.ReadStop();
        /*
        if ( UseMedian ){
            MedianFilter();            
        }
        */
        
        // Difference Check!!
        pc.printf("\r\n" ) ;
        
        int err_c=0;
        int backArea = ( face_bank + 1 )&0x01;
        for (int y = 0; y < AREA_DIV_Y; y++) {
            if ( Dbmode ) pc.printf("Diff: " ) ;

            for (int x = 0; x < AREA_DIV_X; x++) {
                int dif = abs( AreaDiff[backArea][x][y] - AreaDiff[face_bank][x][y] );
                if (dif >= AREA_THRESH) {
                    err_c++;        // Error Count

                    if ( Dbmode ) pc.printf("X%6d  " , dif) ;
                }else{
                    if ( Dbmode ) pc.printf("O%6d  " , dif) ;
                }
            }
            if ( Dbmode ) {
                pc.printf("                                                  " ) ;
                pc.printf("                                             \r\n" ) ;
            }
        }
        int led_s =0;
        switch ( err_c ){
        case 1:
            led_s =1;
            break;
        case 2:
            led_s =3;
            break;
        case 3:
            led_s =7;
            break;
        default:
            if ( err_c >= 3) {
                led_s =7;
            }else{
                led_s =0;
            }
            break;
        } 

        led_s |=  ( face_bank )? 0x08 : 0x00 ;
        myleds = led_s ;  
        if ( err_c >= 3 ){
            //pc.printf("\r\n") ;
            if ( Dbmode ){
                pixc = 0;
                for (int y = 0;y < SIZEY;y++) {
                    for (int x = 0;x < SIZEX;x++) {
                        char gray = 0;
                        gray = canvas[x][y];
                        pixel = ((gray>>1)<<11) + (gray<<5) + (gray>>1);
    
                        // send <CRLF> and Count on each 16dot.
                        if ( !(x%16) ){
                            pc.printf("\r\n%04x: ",pixc ) ;
                            pixc +=16;
                        }
                        pc.printf("%04x ",pixel) ;
                    }
                }
            }
            SaveBMP(COLORS);
            
        }        
        face_bank++;
        face_bank&=0x01;


        //pc.printf("\r\n") ;
    }
}

void MedianFilter(void)
{
    int sortM[9] = { 0 };
    int p;
    int ay;
    
    // Median Filter
    memset( &AreaDiff[face_bank], '\0', sizeof(AreaDiff[face_bank]));
    ay = 0;
    for (int y = 1;y < SIZEY-1;y++) {
        for (int x = 1;x < SIZEX-1;x++) {
            p=0;

            // Get Around Pixels
            for (int k = -1; k < 2; k++) {
                for (int j = -1; j < 2; j++) {
                    sortM[p++] = canvas[x+j][y+k];   //grayscale data
                }
            }
            // sort now...
            for (int k = 0; k < 8; k++){
                for (int j = 8; j > k; j--) {
                    if (sortM[j] < sortM[j - 1]) {
                        int dmy = sortM[j];
                        sortM[j] = sortM[j - 1];
                        sortM[j - 1] = dmy; 
                    }
                }
            }

            canvas[x][y] = sortM[4]; // Center Data!
            AreaDiff[face_bank][x/AREA_X][ay] += ( canvas[x][y] );    // 6bit scaler! 
        }            
    }
}

char *filebase =  "/local/Bmap";
//char *filebase =  "/sd/Bmap";

void SearchNewFileNo()
{
    FILE *fp;
    char filename[30] ;
    while( 1 ){    
        sprintf( filename, "%s%04d.bmp", filebase , FileNo);
        if((fp = fopen(filename, "rb")) == NULL){
            pc.printf("%s is not Exist.", filename);
            return ;
        }else{
            pc.printf("%s is Exist. Increment no...", filename);
            fclose(fp);
            FileNo++;
        }
    }
    
    
    
}

//flag 24: 24bit Clolr   8:8itGrayScale
void SaveBMP(int flag)
{
    unsigned char gray8;
    unsigned char bmp_line_data[SIZEX*3]; //画像1行分のRGB情報を格納する
    unsigned char RGBQUAD[4]; //Palette Data
    int back ;
    FILE *fp;
    char filename[30] ;
    
    sprintf( filename, "%s%04d.bmp", filebase , FileNo);
    FileNo++;
    back = myleds;
    
    myleds = 0xff;
    if((fp = fopen(filename, "wb")) == NULL){
        pc.printf("Error: %s could not open.", filename);
        return ;
    }

    // Make BITMAPHEADER to FileHead.
    create_header(fp, SIZEX, SIZEY);
    
    if ( flag==8 ){
        // if 8bit Gray Scale , Make Palette Data
        for (int i=0;i<256;i++){
            RGBQUAD[0] = i;    // 
            RGBQUAD[1] = i;
            RGBQUAD[2] = i;
            RGBQUAD[3] = 0;
            fwrite(RGBQUAD, sizeof(RGBQUAD), 1, fp);
        }
    }
    
    for (int y=0; y<SIZEY; y++) {
        if ( flag==24 ){
            for (int x=0; x<SIZEX; x++) {
                // RGB888
                gray8 = (unsigned char)(canvas[x][y] << 2);   //grayscale data 6bit
    
                bmp_line_data[x*3]     = (unsigned char)gray8;  // b
                bmp_line_data[x*3 + 1] = (unsigned char)gray8;  // g
                bmp_line_data[x*3 + 2] = (unsigned char)gray8;  // r
            }
            fwrite(bmp_line_data, sizeof(unsigned char), sizeof(bmp_line_data), fp);
        }else{
            // GrayScale
            for (int x=0; x<SIZEX; x++) {
                gray8 = (unsigned char)(canvas[x][y] << 2);   //grayscale data 6bit
                fwrite(&gray8 , 1, 1, fp);
            }
        }
    }
    fclose(fp);
    
    myleds = back ;
}
    

int create_header(FILE *fp, int width, int height) {
    int real_width;
    unsigned char header_buf[HEADERSIZE]; //ヘッダを格納する
    unsigned int file_size;
    unsigned int offset_to_data;
    unsigned long info_header_size;
    unsigned int planes;
    unsigned int color;
    unsigned long compress;
    unsigned long data_size;
    long xppm;
    long yppm;

    real_width = width*3 + width%4;

    //ここからヘッダ作成
    file_size = height * real_width + HEADERSIZE;

    offset_to_data = HEADERSIZE;
    if ( COLORS == 8 ) {
        offset_to_data += 256 * 4;
    }

    info_header_size = INFOHEADERSIZE;
    planes = 1;
    color =  COLORS;    // 24;
    compress = 0;
    data_size = height * real_width;
    xppm = 1;
    yppm = 1;

    header_buf[0] = 'B';
    header_buf[1] = 'M';
    memcpy(header_buf + 2, &file_size, sizeof(file_size));
    header_buf[6] = 0;
    header_buf[7] = 0;
    header_buf[8] = 0;
    header_buf[9] = 0;
    memcpy(header_buf + 10, &offset_to_data, sizeof(offset_to_data));
    memcpy(header_buf + 14, &info_header_size, sizeof(info_header_size));
    memcpy(header_buf + 18, &width, sizeof(width));
    height = height * -1; // データ格納順が逆なので、高さをマイナスとしている
    memcpy(header_buf + 22, &height, sizeof(height));
    memcpy(header_buf + 26, &planes, sizeof(planes));
    memcpy(header_buf + 28, &color, sizeof(color));
    memcpy(header_buf + 30, &compress, sizeof(compress));
    memcpy(header_buf + 34, &data_size, sizeof(data_size));
    memcpy(header_buf + 38, &xppm, sizeof(xppm));
    memcpy(header_buf + 42, &yppm, sizeof(yppm));
    header_buf[46] = 0;     // Palette Color (long)
    header_buf[47] = 0;
    header_buf[48] = 0;
    header_buf[49] = 0;
    header_buf[50] = 0;     // Important Color (long)
    header_buf[51] = 0;
    header_buf[52] = 0;
    header_buf[53] = 0;

    //ヘッダの書き込み
    fwrite(header_buf, sizeof(unsigned char), HEADERSIZE, fp);

    return 0;
}