Diff: ccdSensor.h

Revision:

47:df7a88cd249c

Parent:

45:c42166b2878c

Child:

48:058ace2aed1d

--- a/ccdSensor.h	Mon Feb 15 20:30:32 2016 +0000
+++ b/ccdSensor.h	Thu Feb 18 07:32:20 2016 +0000
@@ -28,14 +28,16 @@
 class PlungerSensorCCD: public PlungerSensor
 {
 public:
-    PlungerSensorCCD(
-        int nativePix, int highResPix, int lowResPix, 
-        PinName si, PinName clock, PinName ao1, PinName ao2) 
+    PlungerSensorCCD(int nativePix, PinName si, PinName clock, PinName ao1, PinName ao2)
         : ccd(nativePix, si, clock, ao1, ao2)
     {
-        this->highResPix = highResPix;
-        this->lowResPix = lowResPix;
-        this->pix = new uint16_t[highResPix];
+        // we don't know the direction yet
+        dir = 0;
+        
+        // we don't have contrast information from any prior images yet,
+        // so just peg the low and high brightness levels at the extremes
+        lastLo = 0x00;
+        lastHi = 0xff;
     }
     
     // initialize
@@ -46,158 +48,196 @@
         ccd.clear();
     }
     
-    // Perform a low-res scan of the sensor.  
-    virtual bool lowResScan(float &pos)
+    // Perform a high-res scan of the sensor.
+    virtual bool read(uint16_t &pos)
     {
-        // If we haven't sensed the direction yet, do a high-res scan
-        // first, so that we can get accurate direction data.  Return
-        // the result of the high-res scan if successful, since it 
-        // provides even better data than the caller requested from us.
-        // This will take longer than the caller wanted, but it should
-        // only be necessary to do this once after the system stabilizes
-        // after startup, so the timing difference won't affect normal
-        // operation.
-        if (dir == 0)
-            return highResScan(pos);
+        // start reading the next pixel array
+        ccd.startCapture();
         
-        // read the pixels at low resolution
-        ccd.read(pix, lowResPix);
+        // get the last image array
+        uint8_t *pix;
+        int n;
+        ccd.getPix(pix, n);
+        
+        // process it
+        int pixpos;
+        process(pix, n, pixpos);
         
-        // set the loop variables for the sensor orientation
-        int si = 0;
-        if (dir < 0)
-            si = lowResPix - 1;
+        // if we found the position, return it
+        if (pixpos >= 0)
+        {
+            // adjust for the reversed orientation if necessary
+            if (dir < 0)
+                pixpos = n - pixpos;
+                
+            // normalize to the 16-bit range
+            pos = uint16_t(((pixpos << 16) - pixpos) / n);
             
-        // Figure the shadow edge threshold.  Use the midpoint between
-        // the average levels of a few pixels at each end.
-        uint16_t shadow = uint16_t(
-            (long(pix[0]) + long(pix[1]) + long(pix[2])
-             + long(pix[lowResPix-1]) + long(pix[lowResPix-2]) + long(pix[lowResPix-3])
-            )/6);
+            // success
+            return true;
+        }
+        else
+        {
+            // no position found
+            return false;
+        }
+    }
         
-        // find the current tip position
-        for (int n = 0 ; n < lowResPix ; ++n, si += dir)
+    // Process an image.  Applies noise reduction and looks for edges.
+    // If we detect the plunger position, we set 'pos' to the pixel location
+    // of the edge; otherwise we set pos to -1.  If 'copy is true, we copy
+    // the noise-reduced pixel array back to the caller's pixel array, 
+    // otherwise we leave it unchanged.
+    void process(uint8_t *pix, int n, int &pos, bool copy = false)
+    {
+        // allocate a working buffer
+        uint8_t *tmp = (uint8_t *)malloc(n+2);
+        printf("processing - tmp=%lx\r\n", tmp);
+        
+        // find the low and high pixel values
+        int lo = 256, hi = 0;
+        for (int i = 0 ; i < n ; ++i)
         {
-            // check to see if we found the shadow
-            if (pix[si] <= shadow)
-            {
-                // got it - normalize to the 0.0..1.0 range and return success
-                pos = float(n)/lowResPix;
-                return true;
-            }
+            if (pix[i] < lo) lo = pix[i];
+            if (pix[i] > hi) hi = pix[i];
         }
         
-        // didn't find a shadow - return failure
-        return false;
-    }
-
-    // Perform a high-res scan of the sensor.
-    virtual bool highResScan(float &pos)
-    {
-        // read the array
-        ccd.read(pix, highResPix);
-
-        // Sense the orientation of the sensor if we haven't already.  If 
-        // that fails, we must not have enough contrast to find a shadow edge 
-        // in the image, so there's no point in looking any further - just
-        // return failure.
-        if (dir == 0 && !senseOrientation(highResPix))
-            return false;
+        for (int pass = 1 ; pass <= 2 ; ++pass)
+        {
+            // peg the pixels to their ranges with these parameters
+            pegPixels(pix, tmp, n, lo, hi);
+        
+            // count the edges
+            int nEdges;
+            findEdges(tmp, n, pos, nEdges);
+            
+            // if we found one edge, stop
+            if (nEdges == 1)
+            {
+                // If this is the first pass, we appear to have a good
+                // contrast level.  Note this for future use, in case the
+                // next image has insufficient contrast.
+                lastLo = lo;
+                lastHi = hi;
+                
+                // This also tells us the orientation.  If the bright
+                // end is at the 0th pixel, we're installed in the standard
+                // orientation (dir = 1), otherwise we're installed in the
+                // reverse orientation (dir = -1).
+                dir = (pix[0] == 255 ? 1 : -1);
+                
+                // use this result
+                break;
+            }
             
-        // Get the average brightness for a few pixels at each end.
-        long b1 = (long(pix[0]) + long(pix[1]) + long(pix[2]) + long(pix[3]) + long(pix[4])) / 5;
-        long b2 = (long(pix[highResPix-1]) + long(pix[highResPix-2]) + long(pix[highResPix-3])
-            + long(pix[highResPix-4]) + long(pix[highResPix-5])) / 5;
-
-        // Work from the bright end to the dark end.  VP interprets the
-        // Z axis value as the amount the plunger is pulled: zero is the
-        // rest position, and the axis maximum is fully pulled.  So we 
-        // essentially want to report how much of the sensor is lit,
-        // since this increases as the plunger is pulled back.
-        int si = 0;
-        long hi = b1;
-        if (dir < 0)
-            si = highResPix - 1, hi = b2;
-
-        // Figure the shadow threshold.  In practice, the portion of the
-        // sensor that's not in shadow has all pixels consistently near
-        // saturation; the first drop in brightness is pretty reliably the
-        // start of the shadow.  So set the threshold level to be closer
-        // to the bright end's brightness level, so that we detect the leading
-        // edge if the shadow isn't perfectly sharp.  Use the point 1/3 of
-        // the way down from the high top the low side, so:
-        //
-        //   threshold = lo + (hi - lo)*2/3
-        //             = lo + hi*2/3 - lo*2/3
-        //             = lo - lo*2/3 + hi*2/3
-        //             = lo*1/3 + hi*2/3
-        //             = (lo + hi*2)/3
-        //
-        // Now, 'lo' is always one of b1 or b2, and 'hi' is the other
-        // one, so we can rewrite this as:
-        long midpt = (b1 + b2 + hi)/3;
+            // we have other than one edge, so presume failure
+            pos = -1;
         
-        // If we have enough contrast, proceed with the scan.
-        //
-        // If the bright end and dark end don't differ by enough, skip this
-        // reading entirely.  Either we have an overexposed or underexposed frame,
-        // or the sensor is misaligned and is either fully in or out of shadow
-        // (it's supposed to be mounted such that the edge of the shadow always
-        // falls within the sensor, for any possible plunger position).
-        if (labs(b1 - b2) > 0x1000)
-        {
-            uint16_t *pixp = pix + si;           
-            for (int n = 0 ; n < highResPix ; ++n, pixp += dir)
+            // On the first pass, if we didn't find any edges, or we
+            // found more than one, make another pass with the brightness
+            // range from the *previous* image.  This helps us deal with
+            // images where the plunger is positioned so that entire sensor
+            // is entirely convered or uncovered, in which case we won't
+            // the image won't contain any natural contrast that would
+            // allow us to infer the exposure level automatically.  In
+            // these cases, we assume that the new image has a similar
+            // exposure level to the prior image.
+            if (pass == 1)
+            {
+                // first pass - try again with the previous exposure levels
+                lo = lastLo;
+                hi = lastHi;
+            }
+            else if (nEdges == 0)
             {
-                // if we've crossed the midpoint, report this position
-                if (long(*pixp) < midpt)
+                // There are no edges, and we're on the second pass, so
+                // we're already using an exposure range that worked on a
+                // previous exposure.  If the sensor is reading in the
+                // low end of the range, it must be entirely covered, which
+                // means that the plunger is all the way forward.  If it's
+                // at the high end of the range, the sensor must be entirely
+                // exposed, so the plunger is pulled all the way back.
+                // Report the end based on the known orientation.
+                if (dir != 0)
                 {
-                    // normalize to the 0.0..1.0 range and return success
-                    pos = float(n)/highResPix;
-                    return true;
+                    if (tmp[0] == 0)
+                    {
+                        // all dark - fully covered, plunger is all the way forward
+                        pos = dir > 0 ? 0 : n - 1;
+                    }
+                    else if (tmp[0] == 255)
+                    {
+                        // all bright - fully exposed, plunger is all the way back
+                        pos = dir > 0 ? n - 1 : 0;
+                    }
                 }
             }
         }
         
-        // we didn't find a shadow - return no reading
-        return false;
+        // if desired, copy the processed pixels back to the caller's array
+        if (copy)
+            memcpy(pix, tmp, n);
+            
+        // done with the temp array
+        delete [] tmp;
     }
     
-    // Infer the sensor orientation from the image data.  This determines 
-    // which end of the array has the brighter pixels.  In some cases it 
-    // might not be possible to tell: if the light source is turned off,
-    // or if the plunger is all the way to one extreme so that the entire
-    // pixel array is in shadow or in full light.  To sense the direction
-    // we need to have a sufficient difference in brightness between the
-    // two ends of the array to be confident that one end is in shadow
-    // and the other isn't.  On success, sets member variable 'dir' and
-    // returns true; on failure (i.e., we don't have sufficient contrast
-    // to sense the orientation), returns false and leaves 'dir' unset.
-    bool senseOrientation(int n)
+    // Peg each pixel to its third of the range
+    void pegPixels(const uint8_t *pix, uint8_t *tmp, int n, int lo, int hi)
     {
-        // get the total brightness for the first few pixels at
-        // each end of the array (a proxy for the average - just
-        // save time by skipping the divide-by-N)
-        long a = long(pix[0]) + long(pix[1]) + long(pix[2]) 
-            + long(pix[3]) + long(pix[4]);
-        long b = long(pix[n-1]) + long(pix[n-2]) + long(pix[n-3])
-            + long(pix[n-4]) + long(pix[n-5]);
+        // Figure the thresholds for the top third and bottom third
+        // of the brightness range.
+        int third = (hi - lo)/3;
+        int midHi = hi - third;
+        int midLo = lo + third;
+        
+        // Peg each pixel to its third of the range
+        for (int i = 0 ; i < n ; ++i)
+            tmp[i] = (pix[i] < midLo ? 0 : pix[i] > midHi ? 255 : 127);
             
-        // if the difference is too small, we can't tell
-        const long minPct = 10;
-        const long minDiff = 65535*5*minPct/100;
-        if (labs(a - b) < minDiff)
-            return false;
+        // Set up a circular buffer for a rolling 5-sample window.  To
+        // simplify the loop, fill in two fake pixels before the first
+        // one simply by repeating the first pixel in those slots.
+        uint8_t t[5] = { tmp[0], tmp[0], tmp[0], tmp[1], tmp[2] };
+        int a = 0;
+        
+        // Likewise, fill in two fake pixels at the end by copying the
+        // actual last pixel.
+        tmp[n] = tmp[n+1] = tmp[n-1];
+        int s = t[0] + t[1] + t[2] + t[3] + t[4];
+        
+        // Run through the array and peg each pixel to the consensus
+        // of its two neighbors to either side.  This smooths out noise
+        // by eliminating lone flipped pixels.
+        for (int i = 1 ; i < n ; ++i)
+        {
+            // apply the consensus vote to this pixel
+            tmp[i] = (s < 85*5 ? 0 : s > 382*5 ? 255 : 127);
             
-        // we now know the orientation - set the 'dir' member variable
-        // for future use
-        if (a > b)
-            dir = 1;
-        else
-            dir = -1;
-            
-        // success
-        return true;
+            // update the rolling window with the next sample
+            s -= t[0];
+            s += (t[a] = pix[i+3]);
+            a = (a + 1) % 5;
+        }
+    }
+    
+    // Find the edge(s)
+    void findEdges(const uint8_t *pix, int n, int &edgePos, int &nEdges)
+    {
+        // we don't have any edges yet
+        nEdges = 0;
+        edgePos = -1;
+        
+        // loop over the pixels looking for edges
+        int prv = pix[0], cur = pix[1], nxt = pix[2];
+        for (int i = 1 ; i < n - 1 ; prv = cur, cur = nxt, nxt = pix[++i + 1])
+        {
+            if (cur != prv)
+            {
+                ++nEdges;
+                edgePos = i;
+            }
+        }
     }
     
     // Send an exposure report to the joystick interface.
@@ -207,16 +247,34 @@
     //    0x02  -> low res scan (default is high res scan)
     virtual void sendExposureReport(USBJoystick &js, uint8_t mode)
     {
-        // get the number of pixels according to the high res/low res mode
-        int npix = (mode & 0x02 ? lowResPix : highResPix);
-
         // do a scan
-        ccd.read(pix, npix);
+        ccd.startCapture();
+        
+        // get the last pixel array
+        uint8_t *pix;
+        int n;
+        ccd.getPix(pix, n);
+        
+        // apply processing if desired
+        int pos = -1;
+        if (mode & 0x01)
+            process(pix, n, pos, true);
+        
+        // if a low-res scan is desired, reduce to a subset of pixels
+        if (mode & 0x02)
+        {
+            int lowResPix = 128;
+            int skip = n / lowResPix;
+            int src, dst;
+            for (src = skip, dst = 1 ; dst < lowResPix ; ++dst, src += skip)
+                pix[dst] = pix[src];
+            n = dst;
+        }
         
         // send reports for all pixels
         int idx = 0;
-        while (idx < npix)
-            js.updateExposure(idx, npix, pix);
+        while (idx < n)
+            js.updateExposure(idx, n, pix);
             
         // The pixel dump requires many USB reports, since each report
         // can only send a few pixel values.  An integration cycle has
@@ -225,26 +283,26 @@
         // the integration won't be comparable to a normal cycle.  Throw
         // this one away by doing a read now, and throwing it away - that 
         // will get the timing of the *next* cycle roughly back to normal.
-        ccd.read(pix, highResPix);
+        ccd.startCapture();
     }
     
 protected:
-    // Pixel buffer.  We allocate this to be big enough for the high-res scan,
-    // which is the most pixels we'll need to capture.
-    uint16_t *pix;
-
-    // number of pixels in a high-res scan
-    int highResPix;
-    
-    // number of pixels in a low-res scan
-    int lowResPix;
-    
     // Sensor orientation.  +1 means that the "tip" end - which is always
     // the brighter end in our images - is at the 0th pixel in the array.
     // -1 means that the tip is at the nth pixel in the array.  0 means
     // that we haven't figured it out yet.
     int dir;
     
+    // High and low brightness levels from last successful image.  We keep
+    // track of these so that we can apply them to any images we take with
+    // insufficient contrast to detect an edge.  We assume in these cases
+    // that the plunger is positioned so that the sensor is entirely in
+    // shadow or entirely in light.  We assume that the exposure level is
+    // roughly the same as the previous frame where we did find an edge,
+    // so we use the last frame's levels to determine whether the uniform
+    // brightness we're seeing is shadow or light.
+    uint8_t lastLo, lastHi;
+    
 public:
     // the low-level interface to the CCD hardware
     TSL1410R ccd;
@@ -256,7 +314,7 @@
 {
 public:
     PlungerSensorTSL1410R(PinName si, PinName clock, PinName ao1, PinName ao2) 
-        : PlungerSensorCCD(1280, 256, 80, si, clock, ao1, ao2)
+        : PlungerSensorCCD(1280, si, clock, ao1, ao2)
     {
     }
 };
@@ -266,7 +324,7 @@
 {
 public:
     PlungerSensorTSL1412R(PinName si, PinName clock, PinName ao1, PinName ao2)
-        : PlungerSensorCCD(1536, 256, 128, si, clock, ao1, ao2)
+        : PlungerSensorCCD(1536, si, clock, ao1, ao2)
     {
     }
 };