File content as of revision 23:a1a671d3c9e5:

/*
AnalogIn cam1(CAM1_IN);
DigitalOut camSi(CAM_SI);
DigitalOut camClk(CAM_CLK); // Definining camera pins. These are actually defined in Main... 
*/
#include "LineScan.h"

#define THRESH 5000

#define MEAN_REF 20000      //ideal mean we should see
#define BRIGHTEST_REF 50000
#define CAM_CTRL_GAIN 0.0003 //should be small

uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
    uint16_t pixel;
    
    //read pixel n
    pixel = cam.read_u16();
    
    //clock pulse for next pixel n + 1
    *camClk = 1;
    *camClk = 0;    // Apparently there is no need for any delay. It may be desireable to flatten this, or perhaps the compiler does it. 
    
    return pixel;  //return result as an uint16_t (16 bit integer)
}

void startRead(DigitalOut *camSi, DigitalOut *camClk){
    //pulse first clock cycle and start integral pin
    *camSi = 1;
    *camClk = 1;
    *camSi = 0;
    *camClk = 0;
}

float processFn(uint16_t *array, int arraySz, int* exposure){
    const static int frameLen = NUM_PIX -  2 * SKIP;
    int avg[frameLen];
    int diff[frameLen];
    int highest = 0;
    int lowest = 0;
    int total = 0;
    int h_idx = frameLen/2;
    int l_idx = frameLen/2;
    int left_bound = SKIP;
    int right_bound = NUM_PIX - SKIP;
    uint16_t brightest = 0;
    
    //for AGC
    float exposureChange;
    
    //Just finds line center, does not track crossings (needs this feature)
    if(array){ 
        avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
        diff[0] = 0;
        brightest = avg[0];
        total += avg[0];    //AGC
        
        for(int i = 1; i < frameLen; i++){
            avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2;   //smoothing
            diff[i] = avg[i - 1] - avg[i];          //differential
            
            //total = avg[i];    //AGC
            if (avg[i] > brightest) {
                brightest = avg[i];
            }
            
            if(diff[i] > highest){
                highest = diff[i];
                h_idx = i;
            }
            else if(diff[i] < lowest){
                lowest = diff[i];
                l_idx = i;
            }
        }
        //AGC, simple proportional controller
        //total = total / frameLen;
        exposureChange = ((float)(BRIGHTEST_REF - brightest)) * CAM_CTRL_GAIN;    
        *exposure += (int)exposureChange;
        if(*exposure < 1)
            *exposure = 1;
        else if(*exposure > 30)
            *exposure = 30;
    }
    //valid if white line on black and not blinded
    if (highest > THRESH && -1 * lowest > THRESH)
        return ((float) h_idx + (float) l_idx) / (2*(float)frameLen);
    else if(highest > THRESH && -1 * lowest < THRESH )
        return ((float) h_idx + (float)left_bound - 1.5*(float)(right_bound - h_idx)) / (2*(float)frameLen);    //0.5 is center
    else if (-1*lowest > THRESH && highest < THRESH )
        return ((float) l_idx + (float)right_bound + 1.5*(float)(l_idx - left_bound)) / (2*(float)frameLen);
    else
        return -1.0;  //invalid read
}

//call after integration time is done, returns index of array line is expected to be at and new exposure time
float getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk, int *exposure, telemetry::NumericArray<uint16_t, 128> &tele_linescan){
    uint16_t lineAry[NUM_PIX];
    float position;
    
    //read 
    startRead(camSi, camClk);
    for(int i = 0; i < NUM_PIX; i++){
        lineAry[i] = read1Bit(cam, camClk);
        tele_linescan[i] = lineAry[i];
    }
    
    //process line scan data
    position  = processFn(lineAry, NUM_PIX, exposure);
    
    return position;
}

/*
sample call (handler thread):

while(1){
   linePos  = getLinePos(cameraIn1, si, clk, &exposureTime);   //volatile linePos, replace with steering angle and read 2 cameras?
   Thread::wait(exposureTime);    //sleep for 14 us
}
*/