AnalogIn - use median of the sampling data for stabler input value

Dependencies:   mbed

AnalogIn2.cpp

Committer:
yamaguch
Date:
2011-06-20
Revision:
0:bbe67df5e586

File content as of revision 0:bbe67df5e586:

#include "AnalogIn2.h"

short int quickSelect(unsigned short arr[], int n);

AnalogIn2::AnalogIn2(PinName pinName) : pinName(pinName) {
    switch (pinName) {
        case p15:
            channel = 0;
            break;
        case p16:
            channel = 1;
            break;
        case p17:
            channel = 2;
            break;
        case p18:
            channel = 3;
            break;
        case p19:
            channel = 4;
            break;
        case p20:
            channel = 5;
            break;
    }
    read_u16(1);
}

unsigned short AnalogIn2::read_u16(int nSamples) {
    // power on, clk divider /4
    LPC_SC->PCONP |= (1 << 12);
    LPC_SC->PCLKSEL0 &= ~(0x3 << 24);

    // software-controlled ADC settings
    LPC_ADC->ADCR = (0 << 0) // SEL: 0 = no channels selected
                    | (25 << 8)    // CLKDIV: PCLK max ~= 25MHz, /25 to give safe 1MHz
                    | (0 << 16)    // BURST: 0 = software control
                    | (0 << 17)    // CLKS: not applicable
                    | (1 << 21)    // PDN: 1 = operational
                    | (0 << 24)    // START: 0 = no start
                    | (0 << 27);   // EDGE: not applicable

    switch (pinName) {
        case p15:// =p0.23 of LPC1768
            LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 14);
            LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 14;
            LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 14);
            LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 14;
            break;
        case p16:// =p0.24 of LPC1768
            LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 16);
            LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 16;
            LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 16);
            LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 16;
            break;
        case p17:// =p0.25 of LPC1768
            LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 18);
            LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 18;
            LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 18);
            LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 18;
            break;
        case p18:// =p0.26 of LPC1768:
            LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 20);
            LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 20;
            LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 20);
            LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 20;
            break;
        case p19:// =p1.30 of LPC1768
            LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 28);
            LPC_PINCON->PINSEL3 |= (unsigned int)0x3 << 28;
            LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 28);
            LPC_PINCON->PINMODE3 |= (unsigned int)0x2 << 28;
            break;
        case p20:// =p1.31 of LPC1768
            LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 30);
            LPC_PINCON->PINSEL3 |= (unsigned int)0x3 << 30;
            LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 30);
            LPC_PINCON->PINMODE3 |= (unsigned int)0x2 << 30;
            break;
    }

    // Repeatedly get the sample data until DONE bit
    unsigned short a[nSamples];
    for (int i = 0; i < nSamples; i++) {
        unsigned int data;
        // Select channel and start conversion
        LPC_ADC->ADCR &= ~0xFF;
        LPC_ADC->ADCR |= 1 << channel;
        LPC_ADC->ADCR |= 1 << 24;
        do {
            data = LPC_ADC->ADGDR;
        } while ((data & ((unsigned int)1 << 31)) == 0);
        // Stop conversion
        LPC_ADC->ADCR &= ~(1 << 24);

        a[i] = data & 65535;
    }

    return quickSelect(a, nSamples);
}

float AnalogIn2::read(int nSamples) {
    return (read_u16(nSamples) >> 4) / 4096.0;
}

AnalogIn2::operator float() {
    return read();
}

/*
 * This Quickselect routine is based on the algorithm described in
 * "Numerical recipes in C", Second Edition,
 * Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 * This code by Nicolas Devillard - 1998. Public domain. */

#define SWAP(a,b) {unsigned int t = (a); (a) = (b); (b) = t;}

short int quickSelect(unsigned short arr[], int n) {
    int low = 0, high = n - 1;
    unsigned int median = (low + high) / 2;

    for (;;) {
        if (high <= low) /* One element only */
            return arr[median];
        if (high == low + 1) { /* Two elements only */
            if (arr[low] > arr[high])
                SWAP(arr[low], arr[high]);
            return arr[median];
        }

        /* Find median of low, middle and high items; swap into position low */
        int middle = (low + high) / 2;
        if (arr[middle] > arr[high])
            SWAP(arr[middle], arr[high]);
        if (arr[low] > arr[high])
            SWAP(arr[low], arr[high]);
        if (arr[middle] > arr[low])
            SWAP(arr[middle], arr[low]);
        /* Swap low item (now in position middle) into position (low + 1) */
        SWAP(arr[middle], arr[low + 1]);

        /* Nibble from each end towards middle, swapping items when stuck */
        int ll = low + 1;
        int hh = high;
        for (;;) {
            do {
                ll++;
            } while (arr[low] > arr[ll]);
            do {
                hh--;
            } while (arr[hh] > arr[low]);
            if (hh < ll)
                break;
            SWAP(arr[ll], arr[hh]);
        }

        /* Swap middle item (in position low) back into correct position */
        SWAP(arr[low], arr[hh]);

        /* Re-set active partition */
        if (hh <= median)
            low = ll;
        if (hh >= median)
            high = hh - 1;
    }
}