Measure distance using echo signal from Ultrasonic sensor

Question

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 <wiringPi.h>
#include <stdio.h>
#include <sys/time.h>
#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;
}

Answer 1

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 < 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.

• Iaroslav Karandashev: I love your implementation of ultrasonic_measure and clarification of raw_ultrasonic_measure, thanks!

Answer 2

Oops...

#define Trig 0
// ^ !!!
#define Echo 1
double raw_ultrasonic_measure(int trig, int echo) {
 // ^ !!!
 digitalWrite(Trig, LOW);
 // ^ !!!
 delayMicroseconds(1);
 // ...

Look at the capitalization. Your trig (and echo) function parameters are completely unused. That's why you #define constants to be ALL UPCASE normally.

Naming

Functions are usually named for what they do: "erase" something, "sort" some elements, "activate" some timer, ...

ultrasonic_measure (and it's raw variant) don't follow this style. They name what they return, not what they do in order to be able to return that value. How about measure_ultrasonic_distance? (measure used as a verb then.)

Answer 3

Take these comments together with the comments of others, not instead of them. They have already said much that needs saying.

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

The 5 lines above are basically a "ping" operation - you are generating a sonar ping and shaping it using the 1 and 10 constants. I'd suggest you write a parameterized ultrasonic_ping(duration) function.

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

I find your while loops difficult to read and comprehend. Please consider changing to something more left-to-right, and more explicit, like:

while (digitalRead(Echo) != 0) 
 EMPTY();

Finally, what does 1 mean? What does 0 mean when returned by digitalRead(Echo)? Could you use HIGH or LOW there? Or some other constant?

Why do you divide your time intervals by 100? Is that the speed of sound in microseconds per centimeter? A nice constant would help...

• Iaroslav Karandashev: 100 is experimentally found value, probably I need better idea.
• Harald Scheirich: Your suggestion for the empty while loop makes things more unreadable, now somebody reading the code has to guess that EMPTY() does nothing, rather than seeing an empty loop body.
• aghast: You could use /*EMPTY*/ instead. Or {}. Anything to make clear what's happening.