Go Down

Topic: HC-SR04 Code with Lots of bells and wistles (Read 109 times) previous topic - next topic

zhomeslice

This is a small compact function that handles everything for the HC-SR04 including averaging 100 readings it runs fast and has object detection as well as error value min and max it also has a signal loss mode that easily detects and handles failed pings.

Any Suggestions.
Advanced Code uses a Lambda inline functions passed to attachInterrupts callback function and interrupt 1 for echo and a ms clock on timer zero
Echo on pin 3
Trigger pin is set to 4 set to pulse every 10 ms. 
note: I have seen no ill affects pinging the trigger input on the HC-sr04 while waiting for return echo.

use Serial Plotter to best see results.


Thank you for looking at this code I'm looking for your suggestions and even criticism if you would like :)
Code: [Select]
#define PingPin 3
#define TriggerPin 4
#define DTSize 100// Number of readings to average
#define MaxPingRate 10// ms 100 times a second
//Global Variables
static byte nextIn = DTSize ;
volatile byte numReadings;
volatile uint16_t DeltaT[DTSize];
uint16_t DeltaTMin;
uint16_t DeltaTMax;
volatile uint8_t NewData = 1;
char Alert = 0;
uint16_t  Error;
volatile bool NoData;


volatile float SinplePing() {
  if (NoData) return (DeltaT[(nextIn + DTSize - 1) % DTSize]); // return the last known reading 5 Samples ago
  if (!NewData)return (0);
  NewData = 0;
  if (numReadings) {
    float sum = 0.0;
    DeltaTMin = DeltaTMax = DeltaT[nextIn];
    uint16_t Val;
    for (char i = 0; i < numReadings; i++) {
      Val = (float)DeltaT[(nextIn + DTSize  - i) % DTSize];
      sum += Val;
      if (i < 10) {
        DeltaTMin = min(Val, DeltaTMin);
        DeltaTMax = max(Val, DeltaTMax);
      }
    }
    Error = DeltaTMax - DeltaTMin;
    sum /= numReadings;

    if ((DeltaTMin > sum) || (DeltaTMax < sum)) {
      if(abs(DeltaT[nextIn ] - (float)DeltaT[(nextIn + DTSize  - 5) % DTSize]) > 5)  Alert = (DeltaTMin > sum) ? 1 : (DeltaTMax < sum) ? -1 : 0;
      numReadings = 10;
    }
    return sum;
  }

  static bool init;
  if (init) return (0); //if started return
  init = true;
  pinMode(3, INPUT);
  pinMode(4, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(PingPin), [] {
    static uint32_t EdgeTime;
    static byte SkipCtr = 5;
    uint32_t cTime = micros();
    if (digitalRead(PingPin)) {
      EdgeTime = cTime; //Pulse went HIGH store the start time
    } else {                         // Pulse Went low calculate the duratoin

      byte lastIn = nextIn;
      nextIn++;
      DeltaT[nextIn %= DTSize] = cTime - EdgeTime; // Store the ping reading in the next array spot
      if (NoData = (DeltaT[nextIn] > 50000) || (SkipCtr > 0 )) { // reading is out of bounds. a hardware timeout occured on the ping sensor resetting the echo pin.
        numReadings = max(numReadings - 10, 2); // shift the average by 20 due to false reading the output will eliminate the old readings in favor of the last 3 good readings (2+1) during the sensor failure
        nextIn = lastIn;// use the last reading to start the average
        ++SkipCtr %= 10; // count to 5 then back to 0
      } else {
        NewData = 1;
        if (numReadings < DTSize) numReadings++;
      }

    }
  }, CHANGE);
  NewData = 0;
  OCR0A = 0xAF;
  TIMSK0 |= _BV(OCIE0A);
  return (0);
}



// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  static byte ctr;
  ctr++;
  if (ctr >= MaxPingRate) {
    digitalWrite(TriggerPin, HIGH); // Trigger another pulse
    delayMicroseconds(5);
    digitalWrite(TriggerPin, LOW);
    ctr = 0;
  }
}

void setup() {
  Serial.begin(115200); //115200
  Serial.println("Ping Test");
}

void loop() {
  static int loopctr;
  loopctr++;
  unsigned long DeltaPingTime;
  static unsigned long SpamTimer;
  if ((DeltaPingTime = SinplePing()) && ((millis() - SpamTimer) >= (10))) {
    SpamTimer = millis();
    //  Serial.print(loopctr);
    //   Serial.print(",");
    Serial.print( microsecondsToCentimeters(DeltaPingTime), 2);
    Serial.print(",");
    Serial.print( microsecondsToCentimeters(DeltaT[nextIn]), 2);
    Serial.print(",");
    Serial.print( microsecondsToCentimeters(DeltaTMin), 2);
    Serial.print(",");
    Serial.print( microsecondsToCentimeters(DeltaTMax), 2);
    Serial.print(",");
    Serial.print( microsecondsToCentimeters(Error), 2);
    Serial.print(",");
    Serial.print( (Alert > 0) ? 20 : 12);
    Serial.print(",");
    Serial.print( (Alert < 0) ? 30 : 22);
    Serial.print(",");
    Serial.println(NoData ? 50 : 32);

    loopctr = 0;
    if (Alert != 0) {
      // Major change
      // Alert == 1 object appeared
      // Alert == -1 object dissapeared
      Alert = 0; // you must clear the alert or you could watch for it to change but no garantee opposite trigger will occure
    }
  }
}

float microsecondsToCentimeters(float microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return (float)microseconds / 58.77;
}


Thanks,
Z

HC

Go Up