rgb_sensor.cpp

00001 /* Discrete RGB color sensor
00002  *
00003  * - uses single-channel light-dependent resistor (via ADC)
00004  *   and a RGB LED.
00005  * -  compensates background light
00006  *
00007  * Copyright (c) 2014 ARM Limited
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014  *
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #include <mbed.h>
00023 #include <pinmap.h>
00024 #include "rgb_sensor.h"
00025 
00026 static const PinMap RGB_Sensor__PinMap_ADC[] = {
00027     {P0_23, ADC0_0, 1},
00028     {P0_24, ADC0_1, 1},
00029     {P0_25, ADC0_2, 1},
00030     {P0_26, ADC0_3, 1},
00031     {P1_30, ADC0_4, 3},
00032     {P1_31, ADC0_5, 3},
00033     {P0_2,  ADC0_7, 2},
00034     {P0_3,  ADC0_6, 2},
00035     {NC,    NC,     0}
00036 };
00037 
00038 DigitalOut g_dbg(LED1);
00039 
00040 /* initialize globally */
00041 RGB_Sensor* RGB_Sensor::m_global = NULL;
00042 
00043 RGB_Sensor::RGB_Sensor(PinName red, PinName green, PinName blue, PinName adc)
00044     :m_red(red), m_green(green), m_blue(blue)
00045 {
00046     uint32_t clkdiv;
00047 
00048     m_done=true;
00049 
00050     /* allow only one instance */
00051     if(m_global)
00052     {
00053         m_adc_channel = (ADCName)NC;
00054         return;
00055     }
00056 
00057     /* disable LED's */
00058     m_red   = !RGB_LED_ON;
00059     m_green = !RGB_LED_ON;
00060     m_blue  = !RGB_LED_ON;
00061 
00062     // enable ADC power
00063     LPC_SC->PCONP |= (1<<12);
00064     // set ADC clock to PCLK
00065     LPC_SC->PCLKSEL0 = (LPC_SC->PCLKSEL0 & ~(0x3UL<<24)) | (0x1UL<<24);
00066 
00067     // determine mapping
00068     m_adc_channel = (ADCName)pinmap_peripheral(adc, RGB_Sensor__PinMap_ADC);
00069     if (m_adc_channel == (ADCName)NC)
00070         return;
00071 
00072     // initialize ADC
00073     clkdiv = ((SystemCoreClock+RGB_MAX_ADC_CLK-1)/RGB_MAX_ADC_CLK)-1;    
00074     LPC_ADC->ADCR = (1UL<<m_adc_channel) | (clkdiv<<8) | (1UL<<21);
00075 
00076     // set up ADC IRQ
00077     NVIC_SetVector(ADC_IRQn, (uint32_t)&__adc_irq);
00078     LPC_ADC->ADINTEN = (1UL<<m_adc_channel);
00079     NVIC_EnableIRQ(ADC_IRQn);
00080 
00081     // propagate pin setting
00082     pinmap_pinout(adc, RGB_Sensor__PinMap_ADC);
00083 
00084     // remember this instance
00085     m_global = this;
00086 }
00087 
00088 RGB_Sensor::~RGB_Sensor(void)
00089 {
00090     /* only deal with fully initialized objects */
00091     if(m_adc_channel == (ADCName)NC)
00092         return;
00093     /* wait for completion */
00094     wait();
00095     /* reset global reference */
00096     m_global = NULL;
00097     /* turn off ADC */
00098     LPC_ADC->ADINTEN = 0;
00099     LPC_ADC->ADCR = 0;
00100     LPC_SC->PCONP &= ~(1UL<<12);
00101 }
00102 
00103 void RGB_Sensor::__adc_irq(void)
00104 {
00105     if(m_global)
00106         m_global->adc_irq();
00107 }
00108 
00109 int RGB_Sensor::filter(int sample)
00110 {
00111     int a,b,x,y,z;
00112 
00113     /* get the two previous samples */
00114     a = m_adc_filter[m_rgb_channel][0];
00115     b = m_adc_filter[m_rgb_channel][1];
00116 
00117     /* calculate node distances in triplet */
00118     x = abs(a-sample);
00119     y = abs(b-sample);
00120     z = abs(a-b);
00121 
00122     /* remember current sample */
00123     m_adc_filter[m_rgb_channel][m_adc_filter_pos] = sample;
00124 
00125     /* choose edge with shortest distance and
00126      * return average of the two edge nodes */
00127     if((x<=y) && (x<=z))
00128         return (a+sample)/2;
00129     if((y<=x) && (y<=z))
00130         return (b+sample)/2;
00131     return (a+b)/2;
00132 }
00133 
00134 void RGB_Sensor::adc_irq(void)
00135 {
00136     int sample;
00137     uint32_t status;
00138 
00139     status = LPC_ADC->ADSTAT;
00140     if(status & (1UL<<m_adc_channel))
00141     {
00142         /* always read sample to acknowledge IRQ */
00143         sample = (((&LPC_ADC->ADDR0)[m_adc_channel])>>4) & RGB_MASK;
00144 
00145         if(!m_done)
00146         {
00147             /* filter value to remove ADC noise/conversion errors */
00148             sample = filter(sample);
00149             if(m_adc_count>=RGB_SENSOR_IGNORE)
00150                 m_adc_aggregation[m_rgb_channel] += sample;
00151 
00152             m_adc_count++;
00153             if(m_adc_count>=(RGB_OVERSAMPLING+RGB_SENSOR_IGNORE))
00154             {
00155                 m_adc_count=0;
00156                 m_rgb_channel++;
00157 
00158                 /* prepare LEDs for upcoming channel */
00159                 switch(m_rgb_channel)
00160                 {
00161                     case 1:
00162                         m_red = RGB_LED_ON;
00163                         break;
00164 
00165                     case 2:
00166                         m_red = !RGB_LED_ON;
00167                         m_green = RGB_LED_ON;
00168                         break;
00169 
00170                     case 3:
00171                         m_green = !RGB_LED_ON;
00172                         m_blue = RGB_LED_ON;
00173                         break;
00174 
00175                     default:
00176                         m_blue = !RGB_LED_ON;
00177                         m_rgb_channel = 0;
00178 
00179                         if(!m_callback)
00180                             m_done = true;
00181                         else
00182                         {
00183                             TRGB rgb;
00184                             convert(rgb);
00185                             m_done = !m_callback(rgb);
00186                         }
00187                         /* stop data aquisition */
00188                         if(m_done)
00189                             LPC_ADC->ADCR &= ~(1UL<<16);
00190                         else
00191                         {
00192                             m_adc_filter_pos ^= 1;
00193                             memset(&m_adc_aggregation, 0, sizeof(m_adc_aggregation));
00194                         }
00195                 }                        
00196             }
00197         }
00198     }
00199     LPC_ADC->ADSTAT = status;
00200 }
00201 
00202 bool RGB_Sensor::capture(TRGB_Callback callback)
00203 {
00204     /* ignore mis-configurations */
00205     if(m_adc_channel==(ADCName)NC)
00206         return false;
00207 
00208     m_callback = callback;
00209     m_done = false;
00210     m_adc_filter_pos = m_adc_count = m_rgb_channel = 0;
00211     memset((void*)&m_adc_aggregation, 0, sizeof(m_adc_aggregation));
00212     memset(&m_adc_filter, 0, sizeof(m_adc_filter));
00213 
00214     /* start ADC burst mode */
00215     LPC_ADC->ADCR |= (1UL<<16);
00216 
00217     return true;
00218 }
00219 
00220 bool RGB_Sensor::wait(void)
00221 {
00222     /* ignore mis-configurations */
00223     if(m_adc_channel==(ADCName)NC)
00224         return false;
00225 
00226     while(!m_done)
00227         __WFE();
00228 
00229     return true;
00230 }
00231 
00232 void RGB_Sensor::convert(TRGB &rgb)
00233 {
00234     int i, sample;
00235 
00236     /* correct "DC" offset by subdstracting
00237      * environment light
00238      */
00239     for(i=0; i<3; i++)
00240     {
00241         sample = m_adc_aggregation[0] - m_adc_aggregation[i+1];
00242         rgb.data[i] = (sample<0) ? 0 : sample;
00243     }
00244 } 
00245 
00246 bool RGB_Sensor::capture(TRGB &rgb)
00247 {
00248     if(!(capture(NULL) && wait()))
00249         return false;
00250 
00251     convert(rgb);
00252     return true;  
00253 }