Maxbotix ultrasonic distance sensor library
Fork of MaxbotixDriver by
sonar.cpp
- Committer:
- SomeRandomBloke
- Date:
- 2020-01-30
- Revision:
- 4:95f696f59d94
- Parent:
- 3:c231deea4d36
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; }