File content as of revision 99:c6665262fd3d:

#include "ArduCAM.h"

//#define SW_DEBUG
#include "GlobalVariable.h"
#include "SWCommon.h"


#include "CamRegBuf.h"

#include "ArduUTFT.h"

#include <math.h>

#define CAM_BLK_CAL_ACTIVE

//#define CAM_DISP_DEBUG
//#define CAM_DISP_DEBUG_CENTER
//#define CAM_DISP_IMG

#define IMG_PROC_SIGNAL 0xBB

#ifdef __cplusplus
extern "C" {
#endif

const static uint8_t CAM_BLK_CAL_LEFT  = ((RESOLUTION_WIDTH / 2) - 1);
const static uint8_t CAM_BLK_CAL_RIGHT = ((RESOLUTION_WIDTH / 2) + 1);
const static uint8_t TERMINATE_WIDTH = static_cast<uint8_t>(RESOLUTION_WIDTH * 0.28f);

static DigitalOut cam_cs(PIN_ACC_CS, 1);

static uint8_t temp_mid_pos = RESOLUTION_WIDTH / 2;
static uint8_t black_calibrate = 70;
static volatile uint8_t centerLine[CAM_ROI_UPPER_LIMIT * 2];
static volatile uint8_t is_encounter_terminate = 0;
//static volatile uint8_t is_border_find = BOTH_NOT_FOUND;
static volatile uint8_t is_intersection_detected = 0;

void cal_black_calibrate();

inline void ardu_cam_spi_write_8(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 | 0x80);
    g_spi_port.write(value);
    // take the SS pin high to de-select the chip:
    cam_cs = 1;
}
 
inline uint8_t ardu_cam_spi_read_8(int address)
{ 
    // 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 & 0x7F);
    uint8_t value = static_cast<uint8_t>(g_spi_port.write(0x00));
    // take the SS pin high to de-select the chip:
    cam_cs = 1;
    
    return value;
}

bool ardu_cam_init()
{
    CamRegBuf * camReg = new CamRegBuf(CAM_SCCB_WRITE, CAM_SCCB_READ);

    camReg->WriteRegSet(ResetProg);
    wait_ms(10);
    camReg->WriteRegSet(QVGA);
    wait_ms(10);
    
#if defined(ARDUCAM_OV2640)
    camReg->SCCBWrite(0xff, 0x01);
#endif
    
    delete camReg;
    camReg = NULL;
    
    uint8_t VerNum = ardu_cam_spi_read_8(ARDUCHIP_VER);
    VerNum = ardu_cam_spi_read_8(ARDUCHIP_VER);
    
    ardu_cam_spi_write_8(ARDUCHIP_TEST1, ARDUCHIP_TEST_MSG);
    uint8_t testV = ardu_cam_spi_read_8(ARDUCHIP_TEST1);
    if(VerNum != ARDUCHIP_VER_NUM || testV != ARDUCHIP_TEST_MSG)
    {
        return false;
    }
    
    ardu_cam_spi_write_8(ARDUCHIP_CAP_CTRL, 0x00);
    
    cal_black_calibrate();
    
    return true;
}

void ardu_cam_start_capture()
{
    ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
    
    ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_START_MASK);
}

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

uint8_t ardu_cam_get_pixel()
{
    uint16_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
    uint16_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
    //uint16_t VL = ardu_cam_spi_burst_read_16();
    
    VL = (VL & 0x00FF) | ((VH << 8) & 0xFF00);
    uint8_t ch = ((VL & 0xF800) >> 9);// << 2;
    float pixel = (static_cast<float>(ch) * 0.21);
    
    ch = ((VL & 0x07E0) >> 3);// << 2;
    pixel += (static_cast<float>(ch) * 0.72);
    
    ch = (VL & 0x001F) << 2;
    pixel += (static_cast<float>(ch) * 0.07);
    
    return static_cast<uint8_t>(pixel);
}

uint8_t ardu_cam_is_capture_finished()
{
    return (ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK);
}

void cal_black_calibrate()
{
    ardu_cam_start_capture();
    
    while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
    
    float temp = 0.0f;
    static uint16_t pixel = 0x00;
    for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
    {
        pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
        pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);

        if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
        {
            temp += static_cast<uint8_t>((pixel >> 3) & 0x00FF);
        }
    }
    temp = temp / (CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
    black_calibrate = temp * 0.7f;
}

inline void get_img_row_info(const uint8_t rowI, uint8_t * left, uint8_t * right, uint8_t* isBorderFound)
{
    *left = 0;
    *right = RESOLUTION_WIDTH;
    *isBorderFound = BOTH_NOT_FOUND;
    uint8_t isRightFound = 0;
    static uint16_t pixel = 0x0000;
    static uint8_t pGreen = 0x00;
    
    for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
    {
        pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
        pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);

        pGreen = static_cast<uint8_t>((pixel & 0x07E0) >> 3);
        if((pGreen < black_calibrate))
        {
            if(j < temp_mid_pos)
            {
                *isBorderFound = LEFT_FOUND;
                *left = j;
#ifdef CAM_DISP_DEBUG
                ardu_utft_write_DATA(0xF8, 0x00);
#endif
            }
            else if(!isRightFound)
            {
                *right = j;
                isRightFound = 1;
#ifdef CAM_DISP_DEBUG
                ardu_utft_write_DATA(0xF8, 0x00);
#endif
            }
#ifdef CAM_DISP_DEBUG
            else
            {
                ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
            }
#endif
        }
#ifdef CAM_DISP_DEBUG
        else
        {
            ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
        }
        
#elif defined(CAM_DISP_IMG)
        ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
#endif
    }
    
    if(*isBorderFound == LEFT_FOUND && isRightFound)
    {
        *isBorderFound = BOTH_FOUND;
    }
    else if(isRightFound)
    {
        *isBorderFound = RIGHT_FOUND;
    }

}

volatile const uint8_t* ardu_cam_get_center_array()
{
    return centerLine;
}

void image_processing()
{
    
    //while(true)
    {
        ardu_cam_start_capture();
    
        while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
        
        temp_mid_pos = RESOLUTION_WIDTH / 2;
        //is_border_find = BOTH_NOT_FOUND;
#ifdef CAM_BLK_CAL_ACTIVE
        static float calTemp = 0.0f;
        calTemp = 0.0f;
        static uint16_t greenPixel = 0x00;
        uint8_t isValid = 1;
        for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
        {
            if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
            {
                greenPixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
                greenPixel = greenPixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
                greenPixel = (greenPixel & 0x07E0) >> 3;
                if(static_cast<uint8_t>(greenPixel) < black_calibrate)
                {
                    isValid = 0;
                }
                calTemp += static_cast<uint8_t>(greenPixel);
            }
            else
            {
                ardu_cam_spi_read_8(SINGLE_FIFO_READ);
                ardu_cam_spi_read_8(SINGLE_FIFO_READ);
            }
        }
        
        if(isValid)
        {
            calTemp = calTemp / static_cast<float>(CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
            black_calibrate = static_cast<uint8_t>(calTemp * 0.7f);
        }
#endif
        //cal_black_calibrate();
        uint8_t leftPos = 0;
        uint8_t rightPos = 0;
        uint8_t isBorderFound = BOTH_NOT_FOUND;
        
        is_intersection_detected = 0;
        uint8_t leftNearCenter1 = 0;
        uint8_t leftNearCenter2 = 0;
        uint8_t rightNearCenter1 = 0;
        uint8_t rightNearCenter2 = 0;
        
        is_encounter_terminate = 0;
        for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
        {
#if defined(CAM_DISP_IMG) or defined(CAM_DISP_DEBUG)
            ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
#endif
            get_img_row_info(i, &leftPos, &rightPos, &isBorderFound);
            temp_mid_pos = (leftPos + rightPos) / 2;
            uint8_t dis_btw_left_right = rightPos - leftPos - 1;
            if(dis_btw_left_right < TERMINATE_WIDTH)
            {
                is_encounter_terminate = 1;
            }

#ifdef CAM_DISP_DEBUG_CENTER
            ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
            ardu_utft_write_DATA(0xF8, 0x00);
#endif

            centerLine[CAM_ROI_UPPER_LIMIT - i - 1] = temp_mid_pos;
            centerLine[(2 * CAM_ROI_UPPER_LIMIT) - i - 1] = isBorderFound;
            
            if(leftPos > leftNearCenter1)
            {
                leftNearCenter2 = leftNearCenter1; 
                leftNearCenter1 = CAM_ROI_UPPER_LIMIT - i - 1;
            }
            if(rightPos < rightNearCenter1)
            {
                rightNearCenter2 = rightNearCenter1;
                rightNearCenter1 = CAM_ROI_UPPER_LIMIT - i - 1;
            }
        }
        
        if( ((1 < rightNearCenter1 && rightNearCenter1 < (CAM_ROI_UPPER_LIMIT - 2)) && (abs(rightNearCenter1 - rightNearCenter2) == 1)) ||
            ((1 < leftNearCenter1 && leftNearCenter1 < (CAM_ROI_UPPER_LIMIT - 2)) && (abs(leftNearCenter1 - leftNearCenter1) == 1)) )
        {
            is_intersection_detected = 1;
        }
    }
}


inline void get_img_row_disp(const uint8_t rowI, uint8_t * left, uint8_t * right)
{
    *left = 0;
    *right = RESOLUTION_WIDTH;
    uint8_t isRightFound = 0;
    static uint16_t pixel = 0x0000;
    static uint8_t pGreen = 0x00;
    
    for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
    {
        pixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
        pixel = pixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);

        pGreen = static_cast<uint8_t>((pixel & 0x07E0) >> 3);
        if((pGreen < black_calibrate))
        {
            if(j < temp_mid_pos)
            {
                *left = j;
                ardu_utft_write_DATA(0xF8, 0x00);
            }
            else if(!isRightFound)
            {
                *right = j;
                isRightFound = 1;
                ardu_utft_write_DATA(0xF8, 0x00);
            }
            else
            {
                ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
            }
        }
        else
        {
            ardu_utft_write_DATA(static_cast<uint8_t>(pixel >> 8), static_cast<uint8_t>(pixel));
        }
    }

}

void ardu_cam_display_img_utft()
{
    ardu_cam_start_capture();
    
    while (!(ardu_cam_spi_read_8(ARDUCHIP_TRIG) & CAP_DONE_MASK));
    
    temp_mid_pos = RESOLUTION_WIDTH / 2;
#ifdef CAM_BLK_CAL_ACTIVE
    static float calTemp = 0.0f;
    calTemp = 0.0f;
    static uint16_t greenPixel = 0x00;
    uint8_t isValid = 1;
    for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
    {
        if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
        {
            greenPixel = static_cast<uint16_t>(ardu_cam_spi_read_8(SINGLE_FIFO_READ)) << 8;
            greenPixel = greenPixel | ardu_cam_spi_read_8(SINGLE_FIFO_READ);
            greenPixel = (greenPixel & 0x07E0) >> 3;
            if(static_cast<uint8_t>(greenPixel) < black_calibrate)
            {
                isValid = 0;
            }
            calTemp += static_cast<uint8_t>(greenPixel);
        }
        else
        {
            ardu_cam_spi_read_8(SINGLE_FIFO_READ);
            ardu_cam_spi_read_8(SINGLE_FIFO_READ);
        }
    }
    
    if(isValid)
    {
        calTemp = calTemp / static_cast<float>(CAM_BLK_CAL_RIGHT - CAM_BLK_CAL_LEFT);
        black_calibrate = static_cast<uint8_t>(calTemp * 0.7f);
    }
#endif
    //cal_black_calibrate();
    uint8_t leftPos = 0;
    uint8_t rightPos = 0;
    is_encounter_terminate = 0;
    for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
    {
        ardu_utft_set_camimg_row(CAM_ROI_UPPER_LIMIT - i);
        
        get_img_row_disp(i, &leftPos, &rightPos);
        temp_mid_pos = (leftPos + rightPos) / 2;

        ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
        ardu_utft_write_DATA(0xF8, 0x00);

    }
}

uint8_t ardu_cam_get_is_encounter_terminate()
{
    return is_encounter_terminate;
}
/*
uint8_t ardu_cam_get_is_border_found()
{
    return is_border_find;
}
*/

uint8_t ardu_cam_get_ver_num()
{
    return ardu_cam_spi_read_8(ARDUCHIP_VER);
}

uint8_t ardu_cam_get_is_intersection_detected()
{
    return is_intersection_detected;
}

#ifdef __cplusplus
}
#endif