File content as of revision 27:173b2f564143:

#include "LineScan.h"
#include <string.h>

#define THRESH 1000

#define MEAN_REF 10000      //ideal mean we should see
#define CAM_CTRL_GAIN 0.002 //should be small

#define NEUTRAL 0
#define FIND_PEAK 1
#define FIND_TROUGH 2

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;

    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, telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff)
{
    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_ary[10];
    int h_idx_pos = 0;
    int l_idx_ary[10];
    int l_idx_pos = 0;
    int state = NEUTRAL;
    int *l_walker, *h_walker;
    int out_width = frameLen;
    int i;
    float out = -1.0;
    int thinnest = 128;
    int linestart = -1;
    int linestop = -1;
    int tempstart = 0;

    //for AGC
    float exposureChange;

    h_idx_ary[0] = -1;
    l_idx_ary[0] = -1;

    if(array) {
        avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;     // To cut off the (invalid) ends of the data. 
        diff[0] = 0;
        tele_linescan_diff[0] = diff[0];

        total += avg[0];    //AGC

        for(i = 1; i < frameLen; i++) {
            avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2;   //smoothing
            if ((avg[i] - avg[i-1] >= THRESH) || (avg[i] - avg[i-1] <= -THRESH)) {
                diff[i] = avg[i] - avg[i-1];          //differential
            } else {
                diff[i] = 0;
            }
            if (diff [i] > 0) {
                tele_linescan_diff[i] = 10000;
            } else if (diff[i] < 0) {
                tele_linescan_diff[i] = 5000;
            } else {
                tele_linescan_diff[i] = 0;
            }

            total += avg[i];    //AGC


            //Finite State Machine
            //problem in this section, rewrite? find peaks and troughs
            switch(state) {
                case NEUTRAL:
                    //serial.printf("Neutral case in processFn\r\n");
                    if( diff[i] > THRESH ) {
                        state = FIND_PEAK;
                        highest = diff[i];
                        h_idx_ary[h_idx_pos] = i;
                        tempstart = i;
                    } /*else if( diff[i] < -THRESH ) {
                        state = FIND_TROUGH;
                        lowest = diff[i];
                        l_idx_ary[l_idx_pos] = i;
                        tempstart = i;
                    }*/
                    break;

                case FIND_PEAK:
                    //serial.printf("Peak case in processFn\r\n");
                    if( diff[i] <= -THRESH ) {
                        state = NEUTRAL;
                        h_idx_pos++;
                        h_idx_ary[h_idx_pos] = -1;  //last element is always -1
                        if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
                            thinnest = i - tempstart;
                            linestart = tempstart;
                            linestop = i;
                        }
                    } else if(diff[i] > highest) {
                        highest = diff[i];
                        h_idx_ary[h_idx_pos] = i;  //insert to array, clobbers
                        tempstart = i;
                    }
                    break;

                case FIND_TROUGH:
                    //serial.printf("Trough case in processFn\r\n");
                    if( diff[i] >= THRESH ) {
                        state = NEUTRAL;
                        l_idx_pos++;
                        l_idx_ary[l_idx_pos] = -1;  //last element is always -1
                        if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
                            thinnest = i - tempstart;
                            linestart = tempstart;
                            linestop = i;
                        }
                    } else if(diff[i] < lowest) {
                        lowest = diff[i];
                        l_idx_ary[l_idx_pos] = i;  //insert to array, clobbers
                        tempstart = i;
                    }
                    break;

                default:
                    //exit case if exception happened
                    serial.printf("Default case in processFn\r\n");
                    return out;
            }
        }
        //AGC, simple proportional controller
        total = total / frameLen;
        exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
        *exposure += (int)exposureChange;
        if(*exposure < 0)
            *exposure = 0;
        else if(*exposure > UPDATE_RATE)
            *exposure = UPDATE_RATE;
    }

    l_walker = l_idx_ary;
    h_walker = h_idx_ary;

    /*while(*l_walker != -1 && *h_walker != -1){
        //evaluate out and advance if line is white on black and returns center of smallest white band
        //if interval is black on white, advance the pointer for the peak array
        //width needs to be larger than 4 pixels wide to be a good read.

        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
            out_width = *h_walker - *l_walker;
            out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
            l_walker++;
        }
        h_walker++;

        //serial.printf("%d %d\r\n", *h_walker, *l_walker);
        //serial.printf("%.2f\r\n", out);
    }*/

    if (linestart != -1) {
        out = ((float)(linestart + linestop)) / (2.0 * (float)frameLen);
    } else {
        out = -1;
    }
    return out;
}

float processFn2(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_ary[10];
    int h_idx_pos = 0;
    int l_idx_ary[10];
    int l_idx_pos = 0;
    int state = NEUTRAL;
    int *l_walker, *h_walker;
    int out_width = frameLen;
    int i;
    float out = -1.0;
    int thinnest = 128;
    int linestart = -1;
    int linestop = -1;
    int tempstart = 0;

    //for AGC
    float exposureChange;

    h_idx_ary[0] = -1;
    l_idx_ary[0] = -1;

    if(array) {
        avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
        diff[0] = 0;
        //tele_linescan_diff[0] = diff[0];

        total += avg[0];    //AGC

        for(i = 1; i < frameLen; i++) {
            avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2;   //smoothing
            diff[i] = avg[i] - avg[i-1];          //differential
            //tele_linescan_diff[i] = diff[i];

            total += avg[i];    //AGC


            //Finite State Machine
            //problem in this section, rewrite? find peaks and troughs
            switch(state) {
                case NEUTRAL:
                    //serial.printf("Neutral case in processFn\r\n");
                    if( diff[i] > THRESH ) {
                        state = FIND_PEAK;
                        highest = diff[i];
                        h_idx_ary[h_idx_pos] = i;
                        tempstart = i;
                    } else if( diff[i] < -THRESH ) {
                        state = FIND_TROUGH;
                        lowest = diff[i];
                        l_idx_ary[l_idx_pos] = i;
                        tempstart = i;
                    }
                    break;

                case FIND_PEAK:
                    //serial.printf("Peak case in processFn\r\n");
                    if( diff[i] <= THRESH ) {
                        state = NEUTRAL;
                        h_idx_pos++;
                        h_idx_ary[h_idx_pos] = -1;  //last element is always -1
                        if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
                            thinnest = i - tempstart;
                            linestart = tempstart;
                            linestop = i;
                        }
                    } else if(diff[i] > highest) {
                        highest = diff[i];
                        h_idx_ary[h_idx_pos] = i;  //insert to array, clobbers
                        tempstart = i;
                    }
                    break;

                case FIND_TROUGH:
                    //serial.printf("Trough case in processFn\r\n");
                    if( diff[i] >= -THRESH ) {
                        state = NEUTRAL;
                        l_idx_pos++;
                        l_idx_ary[l_idx_pos] = -1;  //last element is always -1
                        if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
                            thinnest = i - tempstart;
                            linestart = tempstart;
                            linestop = i;
                        }
                    } else if(diff[i] < lowest) {
                        lowest = diff[i];
                        l_idx_ary[l_idx_pos] = i;  //insert to array, clobbers
                        tempstart = i;
                    }
                    break;

                default:
                    //exit case if exception happened
                    serial.printf("Default case in processFn\r\n");
                    return out;
            }
        }
        //AGC, simple proportional controller
        total = total / frameLen;
        exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;       // This is actually I control. 
        *exposure += (int)exposureChange;
        if(*exposure < 0)
            *exposure = 0;
        else if(*exposure > UPDATE_RATE)
            *exposure = UPDATE_RATE;
    }

    l_walker = l_idx_ary;
    h_walker = h_idx_ary;

    /*while(*l_walker != -1 && *h_walker != -1){
        //evaluate out and advance if line is white on black and returns center of smallest white band
        //if interval is black on white, advance the pointer for the peak array
        //width needs to be larger than 4 pixels wide to be a good read.

        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
            out_width = *h_walker - *l_walker;
            out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
            l_walker++;
        }
        h_walker++;

        //serial.printf("%d %d\r\n", *h_walker, *l_walker);
        //serial.printf("%.2f\r\n", out);
    }*/

    if (linestart != -1) {
        out = ((float)(linestart + linestop)) / (2.0 * (float)frameLen);
    } else {
        out = -1;
    }
    return out;
}

//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, NUM_PIX> &tele_linescan,
                 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff)
{
    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, tele_linescan_diff);

    return position;
}



float getLinePos2(AnalogIn cam,
                  DigitalOut *camSi,
                  DigitalOut *camClk,
                  int *exposure)            // Doesn't send telem. 
{
    uint16_t lineAry[NUM_PIX];
    float position;

    //read
    startRead(camSi, camClk);
    for(int i = 0; i < NUM_PIX; i++) {
        lineAry[i] = read1Bit(cam, camClk);
    }

    //process line scan data
    position  = processFn2(lineAry, NUM_PIX, exposure);

    return position;
}