-
-
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
ee4368b
commit 004f9aa
Showing
5 changed files
with
397 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,255 @@ | ||
| package gpio | ||
|
|
||
| import ( | ||
| "fmt" | ||
| "log" | ||
| "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 | ||
| hcsr04PollInputIntervall time.Duration = 10 * time.Microsecond // this adds around 2 mm inaccuracy (resolution 3 mm) | ||
| hcsr04EmitTriggerDuration time.Duration = 10 * time.Microsecond // according to specification | ||
| hcsr04MonitorUpdate time.Duration = 200 * time.Millisecond | ||
| ) | ||
|
|
||
| // HCSR04 is a driver for ultrasonic range measurement. | ||
| type HCSR04 struct { | ||
| *Driver | ||
| triggerPinID string | ||
| echoPinID string | ||
| useEventHandler bool // use gpiod event handler instead own implementation | ||
| 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 | ||
| } | ||
|
|
||
| // 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, useEventHandler bool) *HCSR04 { | ||
| h := HCSR04{ | ||
| Driver: NewDriver(a, "HCSR04"), | ||
| triggerPinID: triggerPinID, | ||
| echoPinID: echoPinID, | ||
| useEventHandler: useEventHandler, | ||
| 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) | ||
| } | ||
|
|
||
| if useEventHandler { | ||
| if err := epin.ApplyOptions(system.WithPinEventOnBothEdges(h.createEventHandler())); err != nil { | ||
| return fmt.Errorf("error on apply event handler for echo pin: %v", err) | ||
| } | ||
| } | ||
| h.echoPin = epin | ||
|
|
||
| h.delayMicroSecChan = make(chan int64) | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| h.beforeHalt = func() error { | ||
| // 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) measureDistance() error { | ||
| h.measureMutex.Lock() | ||
| defer h.measureMutex.Unlock() | ||
|
|
||
| errChan := make(chan error) | ||
| quitChan := make(chan struct{}) | ||
| defer close(quitChan) | ||
| if !h.useEventHandler { | ||
| go h.edgePolling(hcsr04PollInputIntervall, errChan, quitChan, h.createEventHandler()) | ||
| } | ||
|
|
||
| if err := h.emitTrigger(); err != nil { | ||
| return err | ||
| } | ||
|
|
||
| // stop the loop if the state is on target state, the timeout is elapsed or an error occurred | ||
| 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 err := <-errChan: | ||
| return fmt.Errorf("error while detect echo: %v", err) | ||
| case durMicro := <-h.delayMicroSecChan: | ||
| log.Println(durMicro, "us") | ||
| h.lastDistanceMm = int(durMicro * hcsr04SoundSpeed / 1000 / 2) | ||
| } | ||
|
|
||
| return nil | ||
| } | ||
|
|
||
| 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) emitTrigger() error { | ||
| if err := h.triggerPin.Write(1); err != nil { | ||
| return err | ||
| } | ||
| time.Sleep(hcsr04EmitTriggerDuration) | ||
| return h.triggerPin.Write(0) | ||
| } | ||
|
|
||
| // TODO: move this function to system level and add a "WithEdgePolling()" option | ||
| func (h *HCSR04) edgePolling( | ||
| pollIntervall time.Duration, | ||
| errChan chan error, | ||
| quitChan chan struct{}, | ||
| eventHandler func(offset int, t time.Duration, et string, sn uint32, lsn uint32), | ||
| ) { | ||
| var oldState int | ||
| for { | ||
| select { | ||
| case <-quitChan: | ||
| return | ||
| default: | ||
| readedValue, err := h.echoPin.Read() | ||
| if err != nil { | ||
| errChan <- fmt.Errorf("an error occurred while reading the pin %s: %v", h.echoPinID, err) | ||
| return | ||
| } | ||
| if readedValue > oldState { | ||
| eventHandler(0, time.Duration(time.Now().UnixNano()), system.DigitalPinEventRisingEdge, 0, 0) | ||
| } else if readedValue < oldState { | ||
| eventHandler(0, time.Duration(time.Now().UnixNano()), system.DigitalPinEventFallingEdge, 0, 0) | ||
| } | ||
| oldState = readedValue | ||
| time.Sleep(pollIntervall) | ||
| } | ||
| } | ||
| } |
Oops, something went wrong.