Diff: rgb_sensor.cpp

Revision:

5:1fed2b68e661

Parent:

4:0ffadc2caaf6

Child:

6:fc64a14a2f4a

--- a/rgb_sensor.cpp	Fri Jun 27 12:14:36 2014 +0000
+++ b/rgb_sensor.cpp	Sat Jun 28 06:59:51 2014 +0000
@@ -106,6 +106,32 @@
         m_global->adc_irq();
 }
 
+int RGB_Sensor::filter(int sample)
+{
+    int a,b,x,y,z;
+    
+    /* get the two previous samples */
+    a = m_adc_filter[m_rgb_channel][0];
+    b = m_adc_filter[m_rgb_channel][1];
+
+    /* calculate node distances in triplet */
+    x = abs(a-sample);
+    y = abs(b-sample);
+    z = abs(a-b);
+
+    /* remember current sample */
+    m_adc_filter[m_rgb_channel][m_adc_filter_pos] = sample;
+    m_adc_filter_pos ^= 1;
+
+    /* choose edge with shortest distance and
+     * return average of the two edge nodes */
+    if((x<=y) && (x<=z))
+        return (a+sample)/2;
+    if((y<=x) && (y<=z))
+        return (b+sample)/2;
+    return (a+b)/2;
+}
+
 void RGB_Sensor::adc_irq(void)
 {
     uint32_t sample, status;
@@ -118,7 +144,8 @@
 
         if(!m_done)
         {
-            m_adc_aggregation[m_rgb_channel] += sample;
+            /* filter value to remove ADC noise/conversion errors */
+            m_adc_aggregation[m_rgb_channel] += filter(sample);
 
             m_adc_count++;
             if(m_adc_count>=RGB_OVERSAMPLING)
@@ -176,6 +203,9 @@
     m_done = false;
     m_adc_count = m_rgb_channel = 0;
     memset((void*)&m_adc_aggregation, 0, sizeof(m_adc_aggregation));
+    m_adc_filter_pos = 0;
+    memset(&m_adc_filter, 0, sizeof(m_adc_filter));
+
     /* start ADC burst mode */
     LPC_ADC->ADCR |= (1UL<<16);