Diff: sonar.cpp

Revision:

4:95f696f59d94

Parent:

3:c231deea4d36

--- a/sonar.cpp	Fri Jan 17 14:49:13 2020 +0000
+++ b/sonar.cpp	Thu Jan 30 11:52:10 2020 +0000
@@ -1,4 +1,5 @@
 #include "sonar.h"
+extern Serial pc;
 
 Sonar::Sonar(PinName input, Timer& t) :
     interrupt(input),
@@ -27,6 +28,141 @@
 
 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
-}
\ No newline at end of file
+}
+
+
+// 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;
+}