File content as of revision 0:ccbc44580fab:

#include "TFC.h"
#include "Vision.h"

const uint16_t _vision_lightcomp[] = {403,372,303,219,160,111,85,70,60,53,47,43,41,37,36,33,32,31,29,29,28,27,27,26,26,25,25,25,24,24,24,23,23,23,23,22,22,22,21,22,21,21,21,21,21,21,21,21,21,20,21,21,20,21,21,20,20,20,21,21,21,20,20,21,20,20,20,20,20,20,20,20,20,21,20,21,20,20,20,20,20,20,21,21,20,20,20,21,20,21,21,21,20,21,21,22,22,22,22,22,23,23,23,23,23,25,24,25,25,26,26,27,27,29,29,31,32,35,38,41,43,46,47,53,62,77,93,113};

Vision::Vision()
{
    lightCompensation = false;
    newDataToProcess = false;
}

void Vision::saveData(uint16_t* newData)
{
    uint8_t i;
    
    for(i=0;i<128;i++)
    {
        lineData[i] = newData[i];
    }
    newDataToProcess = true;
}

struct lineScanData Vision::processLine()
{
    uint32_t edgeProcessingBuffer[128];
    int16_t edgeGradients[128];
    uint8_t i;
    uint32_t maxWhite = 0;
    uint8_t maxWhiteIndex = 64;
    uint16_t edgeThreshold = 100;
    struct lineScanData result;
    
//    uint32_t maxWhiteSum = 0;
//    uint8_t whiteAreaDelta = 5;
    
    // remove noise from data
    for(i=2;i<126;i++)
    {
        // apply gaussian filter
        edgeProcessingBuffer[i] = (lineData[i-2]+lineData[i+2])*5+(lineData[i-1]+lineData[i+1])*12+lineData[i]*15; // filtered data is multiplied by 49 here
        if(edgeProcessingBuffer[i] > maxWhite)
        {
            maxWhite = edgeProcessingBuffer[i];
            maxWhiteIndex = i;
        }
    }
    
/*    // Find the area of 2*whiteAreaDelta which is the brighter
    // init max white search
    maxWhiteSum = 0;
    for(i=2;i<(2+whiteAreaDelta*2);i++)
    {
        maxWhiteSum += edgeProcessingBuffer[i];
    }
    maxWhite = maxWhiteSum;
    maxWhiteIndex = 2+whiteAreaDelta;
    for(i=3+whiteAreaDelta;i<126-whiteAreaDelta;i++)
    {
        maxWhiteSum += edgeProcessingBuffer[i+whiteAreaDelta-1]-edgeProcessingBuffer[i-whiteAreaDelta-1];
        if(maxWhiteSum > maxWhite)
        {
            maxWhite = maxWhiteSum;
            maxWhiteIndex = i;
        }
    }*/
    
    result.mostWhiteIndex = maxWhiteIndex;
    result.maxLightValue = (uint16_t)(maxWhite/49);
    edgeThreshold = maxWhite/20;
    
    // find gradients
    for(i=2;i<125;i++)
    {
        edgeGradients[i] = edgeProcessingBuffer[i]-edgeProcessingBuffer[i+1];
    }
    
    // find right edge
    result.rightEdgeIndex = 127;
    for(i=maxWhiteIndex;i<125;i++)
    {
        if(edgeGradients[i]>edgeThreshold)
        {
            result.rightEdgeIndex = i;
            break;
        }
    }
    
    // find left edge
    result.leftEdgeIndex = 0;
    for(i=maxWhiteIndex;i>=2;i--)
    {
        if(edgeGradients[i] < -edgeThreshold)
        {
            result.leftEdgeIndex = i;
            break;
        }
    }
    
    return result;
}

void Vision::lineScanLightAdjust()
{
    uint8_t i;

    for(i=0;i<128;i++)
    {
        lineData[i] = (lineData[i]-150)*_vision_lightcomp[i];
    }
}

void Vision::processTasks()
{
    if(newDataToProcess)
    {
        //TFC_BAT_LED3_ON;
        if(lightCompensation)
            lineScanLightAdjust();
        currentRoadData = processLine();
        newDataToProcess = false;
        //TFC_BAT_LED3_OFF;
    }
}

struct lineScanData Vision::getRoadData()
{
    return currentRoadData;
}