Measure distance using echo signal from Ultrasonic sensor I was not happy with original code for Ultrasonic sensor and decided to write my own implementation.

I managed to do this, but it still looks weird. Could you help me to improve it?

#include &lt;wiringPi.h&gt;
#include &lt;stdio.h&gt;
#include &lt;sys/time.h&gt;

#define Trig    0
#define Echo    1

double raw_ultrasonic_measure(int trig, int echo) {
  digitalWrite(Trig, LOW);
  delayMicroseconds(1);

  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);

  while(!(digitalRead(Echo) == 1));
  int time1 = micros();

  while(!(digitalRead(Echo) == 0));
  int time2 = micros();
  return (double)(time2 - time1) / 100;
}

double ultrasonic_measure(int trig, int echo) {
  double value1 = raw_ultrasonic_measure(trig, echo);
  delay(1);
  double value2 = raw_ultrasonic_measure(trig, echo);
  delay(1);
  double value3 = raw_ultrasonic_measure(trig, echo);
  delay(1);
  double value4 = raw_ultrasonic_measure(trig, echo);
  delay(1);
  double value5 = raw_ultrasonic_measure(trig, echo);
  return (value1 + value2 + value3 + value4 + value5) / 5;
} 

int main(void) {
  wiringPiSetup();
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);

  while(1){
    double value = raw_ultrasonic_measure(Trig, Echo);
    printf("%0.1f cm\n", value);
    delay(1000);
  }

  return 0;
}
raw_ultrasonic_measure measures the duration of an echo staying high. It is not related to the distance in any way, it is just a duration of trig signal.

You need a time elapsed from the beginning of a signal to reception of echo - that is, a duration of echo being low.

In a quick and dirty way,

        signal_start = micros();
        digitalWrite(Trig, HIGH);

        while (digitalRead(Echo) == 0) {
            ;
        }

        echo_received = micros();
        digital_write(Trig, LOW);

        return (echo_received - signal_start) / (2 * speed_of_sound);


In a real world application you would want to debounce echo, provide for a timeout, etc.

You may also want to account for the time taken by a first call to digitalWrite, but I doubt it will make any perceptible difference.



ultrasonic_measure is in fact a loop:

        double value = 0.0;
        for (int i = 0; i &lt; 5; i++) {
            value += raw_ultrasonic_measure(Trig, Echo);
        }
        return value / 5;


One benefit of such approach is an ability to pass a number of samples as a parameter.