107 lines
3 KiB
Python
107 lines
3 KiB
Python
import time
|
|
import RPi.GPIO as GPIO
|
|
|
|
trigger_pin = 27
|
|
echo_pin = 22
|
|
speed_of_sound_mps = 340 # in m/s
|
|
|
|
rising_found = False
|
|
rising_time = 0
|
|
falling_found = False
|
|
falling_time = 0
|
|
|
|
class Ultrasonic:
|
|
|
|
@staticmethod
|
|
def __send_trigger():
|
|
GPIO.setup(trigger_pin, GPIO.OUT)
|
|
GPIO.output(trigger_pin, False) # Ensure a clean transition
|
|
time.sleep(0.000005)
|
|
GPIO.output(trigger_pin, True) # Trigger now
|
|
time.sleep(0.000010)
|
|
GPIO.output(trigger_pin, False)
|
|
|
|
@staticmethod
|
|
def __signal_start_callback(channel):
|
|
global rising_found
|
|
global rising_time
|
|
rising_found = True
|
|
rising_time = time.process_time_ns()
|
|
|
|
@staticmethod
|
|
def __signal_end_callback(channel):
|
|
global falling_found
|
|
global falling_time
|
|
falling_found = True
|
|
falling_time = time.process_time_ns()
|
|
|
|
@staticmethod
|
|
def __measure_pulse(timeout = 1000000) -> int:
|
|
""" Measure the length of a pulse in us. When either the pulse doesn't start in [timeout] us
|
|
or the pulse is longer than [timeout] us, 0 is returned.
|
|
|
|
:param timeout: Timeout in us
|
|
:return: The length of the pulse or 0 on timeout
|
|
:rtype: int
|
|
"""
|
|
|
|
global rising_found
|
|
global rising_time
|
|
global falling_found
|
|
global falling_time
|
|
|
|
# Setup pin
|
|
GPIO.setup(echo_pin, GPIO.IN)
|
|
|
|
# Wait for the rising edge
|
|
rising_found = False
|
|
GPIO.add_event_detect(echo_pin, GPIO.RISING, callback = Ultrasonic.__signal_start_callback)
|
|
start = time.process_time_ns()
|
|
while (time.process_time_ns() < start + timeout * 1000 and not rising_found):
|
|
pass
|
|
|
|
GPIO.remove_event_detect(echo_pin)
|
|
if (not rising_found):
|
|
return 0
|
|
|
|
# Wait for the falling edge
|
|
falling_found = False
|
|
GPIO.add_event_detect(echo_pin, GPIO.FALLING, callback = Ultrasonic.__signal_end_callback)
|
|
start = time.process_time_ns()
|
|
while (time.process_time_ns() < start + timeout * 1000 and not falling_found):
|
|
pass
|
|
|
|
GPIO.remove_event_detect(echo_pin)
|
|
if (not falling_found):
|
|
return 0
|
|
|
|
# Finally, return the time difference
|
|
return int((falling_time - rising_time) / 1000)
|
|
|
|
@staticmethod
|
|
def get_distance() -> float:
|
|
""" Get the distance from the Ultrasonic sensor in cm.
|
|
Returns 'NaN' when the result is invalid.
|
|
|
|
:return: The distance in cm or 'NaN'
|
|
:rtype: float
|
|
"""
|
|
GPIO.setmode(GPIO.BCM)
|
|
|
|
# Trigger the measurement
|
|
Ultrasonic.__send_trigger()
|
|
|
|
# The pulse length in us
|
|
pulse = Ultrasonic.__measure_pulse()
|
|
|
|
if (pulse == 0):
|
|
print("invalid")
|
|
return float('NaN')
|
|
|
|
# Convert the speed of sound from m/s to cm/us for convenience
|
|
speed_of_sound_cmpus = speed_of_sound_mps / 10000
|
|
|
|
# Convert us time to cm distance
|
|
distance = (pulse / 2) * speed_of_sound_cmpus
|
|
|
|
return distance
|