Commits

Anonymous committed 57cef82

reorganizing file structure

Comments (0)

Files changed (25)

firmware/DerivFilter.cpp

     shift = _shift;
     scale = _scale;
     setWindowLen(_windowLen);
-    maxValueX = 0;
-    maxValueY = 0;
 }
 
 void DerivFilter::setWindowLen(uint16 _windowLen) {
 void DerivFilter::apply(uint8 *data, uint16 len) {
     int32 kNeg;
     int32 kPos;
+    uint16 maxValueX = 0;
+    uint16 maxValueY = 0;
     uint16 n = windowLen/2;
     uint8 dataFilt[len];
     float value;
         }
         dataFilt[i] = (uint8) value;
 
-        if (value > maxValueY) {
-            maxValueY = (uint16) value;
-            maxValueX = i;
+        // Search for the max if the index is in the searchable region.
+        if (i < constants::maxSearchPixel) {
+            if (value > maxValueY) {
+                maxValueY = (uint16) value;
+                maxValueX = i;
+            }
         }
     }
     setThreshold(maxValueX,maxValueY);

firmware/DerivFilter.h

         void setThreshold(uint16 x_val,uint16 y_val);
         uint16* getThreshold();
     private:
-        uint16 maxValueX;
-        uint16 maxValueY;
         uint16 threshold[2];
         uint8 shift;
         uint8 scale;

firmware/LevelDetector.cpp

     derivFilter.setShift(constants::derivFilterShift);
 }
 
-float LevelDetector::findLevel(uint8 *dataBuffer, int32* a, int32* b) {
+float LevelDetector::findLevel(uint8 *dataBuffer, int32* a, int32* b, float *lastLevel) {
     float level;
-    uint8* thresholds;
+    uint16* threshold;
 
-    level = findLevel(dataBuffer);
+    level = findLevel(dataBuffer, lastLevel);
     *a = indNeg;
     *b = indPos;
-    //thresholds = derivFilter.getThreshold();
-    //*a = (int32) thresholds[0];
-    //*b = (int32) thresholds[1];
+    //threshold = derivFilter.getThreshold();
+    //*a = (int32) threshold[0];
+    //*a = (int32) findRefLevel();
+    //*b = (int32) threshold[1];
     return level;
 }
 
-float LevelDetector::findLevel(uint8 *dataBuffer) {
+float LevelDetector::findLevel(uint8 *dataBuffer,float *lastLevel) {
     bool found;
     int32 indBegin;
     uint8 refLevel;
     float peakFit;
     float level;
     uint16* threshold;
+    uint8 thresholdDelta;
 
     // Copy data buffer and apply median and derivative filters
     if (!reverseBuffer) {
     }
     medianFilter.apply(workBuffer,numPixel);
     derivFilter.apply(workBuffer,numPixel);
+    lowpassFilter.apply(workBuffer,numPixel);
 
+    // The derivFilter object returns the x,y values
+    // of the maximum within the searchable region of the 
+    // data.  Note: whereas the deriFilter is applied to the
+    // entire workBuffer, the search for a max value only 
+    // takes place within the searchable region. 
     threshold = derivFilter.getThreshold();
 
-    upperThreshold = (uint8) (.95*threshold[1]);
-    lowerThreshold = (uint8) (.90*threshold[1]);
+    upperThreshold = (uint8) threshold[1];
+    lowerThreshold = (uint8) (constants::lowerThresholdFraction*threshold[1]);
 
     // Find reference level
-    refLevel = findRefLevel();
+    refLevel = constants::refLevel;
+    
+    // Compare the upperThreshold to the (baseline) refLevel.
+    // If below some arbitrary threshold, cannot find level.
+    //
+    // Adding hysteresis to this check.
+    //
+    //if (*lastLevel == levelNotFound) {
+    if (*lastLevel < 0) {
+        thresholdDelta = constants::thresholdDeltaHigh;
+    } else {
+        thresholdDelta = constants::thresholdDeltaLow;
+    }
 
-    if ((upperThreshold - refLevel) < constants::thresholdDelta) {
+    if ((upperThreshold - refLevel) < thresholdDelta) {
         return levelNotFound;
     }
 
-    // Find index first data point > upper threshold. This index 
-    // (indBegin) will be used to as the starting place for forward
-    // and backward searches for data points < lower threshold.
-    //indBegin = 0;
-    //found = false;
-    //while ((!found) && (indBegin < maxSearchPixel)) {
-        //if (workBuffer[indBegin] > (upperThreshold)) {
-            //found = true;
-        //}
-        //else {
-            //indBegin++;
-        //}
-    //}
-    //if (!found) {
-        //return levelNotFound;
-    //}
+    // Index corresponds to the x value of the upperThreshold as 
+    // found in the derivative data.
     indBegin = threshold[0];
 
     // Search backward until the first data point less than the lower 
         }
     }
     if (!found) {
-        return levelNotFound;
+        return levelNotFound-1;
     }
 
     // Search forward until the first data point less than the lower 
     // threshold.
     indPos = indBegin;
     found = false;
-    while ((!found) && (indPos < maxSearchPixel)) {
+    while ((!found) && (indPos < (constants::NUM_PIXEL-1))) {
         if (workBuffer[indPos] < (lowerThreshold)) {
             found = true;
         }
         }
     }
     if (!found) {
-        return levelNotFound;
+        return levelNotFound-2;
     }
 
+    // Symmetry check for indexes
+    //if (abs(indPos-2*indBegin+indNeg)>constants::thresholdSymm) {
+        //return levelNotFound-3;    
+    //}
+
     // Compute the mid point, delta and fit the peak with a quadratic
-    midPoint = (float) (indNeg/2 + indPos/2);
+    midPoint = 0.5*(float)(indNeg) + 0.5*(float)(indPos);
     //indDelta = (float) (indPos - indNeg);
     //peakFit = findPeak((uint16) indNeg, workBuffer+indNeg, indPos-indNeg+1);
 

firmware/LevelDetector.h

 #include "constants.h"
 #include "MedianFilter.h"
 #include "DerivFilter.h"
+#include "LowpassFilter.h"
 
 const extern float levelNotFound;
 const extern float levelChanError;
         bool reverseBuffer;
         uint8 workBuffer[constants::NUM_PIXEL];
         LevelDetector();
-        float findLevel(uint8 *data);
-        float findLevel(uint8 *data, int32* a, int32* b);
+        float findLevel(uint8 *data, float *lastLevel);
+        float findLevel(uint8 *data, int32* a, int32* b, float *lastLevel);
         int32 indNeg;
         int32 indPos;
     private:
         float peakFitTol;
         MedianFilter medianFilter;
         DerivFilter derivFilter;
+        LowpassFilter lowpassFilter;
         uint8 findRefLevel();
 };
 

firmware/LowpassFilter.cpp

+// LowpassFilter.h
+//
+// The LowpassFilter class provides a simple implementation of a lowpass filter. 
+//
+// Cisco Zabala, IO Rodeo.
+//
+#include "LowpassFilter.h"
+#include <string.h>
+
+LowpassFilter::LowpassFilter() {
+    _cutoff = constants::cutoff_freq;
+    _t_last = 0;
+}
+
+void LowpassFilter::apply(uint8* data, uint16 len) {
+    uint8 dataFilt[len];  
+    float tau;
+    float alpha;
+    int32 t;
+    int32 dt;
+
+    t = millis()*1000;
+    dt = t-_t_last;
+    _t_last = t;
+
+    dataFilt[0] = data[0];
+    for (uint16 i=1; i<len; i++) {
+        tau = 1.0/(2*M_PI*_cutoff);
+        alpha = dt/(dt  + tau);
+        _value = (1-alpha)*_value + alpha*data[i];
+        dataFilt[i] = (uint8) _value;
+    }
+    memcpy(data,dataFilt, len*sizeof(uint8));
+
+}
+
+void setCutoff(int32 _cutoff) {
+
+}

firmware/LowpassFilter.h

+// LowpassFilter.h
+//
+// The LowpassFilter class provides a simple implementation of a lowpass filter. 
+//
+// Cisco Zabala, IO Rodeo.
+//
+#define _USE_MATH_DEFINES
+#ifndef _LOWPASS_FILTER_H_
+#define _LOWPASS_FILTER_H_
+#include "WProgram.h"
+#include "constants.h"
+
+class LowpassFilter {
+    public:
+        LowpassFilter();
+        void apply(uint8 *data, uint16 len);
+        void setCutoff(int32 _cutoff);
+    private:
+        int32 _cutoff;
+        uint8 _value;
+        int32 _t_last;
+};
+
+#endif

firmware/MessageHandler.cpp

 }
 
 void MessageHandler::getWorkingBuffer() {
-    // NOT DONE 
-    SerialUSB << rspSuccess << endl;
-    sendWorkingBuffer();
+    uint16 mode;
+    uint8 channel;
+
+    mode = systemState.getMode();
+    channel = systemState.getChannel();
+
+    if (mode == sysModeDebug) {
+
+        SerialUSB << rspSuccess << " "; 
+        SerialUSB << systemState.getLevel(channel); 
+        SerialUSB << endl;
+
+        // Send pixel data as raw bytes
+        sendWorkingBuffer();
+        return;
+    }
+    SerialUSB << rspError << endl;
     return;
 }
 

firmware/SystemState.cpp

     channel = 0;
     for (uint8 i=0; i<constants::NUM_AIN; i++) {
         level[i] = levelNotFound; 
+        levelRaw[i] = levelNotFound;
     }
 }
 
 void SystemState::setLevel(uint8 chan, float value) {
     if (chan < constants::NUM_AIN) {
         level[chan] = value;
+        //if (fabs(levelRaw[chan] - value) < constants::levelMaxChange) {
+            //level[chan] = value;
+        //}
+        //else {
+            //level[chan] = levelNotFound;
+        //}
+        //levelRaw[chan] = value;
     }
 } 
 
 } 
 
 void SystemState::update() {
+    float levelTemp;
+    float lastLevel;
     linearArray.readData();
     switch (mode) {
         case sysModeSingleChannel:
             // Find level only for one channel
-            level[channel] = detector.findLevel(linearArray.buffer[channel]);
-            systemState.setLevel(channel,level[channel]);
+            lastLevel = level[channel];
+            levelTemp = detector.findLevel(linearArray.buffer[channel],&lastLevel);
+            setLevel(channel, levelTemp); // special setter - handles levelMaxChange
             break;
         case sysModeAllChannels:
             // Find level for all channels
             for (uint8 i=0; i<constants::NUM_AIN; i++) {
-                level[i] = detector.findLevel(linearArray.buffer[i]);
-                systemState.setLevel(i,level[i]);
+                lastLevel = level[i];
+                levelTemp = detector.findLevel(linearArray.buffer[i],&lastLevel);
+                setLevel(i, levelTemp);  // special setter - handles levelMaxChange
             }   
             break;
         case sysModeDebug:
             int32 a;
             int32 b;
-            level[channel] = detector.findLevel(linearArray.buffer[channel],&a,&b);
-            systemState.setLevel(channel,level[channel]);
-            systemState.setBounds(channel,a,b);
+            lastLevel = level[channel];
+            levelTemp = detector.findLevel(linearArray.buffer[channel],&a,&b,&lastLevel);
+            setLevel(channel, levelTemp);  // special setter - handles levelMaxChange
+            setBounds(channel,a,b);
         default:
             break;
     }  

firmware/SystemState.h

         uint16 mode;                     // Operating mode
         uint8 channel;                   // Channel setting for single channel operation
         float level[constants::NUM_AIN]; // Capillary level data
+        float levelRaw[constants::NUM_AIN]; 
+        float lastLevel;
         int32 bounds[constants::NUM_AIN][2]; // Debug data (bounds for level detection)
         LevelDetector detector;     // Level detector object  
 };

firmware/constants.cpp

     const uint8 normScaleFact[2] = {3,2};
 
     // Level detector constants
-    const uint8 thresholdDelta = 25; 
     const uint16 refLevelSampleNum = 20;
+    const uint8 refLevel = 128; 
     const bool reverseBuffer = false;
     const uint16 maxSearchPixel = NUM_PIXEL-167;
     const float peakFitTol = 0.25;
+    const uint8 thresholdDeltaLow = 15; 
+    const uint8 thresholdDeltaHigh = 28; 
+    const int32 thresholdSymm = 15;
+    const float lowerThresholdFraction = 0.95;
+    const float levelMaxChange = 50.0;
 
     // Median filter constants
     const uint16 medianFilterWindow = 25; 
     const uint8 derivFilterShift = 128;
     const uint8 derivFilterScale = 10;
 
+    // Lowpass filter constants
+    const int32 cutoff_freq = 10;
+
     // Device Id
     const uint16 deviceId = 1;
 }

firmware/constants.h

     extern const uint8 normScaleFact[2];
 
     // Level detector constants
-    extern const uint8 thresholdDelta; 
     extern const uint16 refLevelSampleNum;
+    extern const uint8 refLevel; 
     extern const bool reverseBuffer; 
     extern const uint16 maxSearchPixel;
     extern const float peakFitTol;
+    extern const uint8 thresholdDeltaLow; 
+    extern const uint8 thresholdDeltaHigh; 
+    extern const int32 thresholdSymm; 
+    extern const float lowerThresholdFraction;
+    extern const float levelMaxChange;
 
     // Median filter constants
     extern const uint16 medianFilterWindow; 
     extern const uint8 derivFilterShift;
     extern const uint8 derivFilterScale;
 
+    // Lowpass filter constants
+    extern const int32 cutoff_freq;
+
     extern const uint16 deviceId;
 }
 

python/array_reader.py

         line=self.readline()
         if not line.startswith(SUCCESS_CHR):
             raise IOError, 'unable to read pixel data'
+        line = line.split()
+        level = float(line[1])
 
         data = []
         errorCnt = 0
                 errorCnt += 1
             if errorCnt >= MAX_ERROR_CNT:
                 raise IOError, 'error reading pixel values'
-        return numpy.array(data)
+        return level, numpy.array(data)
 
     def getLevel(self,chan=None):
         """

python/debug/debug_data.py

+import sys,re
 import matplotlib
-matplotlib.use('GtkAgg')
 import pylab
 import numpy as np
-import sys
-import time
 
-with open('debug_data.txt','r') as f:
-    lines = f.readlines()
-upper = []
-lower = []
-t_sec = []
+with open('debug_data.txt') as f:
+    data = f.readlines()
+
+deriv_data = []
+time = []
 level = []
-for line in lines:
-    data_list = line.split(' ')
-    t = time.strptime(data_list[1],'%H:%M:%S')
-    t = time.mktime(t)
-    t_sec.append(t)
-    level.append(float(data_list[2]))
-    lower.append((int(data_list[3]),int(data_list[4])))
-    upper.append((int(data_list[5]),int(data_list[6]))) 
+deriv_data_line = []
+level_plot = []
+cnt = 0
+level_comp = 0
+for datum in data:
+    line = datum.split(' ')
+    if re.search('2012',line[0]):
+        if (len(deriv_data_line) > 0) and (level[-1]!=level_comp):
+            level_comp = level[-1]
+            y = np.array(deriv_data_line)
+            pylab.subplot(211)
+            pylab.plot(deriv_data_line)
+            deriv_data_line = []
+            level_plot.append(level_comp)
+            cnt+=1
+        time.append(' '.join(line[0:1]))
+        level.append(line[2])
+        line = (' '.join(line[3:]).replace('[','')).strip()
+        deriv_data_line = line.split()
+    else:
+        line = (' '.join(line[1:]).replace(']','')).strip()
+        deriv_data_line.extend(line.split())
+    if cnt > 5:
+        break
 
-lower = np.array(lower)
-upper = np.array(upper)
-level = np.array(level)
-t_sec = np.array(t_sec)
-
-mid_point = np.mean(np.array([lower[:,0],upper[:,0]]),axis=0)
-
-#pylab.plot(t_sec-t_sec[0],level,'r.')
-pylab.plot(t_sec-t_sec[0],mid_point,'b.')
-ax = pylab.gca()
-ax.set_ylim([190,220])
-
-pylab.xlabel('time (s)')
-pylab.grid(True)
+pylab.subplot(212)
+pylab.plot(level_plot,'ro')
 pylab.show()