Diff: LineScan.cpp

Revision:

18:8a65598abf2f

Parent:

17:4ba196fe78ea

Child:

19:5a29c887c8eb

--- a/LineScan.cpp	Sat Apr 18 17:58:27 2015 +0000
+++ b/LineScan.cpp	Wed Apr 22 02:05:47 2015 +0000
@@ -1,11 +1,7 @@
-/*
-AnalogIn cam1(CAM1_IN);
-DigitalOut camSi(CAM_SI);
-DigitalOut camClk(CAM_CLK); // Definining camera pins. These are actually defined in Main... 
-*/
 #include "LineScan.h"
+#include <string.h>
 
-#define THRESH 5000 //may need adjusting
+#define THRESH 5000
 
 #define MEAN_REF 20000      //ideal mean we should see
 #define CAM_CTRL_GAIN 0.0009 //should be small
@@ -14,8 +10,6 @@
 #define FIND_PEAK 1
 #define FIND_TROUGH 2
 
-//#define FRAMELEN NUM_PIX -  2 * SKIP
-
 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
     uint16_t pixel;
     
@@ -37,76 +31,89 @@
     *camClk = 0;
 }
 
-float processFn(uint16_t *array, int arraySz, int* exposure, telemetry::NumericArray<uint16_t, NUM_PIX -  2 * SKIP> &tele_linescan_diff){
+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[frameLen];
+    int h_idx_ary[10];
     int h_idx_pos = 0;
-    int l_idx_ary[frameLen];
+    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;
     
     //for AGC
     float exposureChange;
     
-    memset (h_idx_ary, -1, frameLen);
-    memset (l_idx_ary, -1, frameLen);
+    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] = 0;
+        tele_linescan_diff[0] = diff[0];
         
         total += avg[0];    //AGC
         
-        for(int i = 1; i < frameLen; i++){
+        for(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
             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;
                     }else if( diff[i] < -THRESH ){
                         state = FIND_TROUGH;
                         lowest = diff[i];
+                        l_idx_ary[l_idx_pos] = i;
                     }
                     break;
                     
                 case FIND_PEAK:
-                    if( diff[i] <= THRESH )
+                    //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
+                    }
                     else if(diff[i] > highest){
                         highest = diff[i];
-                        h_idx_ary[h_idx_pos] = i;  //insert to array
-                        h_idx_pos++;
+                        h_idx_ary[h_idx_pos] = i;  //insert to array, clobbers
                     }
                     break;
                     
                 case FIND_TROUGH:
-                    if( diff[i] >= -THRESH )
+                    //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
+                    }
                     else if(diff[i] < lowest){
                         lowest = diff[i];
-                        l_idx_ary[l_idx_pos] = i;  //insert to array
-                        l_idx_pos++;
+                        l_idx_ary[l_idx_pos] = i;  //insert to array, clobbers
                     }
                     break;
                     
                 default:
                     //exit case if exception happened
+                    serial.printf("Default case in processFn\r\n");
                     return out;
             }
         }
@@ -120,19 +127,28 @@
             *exposure = UPDATE_RATE;
     }
     
+    //zero pad the telemetry
+    for(i = frameLen; i < NUM_PIX; i++){
+        tele_linescan_diff[i] = 0;
+    }
+    
     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
-        //add width check later
-        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width){
+        //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) > 4){
             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);
     }
     
     return out;
@@ -143,8 +159,8 @@
                  DigitalOut *camSi, 
                  DigitalOut *camClk, 
                  int *exposure, 
-                 telemetry::NumericArray<uint16_t, 128> &tele_linescan,
-                 telemetry::NumericArray<uint16_t, NUM_PIX -  2 * SKIP> &tele_linescan_diff){
+                 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan,
+                 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
     uint16_t lineAry[NUM_PIX];
     float position;
     
@@ -157,6 +173,7 @@
     
     //process line scan data
     position  = processFn(lineAry, NUM_PIX, exposure, tele_linescan_diff);
+    //serial.printf("%.2f\r\n", position);
     
     return position;
 }