Working with the ultrasonic distance sensor
Ultrasonic distance sensor ("sonar") is a distance sensor based on the principle of measuring the time of a sound wave (about 40 kHz) propagation to the obstacle and back. The sonar can measure the distance up to 1.5 – 3 m with the accuracy of several centimeters.
HC-SR04 distance sensor
The distance sensor is attached to the body using double-sided tape. For obtaining acceptable results, the use of vibro-insulation is required. A piece of PU foam may be used for vibro-insulation.
Connect HC-SR04 to Raspberry Pi according to the connection diagram. Use 1.0 and 2.2 kΩ resistors and any free GPIO pins, e.g., 23 and 24:
Instead of a 2.2 kΩ resistor, you can use two 1 kΩ resistors connected in series.
There are several interchangeable pins GND and VCC 5V on Raspberry Pi. Use the [pinout] (https://pinout.xyz) to find them.
Reading the data
To read the data from distance sensor HC-SR04 library for working with GPIO is used –
pigpio. This library is pre-installed in the Clover image, starting with version v0.14. For older versions of the image, use an installation guide.
To work with
pigpio, start appropriate daemon:
sudo systemctl start pigpiod.service
You can also enable
pigpiod auto launch on system startup:
sudo systemctl enable pigpiod.service
Thus, it becomes possible to interact with the
pigpiod daemon from Python:
import pigpio pi = pigpio.pi()
See detailed description of Python API in
An example of the code for reading data from HC-SR04:
import time import threading import pigpio TRIG = 23 # pin connected to the Trig pin of the sonar ECHO = 24 # pin connected to the Echo pin of the sonar pi = pigpio.pi() done = threading.Event() def rise(gpio, level, tick): global high high = tick def fall(gpio, level, tick): global low low = tick - high done.set() def read_distance(): global low done.clear() pi.gpio_trigger(TRIG, 50, 1) if done.wait(timeout=5): return low / 58.0 / 100.0 pi.set_mode(TRIG, pigpio.OUTPUT) pi.set_mode(ECHO, pigpio.INPUT) pi.callback(ECHO, pigpio.RISING_EDGE, rise) pi.callback(ECHO, pigpio.FALLING_EDGE, fall) while True: # Reading the distance: print(read_distance())
Filtering the data
To filter (smooth out) the data and delete outliers, Kalman filter or a simple median filter can be used. An example of median filtering implementation:
import collections import numpy # ... history = collections.deque(maxlen=10) # 10 - number of samples for averaging def read_distance_filtered(): history.append(read_distance()) return numpy.median(history) while True: print(read_distance_filtered())
An example of charts of initial and filtered data:
The source code of the ROS-node used for building the chart can be found on Gist.
RCW-0001 distance sensor
The RCW-0001 distance sensor is compatible with distance sensor HC-SR04. Use the instruction above to connect and work with it.
An example of a flight program with the use of simple_offboard, which makes the copter fly forward until the connected ultrasonic distance sensor detects an obstacle:
set_velocity(vx=0.5, frame_id='body', auto_arm=True) # flying forward at the velocity of 0.5 mps while True: if read_distance_filtered() < 1: # if the obstacle is closer than 1 m, hanging on the spot set_position(x=0, y=0, z=0, frame_id='body') rospy.sleep(0.1)