Andrew Lindsay / MaxbotixDriver

Maxbotix ultrasonic distance sensor library

Fork of MaxbotixDriver by Daniel Casner

sonar.cpp@4:95f696f59d94, 2020-01-30 (annotated)

Committer:
SomeRandomBloke
Date:
Thu Jan 30 11:52:10 2020 +0000
Revision:
4:95f696f59d94
Parent:
3:c231deea4d36 
Added range averaging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DanielC 0:7e65f5077f5a 1 #include "sonar.h"
SomeRandomBloke 4:95f696f59d94 2 extern Serial pc;
DanielC 0:7e65f5077f5a 3
DanielC 0:7e65f5077f5a 4 Sonar::Sonar(PinName input, Timer& t) :
DanielC 0:7e65f5077f5a 5 interrupt(input),
DanielC 0:7e65f5077f5a 6 time(t),
DanielC 0:7e65f5077f5a 7 pulseStartTime(0),
DanielC 0:7e65f5077f5a 8 range(0) {
SomeRandomBloke 3:c231deea4d36 9 interrupt.rise(callback(this,&Sonar::pulseStart));
SomeRandomBloke 3:c231deea4d36 10 interrupt.fall(callback(this,&Sonar::pulseStop));
DanielC 0:7e65f5077f5a 11 }
DanielC 0:7e65f5077f5a 12
DanielC 0:7e65f5077f5a 13 int Sonar::read() {
DanielC 0:7e65f5077f5a 14 return range;
DanielC 0:7e65f5077f5a 15 }
DanielC 0:7e65f5077f5a 16
SomeRandomBloke 1:330989f98a6e 17 int Sonar::readCm() {
SomeRandomBloke 1:330989f98a6e 18 return range / 58; // 58uS per CM
SomeRandomBloke 1:330989f98a6e 19 }
SomeRandomBloke 1:330989f98a6e 20
DanielC 0:7e65f5077f5a 21 Sonar::operator int() {
DanielC 0:7e65f5077f5a 22 return read();
DanielC 0:7e65f5077f5a 23 }
DanielC 0:7e65f5077f5a 24
DanielC 0:7e65f5077f5a 25 void Sonar::pulseStart() {
DanielC 0:7e65f5077f5a 26 pulseStartTime = time.read_us();
DanielC 0:7e65f5077f5a 27 }
DanielC 0:7e65f5077f5a 28
DanielC 0:7e65f5077f5a 29 void Sonar::pulseStop() {
DanielC 0:7e65f5077f5a 30 int endTime = time.read_us();
SomeRandomBloke 4:95f696f59d94 31
DanielC 0:7e65f5077f5a 32 if (endTime < pulseStartTime) return; // Escape if there's been a roll over
SomeRandomBloke 1:330989f98a6e 33 range = (endTime - pulseStartTime); // / 58; // 58uS per CM
SomeRandomBloke 4:95f696f59d94 34 }
SomeRandomBloke 4:95f696f59d94 35
SomeRandomBloke 4:95f696f59d94 36
SomeRandomBloke 4:95f696f59d94 37 // Functions used in processing readings
SomeRandomBloke 4:95f696f59d94 38
SomeRandomBloke 4:95f696f59d94 39 /* isort - Simple sort function for the set of readings
SomeRandomBloke 4:95f696f59d94 40 * Sorting function (Author: Bill Gentles, Nov. 12, 2010)
SomeRandomBloke 4:95f696f59d94 41 * The sorted array is returned in the original array
SomeRandomBloke 4:95f696f59d94 42 * @param a - Array of unsigned 16 bit values to sort
SomeRandomBloke 4:95f696f59d94 43 * @param n - Number of values in array
SomeRandomBloke 4:95f696f59d94 44 */
SomeRandomBloke 4:95f696f59d94 45 void isort(int *a, int n)
SomeRandomBloke 4:95f696f59d94 46 {
SomeRandomBloke 4:95f696f59d94 47 for (int i = 1; i < n; ++i) {
SomeRandomBloke 4:95f696f59d94 48 int j = a[i];
SomeRandomBloke 4:95f696f59d94 49 int k;
SomeRandomBloke 4:95f696f59d94 50 for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
SomeRandomBloke 4:95f696f59d94 51 a[k + 1] = a[k];
SomeRandomBloke 4:95f696f59d94 52 }
SomeRandomBloke 4:95f696f59d94 53 a[k + 1] = j;
SomeRandomBloke 4:95f696f59d94 54 }
SomeRandomBloke 4:95f696f59d94 55 }
SomeRandomBloke 4:95f696f59d94 56
SomeRandomBloke 4:95f696f59d94 57 /** mode - Mode function, returning the mode or median.
SomeRandomBloke 4:95f696f59d94 58 * @param x - Array of unsigned 16 bit values to sort
SomeRandomBloke 4:95f696f59d94 59 * @param n - Number of values in array
SomeRandomBloke 4:95f696f59d94 60 * @return mode or median of the passed data.
SomeRandomBloke 4:95f696f59d94 61 */
SomeRandomBloke 4:95f696f59d94 62 int mode(int *x,int n)
SomeRandomBloke 4:95f696f59d94 63 {
SomeRandomBloke 4:95f696f59d94 64 int i = 0;
SomeRandomBloke 4:95f696f59d94 65 int count = 0;
SomeRandomBloke 4:95f696f59d94 66 int maxCount = 0;
SomeRandomBloke 4:95f696f59d94 67 int mode = 0;
SomeRandomBloke 4:95f696f59d94 68 int bimodal = 0;
SomeRandomBloke 4:95f696f59d94 69 int prevCount = 0;
SomeRandomBloke 4:95f696f59d94 70 while(i<(n-1)) {
SomeRandomBloke 4:95f696f59d94 71 prevCount=count;
SomeRandomBloke 4:95f696f59d94 72 count=0;
SomeRandomBloke 4:95f696f59d94 73 while( x[i]==x[i+1] ) {
SomeRandomBloke 4:95f696f59d94 74 count++;
SomeRandomBloke 4:95f696f59d94 75 i++;
SomeRandomBloke 4:95f696f59d94 76 }
SomeRandomBloke 4:95f696f59d94 77 if( count > prevCount & count > maxCount) {
SomeRandomBloke 4:95f696f59d94 78 mode=x[i];
SomeRandomBloke 4:95f696f59d94 79 maxCount=count;
SomeRandomBloke 4:95f696f59d94 80 bimodal=0;
SomeRandomBloke 4:95f696f59d94 81 }
SomeRandomBloke 4:95f696f59d94 82 if( count == 0 ) {
SomeRandomBloke 4:95f696f59d94 83 i++;
SomeRandomBloke 4:95f696f59d94 84 }
SomeRandomBloke 4:95f696f59d94 85 if( count == maxCount ) { //If the dataset has 2 or more modes.
SomeRandomBloke 4:95f696f59d94 86 bimodal=1;
SomeRandomBloke 4:95f696f59d94 87 }
SomeRandomBloke 4:95f696f59d94 88 if( mode==0 || bimodal==1 ) { // Return the median if there is no mode.
SomeRandomBloke 4:95f696f59d94 89 mode=x[(n/2)];
SomeRandomBloke 4:95f696f59d94 90 }
SomeRandomBloke 4:95f696f59d94 91 return mode;
SomeRandomBloke 4:95f696f59d94 92 }
SomeRandomBloke 4:95f696f59d94 93 return mode;
SomeRandomBloke 4:95f696f59d94 94 }
SomeRandomBloke 4:95f696f59d94 95
SomeRandomBloke 4:95f696f59d94 96 // Ultrasonic sensor code
SomeRandomBloke 4:95f696f59d94 97
SomeRandomBloke 4:95f696f59d94 98 /** getrange - Get the range in cm. Need to enable sensor first, waiting briefly for it to power up.
SomeRandomBloke 4:95f696f59d94 99 * Request temperature sensor to take a reading.
SomeRandomBloke 4:95f696f59d94 100 * Disable ultrasonic sensor after use for power saving.
SomeRandomBloke 4:95f696f59d94 101 * Returns uncompensated, compensated and temperature values by reference.
SomeRandomBloke 4:95f696f59d94 102 * Calling function can still do checks to determine whether to use reading or not.
SomeRandomBloke 4:95f696f59d94 103 *
SomeRandomBloke 4:95f696f59d94 104 * @param outRawDistance - Raw uncompensated distance
SomeRandomBloke 4:95f696f59d94 105 * @param outDistance - Compensated distance if temperature available, otherwise raw distance
SomeRandomBloke 4:95f696f59d94 106 * @param outTemperature - Temperature reading
SomeRandomBloke 4:95f696f59d94 107 * @return boolean true returned form function for valid reading. false for <23cm or out of range
SomeRandomBloke 4:95f696f59d94 108 */
SomeRandomBloke 4:95f696f59d94 109 bool Sonar::getRange( int *outRawDistance, int *outDistance, float temperature )
SomeRandomBloke 4:95f696f59d94 110 {
SomeRandomBloke 4:95f696f59d94 111 int pulse; // number of pulses from sensor
SomeRandomBloke 4:95f696f59d94 112 int8_t i=0;
SomeRandomBloke 4:95f696f59d94 113 // These values are for calculating a mathematical median for a number of samples as
SomeRandomBloke 4:95f696f59d94 114 // suggested by Maxbotix instead of a mathematical average
SomeRandomBloke 4:95f696f59d94 115 int8_t arraysize = 9; // quantity of values to find the median (sample size). Needs to be an odd number
SomeRandomBloke 4:95f696f59d94 116 //declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer
SomeRandomBloke 4:95f696f59d94 117 int pulsevalue[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
SomeRandomBloke 4:95f696f59d94 118 // float temperature = 0.0;
SomeRandomBloke 4:95f696f59d94 119
SomeRandomBloke 4:95f696f59d94 120 // Start Timer for Maxbotix ultrasonic sensor
SomeRandomBloke 4:95f696f59d94 121 time.reset();
SomeRandomBloke 4:95f696f59d94 122 time.start();
SomeRandomBloke 4:95f696f59d94 123
SomeRandomBloke 4:95f696f59d94 124 // SENSOR_ON;
SomeRandomBloke 4:95f696f59d94 125 // wait_ms(100); // Startup delay, datasheet says 175mS, but we have a temperature sensor to read
SomeRandomBloke 4:95f696f59d94 126 // Read temperature for compensation calculation
SomeRandomBloke 4:95f696f59d94 127 // ds18b20.startConversion();
SomeRandomBloke 4:95f696f59d94 128 // ds18b20.convertTemperature(true, DS1820::this_device); //Start temperature conversion, wait until ready, 9 bits takes 93.75mS
SomeRandomBloke 4:95f696f59d94 129 // temperature = ds18b20.temperature();
SomeRandomBloke 4:95f696f59d94 130
SomeRandomBloke 4:95f696f59d94 131 // Perform a quick dummy read
SomeRandomBloke 4:95f696f59d94 132 while( i++ < arraysize ) {
SomeRandomBloke 4:95f696f59d94 133 pulse = read();
SomeRandomBloke 4:95f696f59d94 134 wait_ms(50);
SomeRandomBloke 4:95f696f59d94 135 }
SomeRandomBloke 4:95f696f59d94 136
SomeRandomBloke 4:95f696f59d94 137 // Read temrperature
SomeRandomBloke 4:95f696f59d94 138 // temperature = ds18b20.read();
SomeRandomBloke 4:95f696f59d94 139 // *outTemperature = temperature;
SomeRandomBloke 4:95f696f59d94 140
SomeRandomBloke 4:95f696f59d94 141 i = 0;
SomeRandomBloke 4:95f696f59d94 142 //pc.printf("Reading sensor\n\r");
SomeRandomBloke 4:95f696f59d94 143 // pc.printf("Timer: %d \n\r",maxBotixTimer.read_us());
SomeRandomBloke 4:95f696f59d94 144
SomeRandomBloke 4:95f696f59d94 145 while( i < arraysize ) {
SomeRandomBloke 4:95f696f59d94 146 pulse = read();
SomeRandomBloke 4:95f696f59d94 147 if( pulse == 0 ) {
SomeRandomBloke 4:95f696f59d94 148 // SENSOR_OFF;
SomeRandomBloke 4:95f696f59d94 149 time.stop();
SomeRandomBloke 4:95f696f59d94 150 return 0;
SomeRandomBloke 4:95f696f59d94 151 }
SomeRandomBloke 4:95f696f59d94 152 if( pulse >= 1500 && pulse < 36000 )
SomeRandomBloke 4:95f696f59d94 153 pulsevalue[i++] = pulse; // ensure no values out of range
SomeRandomBloke 4:95f696f59d94 154 wait_ms(35); // wait between samples
SomeRandomBloke 4:95f696f59d94 155 }
SomeRandomBloke 4:95f696f59d94 156
SomeRandomBloke 4:95f696f59d94 157 // Turn off sensor as no longer needed
SomeRandomBloke 4:95f696f59d94 158 // SENSOR_OFF;
SomeRandomBloke 4:95f696f59d94 159 time.stop();
SomeRandomBloke 4:95f696f59d94 160
SomeRandomBloke 4:95f696f59d94 161 isort(pulsevalue,arraysize); // sort samples
SomeRandomBloke 4:95f696f59d94 162 int rawtof = mode(pulsevalue,arraysize); // get median
SomeRandomBloke 4:95f696f59d94 163 *outRawDistance = rawtof/58; // time of flight / 58uS for both there and back
SomeRandomBloke 4:95f696f59d94 164 *outDistance = (rawtof * (331.3f + 0.606f * temperature)) / 20000;
SomeRandomBloke 4:95f696f59d94 165
SomeRandomBloke 4:95f696f59d94 166 // Add check for validity of reading, between 27 and 600cm
SomeRandomBloke 4:95f696f59d94 167 return *outRawDistance > 26 && *outRawDistance < 601;
SomeRandomBloke 4:95f696f59d94 168 }