Commits

iorodeo committed 2f81957

Added heuristic for detecting bad samples when reading from ADXL345 device.
They only happed occasionally - currently I just throw them out.

  • Participants
  • Parent commits 0e4eee2

Comments (0)

Files changed (8)

File firmware/accel_adxl345/AccelerometerBuf.h

 #include <WProgram.h>
 #include <ByteBuffer.h>
 #include <FastADXL345.h>
+//#include <ADXL345.h>
 
 class AccelerometerBuf {
 

File firmware/accel_adxl345/SystemState.cpp

 SystemState::SystemState() {
     mode = MODE_STOPPED;
     timerPeriod = defaultTimerPeriod;
-    range = defaultRange;
+    setRange(defaultRange);
+    badSampleCount = 0;
 }
 
 void SystemState::init() {
             test = true;
         }
     }
-    // Set range value
+    // Set range and maxValue
     if (test) {
         range = value;
     }

File firmware/accel_adxl345/SystemState.h

         unsigned long getTimerPeriod();
         void setRange(unsigned int value);
         unsigned int getRange();
-
+        unsigned int badSampleCount;
     private:
         unsigned long timerPeriod; 
         unsigned int range;

File firmware/accel_adxl345/accel_adxl345.pde

  */ 
 #include <FastWire.h>
 #include <FastADXL345.h>
+//#include <Wire.h>
+//#include <ADXL345.h>
 #include <Streaming.h>
 #include <util/atomic.h>
 #include <TimerOne.h>
 AccelerometerBuf buffer;             
 SystemState state; 
 SerialReceiver receiver;
+bool isFirst = true;
 
 // Initialize serial port, I2C communications, setup accelerometer, acceleromter 
 //  buffer and timer
     Serial.begin(baudRate);
 
     Wire.begin();
+    // Disable internal pullups
+    digitalWrite(A4,LOW);
+    digitalWrite(A5,LOW);
     accel = ADXL345();
 
     buffer.init(bufferSize);
             ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
                 state.mode = MODE_STOPPED;
             }
+            isFirst = true;
             buffer.clear();
             break;
 
         case CMD_START_STREAM:
             ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
                 state.mode = MODE_STREAMING;
+                isFirst = true;
             }
             break;
 
         case CMD_GET_SAMPLE:
             if (state.mode == MODE_STOPPED) {
                 raw = accel.ReadRawAxis();
-                Serial << raw.XAxis << "," << raw.YAxis << "," << raw.ZAxis << endl;
+                Serial << raw.XAxis << " " << raw.YAxis << " " << raw.ZAxis << endl;
+                //Serial << 11 << " " << 22 << " " << 33 << endl;
             }
             break;
 
             }
             break;
 
+        case CMD_GET_BAD_SAMPLE_COUNT:
+            if (state.mode == MODE_STOPPED) {
+                Serial << state.badSampleCount << endl;
+            }
+            break;
+
         default:
             break;
     }
 // Sends accelerometer data to the host PC
 void sendAccelData() {
     unsigned int sendCnt=0;
+    int maxValue;
     static AccelerometerRaw raw;
+    static AccelerometerRaw rawlast;
 
     while ((buffer.getSize() > 0) && (sendCnt < maxSendCnt)) {
         raw = buffer.getVal();
         Serial << _BYTE(lowByte(raw.ZAxis)) << _BYTE(highByte(raw.ZAxis));
         Serial << _BYTE(0); // bit to check that data is in sync.
         sendCnt++;
+        isFirst = false;
     }
 }
 
 // Interrupt serive routine for timer1 overflow. Reads data from the accelerometer
 // and puts it into the accelerometer buffer.
 void timerCallback() {
-    static AccelerometerRaw raw;
+    bool test = true;
+    AccelerometerRaw raw0;
+    AccelerometerRaw raw1;
+    AccelerometerRaw rawMedian;
+    unsigned long t0;
+    unsigned long t1;
+    unsigned long t2;
+
     if ((accel.IsConnected) && (state.mode == MODE_STREAMING)) { 
         sei();
-        raw = accel.ReadRawAxis();
-        buffer.putVal(raw);
-        //Serial << raw.XAxis << endl;
-        //Serial << raw.YAxis << endl;
-        //Serial << raw.ZAxis << endl;
+        // Take two measurements ... and compare them
+        raw0 = accel.ReadRawAxis();
+        raw1 = accel.ReadRawAxis();
+        if (abs(raw0.XAxis - raw1.XAxis) > maxSampleDiff) {
+            test=false;
+        }
+        if (abs(raw0.YAxis - raw1.YAxis) > maxSampleDiff) {
+            test=false;
+        }
+        if (abs(raw0.ZAxis - raw1.ZAxis) > maxSampleDiff) {
+            test=false;
+        }
+        if (test) {
+            buffer.putVal(raw0);
+        }
+        else {
+            state.badSampleCount++;
+        }
+        //Serial << raw0.XAxis << " " << raw1.XAxis << endl;
+        //Serial << raw0.YAxis << " " << raw1.YAxis << endl;
+        //Serial << raw0.ZAxis << " " << raw1.ZAxis << endl;
+        //Serial << raw0.XAxis - raw1.XAxis << endl;
+        //Serial << raw0.YAxis - raw1.YAxis << endl;
+        //Serial << raw0.ZAxis - raw1.ZAxis << endl;
+        //Serial << endl;
     }
 }
 
+int medianOfThree(int val0, int val1, int val2) {
+    int vals[3];
 
+}
 
+
+

File firmware/accel_adxl345/constants.h

 const unsigned int allowedRange[] = {2,4,8,16};
 const unsigned int defaultRange = 16;
 
+const int maxSampleDiff = 200;
+
 // Enumss
 enum OperatingMode {
     MODE_STOPPED, 
     CMD_GET_RANGE,
     CMD_GET_SAMPLE,
     CMD_GET_MAX_TIMER_PERIOD,
-    CMD_GET_MIN_TIMER_PERIOD
+    CMD_GET_MIN_TIMER_PERIOD,
+    CMD_GET_BAD_SAMPLE_COUNT
 };
 
 #endif

File python/accel_adxl345/accel_adxl345.py

                 'get_sample'           : 6,
                 'get_max_timer_period' : 7,
                 'get_min_timer_period' : 8,
+                'get_bad_sample_count' : 9,
                 }
 
         # Allowed accelerations ranges and scale factors
             self.flushInput()
             time.sleep(BUF_EMPTY_DT)
 
+
     def checkAccelRange(self,value):
         """
         Check if the value is within the allowed range set.
         dt = self.readFloat()
         return dt 
 
+    def getBadSampleCount(self):
+        """
+        Returns the number of bad/corrupted samples.
+        """
+        cmd = '[{0}]\n'.format(self.cmd_id['get_bad_sample_count'])
+        self.sendCmd(cmd)
+        val = self.readInt()
+        return val
+
     def setSampleDt(self,dt):
         """
         Sets the sample interval in microseconds.

File python/examples/peek.py

 """
 peek.py - demonstrates how to grab single readings from the accelerometer
 """
+import time
 from accel_adxl345 import AccelADXL345
 
 port = '/dev/ttyUSB0'
 if 0:
     print dev.peekValue()
 
-if 1:
+if 0:
     for i in range(0,100):
         print dev.peekValue()
+if 1:
+    t0 = time.time()
+    while 1:
+        vals = dev.peekValue()
+        print '%1.2f, %1.2f, %1.2f'%(vals[0], vals[1], vals[2])
 
+

File python/gui/accel_sensor.py

                 self.axes[i].set_ylim(minData,maxData)
 
             self.mpl.canvas.fig.canvas.draw()
+            print '# bad samples:', self.dev.getBadSampleCount()
         else:
             pass