Speed measurement with ultrasonic sensor HC-SR04 (C++)

This tutorial guides you through calculating a relative velocity by using the distance measurement of the HC-SR04 sensor over the time.

In order to be able to measure a speed at all, we must first be able to measure a distance to an object. Ultimately, the ultrasonic sensor can do no more. Therefore, first read Distance measurement with ultrasonic sensor HC-SR04 (C++).

Calculating the speed

Let's take another closer look at the triangle from the previous chapter:

So we need

Speed = Distance / Time

However, we do not want to go to the speed of sound now and check whether there is a deviation here.

I will show you how to measure the speed of movement of an object using this sensor. For that purpose we need to take two distance measurements in a short time apart and we have:

distance2 - distance1 = distance speed at a given time

If we make the measurements in a time period of 1 second, then we get the speed of movement of the object in cm/s.

When the object is moving in the opposite direction, the speed represented on the display has a negative sign.

Programming the speed measurement

We change our code from the previous example as follows:

#include <iostream>
#include <wiringPi.h>
#include "libHCSR04.h"

HCSR04::HCSR04(){}

void HCSR04::init(int trigger, int echo)
{
    this->trigger=trigger;
    this->echo=echo;
    pinMode(trigger, OUTPUT);
    pinMode(echo, INPUT);
    digitalWrite(trigger, LOW);
    delay(500);
}

double HCSR04::distance(int timeout)
{
    delay(10);

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

    now=micros();

    while (digitalRead(echo) == LOW && micros()-now<timeout);
        recordPulseLength();

    travelTimeUsec = endTimeUsec - startTimeUsec;
    distanceMeters = 100*((travelTimeUsec/1000000.0)*340.29)/2;

    return distanceMeters;
}

double HCSR04::speed(int timeout)
{
    // calls the distance() function above
    distance1 = this->distance(timeout)

    // giving a time gap of 1 sec
    delay(1000);

    // calls the distance() function above a second time
    distance2 = this->distance(timeout)

    // formula change in distance divided by change in time
    // as the time gap is 1 sec we divide it by 1.
    speed = (distance2 - distance1)/1.0

    return speed
}

void HCSR04::recordPulseLength()
{
    startTimeUsec = micros();
    while ( digitalRead(echo) == HIGH );
    endTimeUsec = micros();
}

The header file looks now like this:

#ifndef DEF_SONAR
#define DEF_SONAR

class HCSR04
{
  public:
    HCSR04();
    void init(int trigger, int echo);
    double distance(int timeout);
    double speed(int timeout);

  private:
    void recordPulseLength();
    int trigger;
    int echo;
    volatile long startTimeUsec;
    volatile long endTimeUsec;
    double distanceMeters;
    long travelTimeUsec;
    long now;
};

#endif

And at least you can test it with following code:

#include <iostream>
#include <wiringPi.h>
#include "libHCSR04.h"

using namespace std;

int trigger = 1;
int echo = 5;

int main()
{
    if (wiringPiSetup() == -1)
        return -1;

    HCSR04 ultrasonic;
    ultrasonic.init(trigger, echo);

    while(1){
        cout << "Speed is " << ultrasonic.speed(1000000) << " cm/s." << endl;
    }
}

Now that we know what we can measure with the HC-SR04 and how, we can write our driver and wrap it in ROS in the next chapter. Please read for this Writing your HC-SR04 driver (C++).