File content as of revision 33:e3fcc4d6bb9b:

#include "ArduCAM.h"

#define RESOLUTION_WIDTH 160
#define RESOLUTION_HEIGHT 120

#include "GlobalVariable.h"
#include "SWUSBServer.h"

#include "CamRegBuf.h"
#include "OV2640RegProg.h"

#include <string>

extern SPI g_spi_port;

DigitalOut cam_cs(PIN_ACC_CS);

DigitalOut testLED2(LED_RED, 1);

/*
inline static uint8_t ardu_cam_spi_io(int address, int value = 0x00)
{
    //g_spi_port.lock();
    
    cam_cs = 0;
    g_spi_port.write(address);
    uint8_t result = g_spi_port.write(value);
    cam_cs = 1;
    
    //g_spi_port.unlock();
    
    return result;
}
*/

//Write ArduChip internal registers
void ardu_cam_write_reg(uint8_t addr, uint8_t data)
{
    ardu_cam_bus_write(addr | 0x80, data);
}
 
//Read ArduChip internal registers
uint8_t ardu_cam_read_reg(uint8_t addr)
{
    uint8_t data;
    data = ardu_cam_bus_read(addr & 0x7F);
    return data;
}

bool ardu_cam_init()
{
    char buf[20];
    
    
    CamRegBuf * camReg = new CamRegBuf(g_core, OV2640_WRITE, OV2640_READ);
    /*
    camReg->SCCBWrite(0xff, 0x00);
    uint8_t camVer = camReg->SCCBRead(0xDA);
    sprintf(buf, "Cam MOD %#x", camVer);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    */
    camReg->SCCBWrite(0xff, 0x01);
    camReg->SCCBWrite(0x12, 0x80);
    wait_ms(5);
    camReg->WriteRegSet(OV2640Prog::QQVGA);
    wait_ms(100);
    
    camReg->SCCBWrite(0xff, 0x01);
    uint8_t camVer = camReg->SCCBRead(OV2640_PID_ADDR);
    sprintf(buf, "Cam VerH %#x", camVer);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    camVer = camReg->SCCBRead(OV2640_VER_ADDR);
    sprintf(buf, "Cam VerL %#x", camVer);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    /*
    camReg->SCCBWrite(0xff, 0x00);
    camVer = camReg->SCCBRead(0xDA);
    sprintf(buf, "Cam MOD %#x", camVer);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    */
    delete camReg;
    camReg = NULL;
    
    
    
    
    uint8_t VerNum = ardu_cam_read_reg(0x40);
    VerNum = ardu_cam_read_reg(0x40);
    
    
    sprintf(buf, "Ardu Ver %#x", VerNum);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    ardu_cam_write_reg(ARDUCHIP_TEST1, ARDUCHIP_TEST_MSG);
    uint8_t testV = ardu_cam_read_reg(ARDUCHIP_TEST1);
    if(VerNum != 0x61 || testV != ARDUCHIP_TEST_MSG)
    {
        g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Fa");
        return false;
    }
    g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Su");
    
    ardu_cam_set_mode(MCU2LCD_MODE);
    ardu_cam_start_capture();
    
    wait(0.1);
    ardu_cam_set_mode(CAM2LCD_MODE);
    
    
    //unsigned char tempV = ardu_cam_read_reg(ARDUCHIP_MODE);
    //sprintf(buf, "Ardu Stat %#x", tempV);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    //tempV = ardu_cam_read_reg(ARDUCHIP_CAP_CTRL);
    //sprintf(buf, "Ardu FS1 %#x", tempV);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    return true;
}

void ardu_cam_set_mode(uint8_t mode)
{
    //ardu_cam_bus_io(ARDUCHIP_MODE, mode);
    switch(mode)
    {
        case MCU2LCD_MODE:
            ardu_cam_write_reg(ARDUCHIP_MODE, MCU2LCD_MODE);
            break;
        case CAM2LCD_MODE:
            ardu_cam_write_reg(ARDUCHIP_MODE, CAM2LCD_MODE);
            break;
        //case LCD2MCU_MODE:
            //ardu_cam_write_reg(ARDUCHIP_MODE, LCD2MCU_MODE);
            //break;
        default:
            ardu_cam_write_reg(ARDUCHIP_MODE, CAM2LCD_MODE);
            break;
    }
}

void ardu_cam_start_capture()
{
    ardu_cam_write_reg(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
    ardu_cam_write_reg(ARDUCHIP_CAP_CTRL, 0x00);
    ardu_cam_write_reg(ARDUCHIP_FIFO, FIFO_START_MASK);
}

uint32_t ardu_cam_get_fifo_length()
{
    uint32_t len1,len2,len3,length=0;
    len1 = ardu_cam_read_reg(FIFO_SIZE1);
    len2 = ardu_cam_read_reg(FIFO_SIZE2);
    len3 = ardu_cam_read_reg(FIFO_SIZE3) & 0x07;
    length = ((len3 << 16) | (len2 << 8) | len1) & 0x07ffff;
    return length;
}

int ardu_cam_bus_write(int address, int value)
{
    // take the SS pin low to select the chip:
    cam_cs = 0;
    //  send in the address and value via SPI:
    g_spi_port.write(address);
    g_spi_port.write(value);
    // take the SS pin high to de-select the chip:
    cam_cs = 1;
    
    return value;
}
 
uint8_t ardu_cam_bus_read(int address)
{ 
    uint8_t value = 0;
    // take the SS pin low to select the chip:
    cam_cs = 0;
    //  send in the address and value via SPI:
    g_spi_port.write(address);
    value = g_spi_port.write(0x00);
    // take the SS pin high to de-select the chip:
    cam_cs = 1;
    return value;
}

uint8_t ardu_cam_get_pixel()
{
    uint16_t VH = ardu_cam_read_reg(SINGLE_FIFO_READ);
    uint16_t VL = ardu_cam_read_reg(SINGLE_FIFO_READ);
    
    //char buf[20];
    //sprintf(buf, "h%#x", VH);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    //sprintf(buf, "l%#x", VL);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    VL = (VL & 0x00FF) | ((VH << 8) & 0xFF00);
    uint8_t ch = ((VL & 0xF800) >> 9);// << 2;
    float pixel = (static_cast<float>(ch) * 0.21);
    
    //sprintf(buf, "c%#x", ch);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    ch = ((VL & 0x07E0) >> 3);// << 2;
    pixel += (static_cast<float>(ch) * 0.72);
    
    //sprintf(buf, "c%#x", ch);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    ch = (VL & 0x001F) << 2;
    pixel += (static_cast<float>(ch) * 0.07);
    
    //sprintf(buf, "c%#x", ch);
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    //sprintf(buf, "p%#x", static_cast<uint8_t>(pixel));
    //g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    return static_cast<uint8_t>(pixel);
}

void ardu_cam_print_debug()
{
    uint32_t len = ardu_cam_get_fifo_length();
    char buf[20];
    sprintf(buf, "Cam L %#x", len);
    g_core.GetUSBServer().PushReliableMsg('D', buf);
    
    //if(len < (RESOLUTION_HEIGHT * RESOLUTION_WIDTH * 2))
    //    return;
    
    //Begin output the picture
    std::string lineBuf;
    lineBuf.resize(RESOLUTION_WIDTH + 1);
    
    ardu_cam_get_pixel(); //Get the first dummy pixel
    
    for (uint8_t i = 0; i < RESOLUTION_HEIGHT; ++i)
    {
        lineBuf[0] = i;
        
        for (int j = 0; j < RESOLUTION_WIDTH; ++j)
        {
            //uint8_t p = ardu_cam_get_pixel();
            lineBuf[j + 1] = ardu_cam_get_pixel();
        }
        
        g_core.GetUSBServer().PushReliableMsg('P', lineBuf);
        wait(0.4);
    }
    
}