File content as of revision 4:95f696f59d94:

#include "sonar.h"
extern Serial pc;

Sonar::Sonar(PinName input, Timer& t) :
    interrupt(input),
    time(t),
    pulseStartTime(0),
    range(0) {
    interrupt.rise(callback(this,&Sonar::pulseStart));
    interrupt.fall(callback(this,&Sonar::pulseStop));
}

int Sonar::read() {
    return range;
}

int Sonar::readCm() {
    return range  / 58; // 58uS per CM
}

Sonar::operator int() {
    return read();
}

void Sonar::pulseStart() {
    pulseStartTime = time.read_us();
}

void Sonar::pulseStop() {
    int endTime = time.read_us();

    if (endTime < pulseStartTime) return; // Escape if there's been a roll over
    range = (endTime - pulseStartTime); //   / 58; // 58uS per CM
}


// Functions used in processing readings

/* isort - Simple sort function for the set of readings
 * Sorting function (Author: Bill Gentles, Nov. 12, 2010)
 * The sorted array is returned in the original array
 * @param a - Array of unsigned 16 bit values to sort
 * @param n - Number of values in array
 */
void isort(int *a, int n)
{
    for (int i = 1; i < n; ++i)  {
        int j = a[i];
        int k;
        for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
            a[k + 1] = a[k];
        }
        a[k + 1] = j;
    }
}

/** mode - Mode function, returning the mode or median.
 * @param x - Array of unsigned 16 bit values to sort
 * @param n - Number of values in array
 * @return mode or median of the passed data.
 */
int mode(int *x,int n)
{
    int i = 0;
    int count = 0;
    int maxCount = 0;
    int mode = 0;
    int bimodal = 0;
    int prevCount = 0;
    while(i<(n-1)) {
        prevCount=count;
        count=0;
        while( x[i]==x[i+1] ) {
            count++;
            i++;
        }
        if( count > prevCount & count > maxCount) {
            mode=x[i];
            maxCount=count;
            bimodal=0;
        }
        if( count == 0 ) {
            i++;
        }
        if( count == maxCount ) {      //If the dataset has 2 or more modes.
            bimodal=1;
        }
        if( mode==0 || bimodal==1 ) {  // Return the median if there is no mode.
            mode=x[(n/2)];
        }
        return mode;
    }
    return mode;
}

// Ultrasonic sensor code

/** getrange - Get the range in cm. Need to enable sensor first, waiting briefly for it to power up.
 * Request temperature sensor to take a reading.
 * Disable ultrasonic sensor after use for power saving.
 * Returns uncompensated, compensated and temperature values by reference.
 * Calling function can still do checks to determine whether to use reading or not.
 *
 * @param outRawDistance - Raw uncompensated distance
 * @param outDistance - Compensated distance if temperature available, otherwise raw distance
 * @param outTemperature - Temperature reading
 * @return boolean true returned form function for valid reading. false for <23cm or out of range
 */
bool Sonar::getRange( int *outRawDistance, int *outDistance, float temperature )
{
    int pulse;  // number of pulses from sensor
    int8_t i=0;
    // These values are for calculating a mathematical median for a number of samples as
    // suggested by Maxbotix instead of a mathematical average
    int8_t arraysize = 9; // quantity of values to find the median (sample size). Needs to be an odd number
    //declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer
    int pulsevalue[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
//    float temperature = 0.0;

    // Start Timer for Maxbotix ultrasonic sensor
    time.reset();
    time.start();

//    SENSOR_ON;
//    wait_ms(100);        // Startup delay, datasheet says 175mS, but we have a temperature sensor to read
    // Read temperature for compensation calculation
//    ds18b20.startConversion();
//    ds18b20.convertTemperature(true, DS1820::this_device); //Start temperature conversion, wait until ready, 9 bits takes 93.75mS
//    temperature = ds18b20.temperature();

    // Perform a quick dummy read
    while( i++ < arraysize ) {
        pulse = read();
        wait_ms(50);
    }

    // Read temrperature
//    temperature = ds18b20.read();
//    *outTemperature = temperature;

    i = 0;
//pc.printf("Reading sensor\n\r");
//    pc.printf("Timer: %d \n\r",maxBotixTimer.read_us());

    while( i < arraysize ) {
        pulse = read();
        if( pulse == 0 ) {
//            SENSOR_OFF;
            time.stop();
            return 0;
        }
        if( pulse >= 1500 && pulse < 36000 )
            pulsevalue[i++] = pulse;  // ensure no values out of range
        wait_ms(35);                      // wait between samples
    }

    // Turn off sensor as no longer needed
//    SENSOR_OFF;
    time.stop();

    isort(pulsevalue,arraysize);        // sort samples
    int rawtof = mode(pulsevalue,arraysize);  // get median
    *outRawDistance = rawtof/58;      // time of flight / 58uS for both there and back
    *outDistance = (rawtof * (331.3f + 0.606f * temperature)) / 20000;

    // Add check for validity of reading, between 27 and 600cm
    return *outRawDistance > 26 && *outRawDistance < 601;
}