-
-
Notifications
You must be signed in to change notification settings - Fork 1k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
gpio(hcsr04): add driver for ultrasonic ranging module
- Loading branch information
1 parent
1f09353
commit 745d602
Showing
6 changed files
with
466 additions
and
4 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,226 @@ | ||
| package gpio | ||
|
|
||
| import ( | ||
| "fmt" | ||
| "sync" | ||
| "time" | ||
|
|
||
| "gobot.io/x/gobot/v2" | ||
| "gobot.io/x/gobot/v2/system" | ||
| ) | ||
|
|
||
| const ( | ||
| hcsr04SoundSpeed = 343 // in [m/s] | ||
| // the device can measure 2 cm .. 4 m, this means sweep distances between 4 cm and 8 m | ||
| // this cause pulse duration between 0.12 ms and 24 ms (at 34.3 cm/ms, ~0.03 ms/cm, ~3 ms/m) | ||
| // so we use 60 ms as a limit for timeout and 100 ms for duration between 2 consecutive measurements | ||
| hcsr04StartTransmitTimeout time.Duration = 100 * time.Millisecond // unfortunately takes sometimes longer than 60 ms | ||
| hcsr04ReceiveTimeout time.Duration = 60 * time.Millisecond | ||
| hcsr04EmitTriggerDuration time.Duration = 10 * time.Microsecond // according to specification | ||
| hcsr04MonitorUpdate time.Duration = 200 * time.Millisecond | ||
| // the resolution of the device is ~3 mm, which relates to 10 us (343 mm/ms = 0.343 mm/us) | ||
| // the poll interval increases the reading interval to this value and adds around 3 mm inaccuracy | ||
| // it takes only an effect for fast systems, because reading inputs is typically much slower, e.g. 30-50 us on raspi | ||
| // so, using the internal edge detection with "cdev" is more precise | ||
| hcsr04PollInputIntervall time.Duration = 10 * time.Microsecond | ||
| ) | ||
|
|
||
| // HCSR04 is a driver for ultrasonic range measurement. | ||
| type HCSR04 struct { | ||
| *Driver | ||
| triggerPinID string | ||
| echoPinID string | ||
| useEdgePolling bool // use discrete edge polling instead "cdev" from gpiod | ||
| triggerPin gobot.DigitalPinner | ||
| echoPin gobot.DigitalPinner | ||
| lastDistanceMm int // distance in mm, ~20..4000 | ||
| continuousStop chan struct{} | ||
| continuousStopWaitGroup *sync.WaitGroup | ||
| measureMutex *sync.Mutex // to ensure that only one measurement is done at a time | ||
| delayMicroSecChan chan int64 // channel for event handler return value | ||
| distanceMonitorStarted bool | ||
| pollQuitChan chan struct{} // channel for quit the continuous polling | ||
| } | ||
|
|
||
| // NewHCSR04 creates a new instance of the driver for HC-SR04 (same as SEN-US01). | ||
| // | ||
| // Datasheet: https://www.makershop.de/download/HCSR04-datasheet-version-1.pdf | ||
| func NewHCSR04(a gobot.Adaptor, triggerPinID string, echoPinID string, useEdgePolling bool) *HCSR04 { | ||
| h := HCSR04{ | ||
| Driver: NewDriver(a, "HCSR04"), | ||
| triggerPinID: triggerPinID, | ||
| echoPinID: echoPinID, | ||
| useEdgePolling: useEdgePolling, | ||
| measureMutex: &sync.Mutex{}, | ||
| } | ||
|
|
||
| h.afterStart = func() error { | ||
| tpin, err := a.(gobot.DigitalPinnerProvider).DigitalPin(triggerPinID) | ||
| if err != nil { | ||
| return fmt.Errorf("error on get trigger pin: %v", err) | ||
| } | ||
| if err := tpin.ApplyOptions(system.WithPinDirectionOutput(0)); err != nil { | ||
| return fmt.Errorf("error on apply output for trigger pin: %v", err) | ||
| } | ||
| h.triggerPin = tpin | ||
|
|
||
| // pins are inputs by default | ||
| epin, err := a.(gobot.DigitalPinnerProvider).DigitalPin(echoPinID) | ||
| if err != nil { | ||
| return fmt.Errorf("error on get echo pin: %v", err) | ||
| } | ||
|
|
||
| epinOptions := []func(gobot.DigitalPinOptioner) bool{system.WithPinEventOnBothEdges(h.createEventHandler())} | ||
| if h.useEdgePolling { | ||
| h.pollQuitChan = make(chan struct{}) | ||
| epinOptions = append(epinOptions, system.WithPinPollForEdgeDetection(hcsr04PollInputIntervall, h.pollQuitChan)) | ||
| } | ||
| if err := epin.ApplyOptions(epinOptions...); err != nil { | ||
| return fmt.Errorf("error on apply options for echo pin: %v", err) | ||
| } | ||
| h.echoPin = epin | ||
|
|
||
| h.delayMicroSecChan = make(chan int64) | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| h.beforeHalt = func() error { | ||
| if useEdgePolling { | ||
| close(h.pollQuitChan) | ||
| } | ||
| // TODO: create summarized error | ||
| if err := h.stopDistanceMonitor(); err != nil { | ||
| fmt.Printf("no need to stop distance monitoring: %v\n", err) | ||
| } | ||
|
|
||
| if err := h.triggerPin.Unexport(); err != nil { | ||
| fmt.Printf("error on unexport trigger pin: %v\n", err) | ||
| } | ||
|
|
||
| if err := h.echoPin.Unexport(); err != nil { | ||
| fmt.Printf("error on unexport echo pin: %v\n", err) | ||
| } | ||
|
|
||
| close(h.delayMicroSecChan) | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| return &h | ||
| } | ||
|
|
||
| // MeasureDistance retrieves the distance in front of sensor in meters and returns the measure. It is not designed | ||
| // to work in a fast loop! For this specific usage, use StartDistanceMonitor() associated with GetDistance() instead. | ||
| func (h *HCSR04) MeasureDistance() (float64, error) { | ||
| err := h.measureDistance() | ||
| if err != nil { | ||
| return 0, err | ||
| } | ||
| return h.GetDistance(), nil | ||
| } | ||
|
|
||
| // StartDistanceMonitor starts continuous measurement. The current value can be read by GetDistance() | ||
| func (h *HCSR04) StartDistanceMonitor() error { | ||
| // ensure that start and stop can not interfere | ||
| h.mutex.Lock() | ||
| defer h.mutex.Unlock() | ||
|
|
||
| if h.distanceMonitorStarted { | ||
| return fmt.Errorf("distance monitor already started for '%s'", h.name) | ||
| } | ||
|
|
||
| h.distanceMonitorStarted = true | ||
| h.continuousStop = make(chan struct{}) | ||
| h.continuousStopWaitGroup = &sync.WaitGroup{} | ||
| h.continuousStopWaitGroup.Add(1) | ||
|
|
||
| go func(name string) { | ||
| defer h.continuousStopWaitGroup.Done() | ||
| for { | ||
| select { | ||
| case <-h.continuousStop: | ||
| return | ||
| default: | ||
| if err := h.measureDistance(); err != nil { | ||
| fmt.Printf("continuous measure distance skipped for '%s': %v\n", name, err) | ||
| } | ||
| time.Sleep(hcsr04MonitorUpdate) | ||
| } | ||
| } | ||
| }(h.name) | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| // StopDistanceMonitor stop the monitor process | ||
| func (h *HCSR04) StopDistanceMonitor() error { | ||
| // ensure that start and stop can not interfere | ||
| h.mutex.Lock() | ||
| defer h.mutex.Unlock() | ||
|
|
||
| return h.stopDistanceMonitor() | ||
| } | ||
|
|
||
| func (h *HCSR04) stopDistanceMonitor() error { | ||
| if !h.distanceMonitorStarted { | ||
| return fmt.Errorf("distance monitor is not yet started for '%s'", h.name) | ||
| } | ||
|
|
||
| h.continuousStop <- struct{}{} | ||
| h.continuousStopWaitGroup.Wait() | ||
| h.distanceMonitorStarted = false | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| // GetDistance returns the last distance measured in meter, it does not trigger a distance measurement | ||
| func (h *HCSR04) GetDistance() float64 { | ||
| return float64(h.lastDistanceMm) / 1000.0 | ||
| } | ||
|
|
||
| func (h *HCSR04) createEventHandler() func(int, time.Duration, string, uint32, uint32) { | ||
| var startTimestamp time.Duration | ||
| return func(offset int, t time.Duration, et string, sn uint32, lsn uint32) { | ||
| switch et { | ||
| case system.DigitalPinEventRisingEdge: | ||
| startTimestamp = t | ||
| case system.DigitalPinEventFallingEdge: | ||
| // unfortunately there is an additional falling edge at each start trigger, so we need to filter this | ||
| // we use the start duration value for filtering | ||
| if startTimestamp == 0 { | ||
| return | ||
| } | ||
| h.delayMicroSecChan <- (t - startTimestamp).Microseconds() | ||
| startTimestamp = 0 | ||
| } | ||
| } | ||
| } | ||
|
|
||
| func (h *HCSR04) measureDistance() error { | ||
| h.measureMutex.Lock() | ||
| defer h.measureMutex.Unlock() | ||
|
|
||
| if err := h.emitTrigger(); err != nil { | ||
| return err | ||
| } | ||
|
|
||
| // stop the loop if the measure is done or the timeout is elapsed | ||
| timeout := hcsr04StartTransmitTimeout + hcsr04ReceiveTimeout | ||
| select { | ||
| case <-time.After(timeout): | ||
| return fmt.Errorf("timeout %s reached while waiting for value with echo pin %s", timeout, h.echoPinID) | ||
| case durMicro := <-h.delayMicroSecChan: | ||
| h.lastDistanceMm = int(durMicro * hcsr04SoundSpeed / 1000 / 2) | ||
| } | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| func (h *HCSR04) emitTrigger() error { | ||
| if err := h.triggerPin.Write(1); err != nil { | ||
| return err | ||
| } | ||
| time.Sleep(hcsr04EmitTriggerDuration) | ||
| return h.triggerPin.Write(0) | ||
| } |
Oops, something went wrong.