diff --git a/cores/arduino/zephyrCommon.cpp b/cores/arduino/zephyrCommon.cpp index 1d4cbdbe..a2946692 100644 --- a/cores/arduino/zephyrCommon.cpp +++ b/cores/arduino/zephyrCommon.cpp @@ -210,6 +210,51 @@ PinStatus digitalRead(pin_size_t pinNumber) { return (gpio_pin_get_dt(&arduino_pins[pinNumber]) == 1) ? HIGH : LOW; } +struct k_timer arduino_pin_timers[ARRAY_SIZE(arduino_pins)]; +struct k_timer arduino_pin_timers_timeout[ARRAY_SIZE(arduino_pins)]; + +void tone_expiry_cb(struct k_timer *timer) { + const struct gpio_dt_spec *spec = (gpio_dt_spec*)k_timer_user_data_get(timer); + gpio_pin_toggle_dt(spec); +} + +void tone_timeout_cb(struct k_timer *timer) { + pin_size_t pinNumber = (pin_size_t)(uintptr_t)k_timer_user_data_get(timer); + noTone(pinNumber); +} + +void tone(pin_size_t pinNumber, unsigned int frequency, unsigned long duration) { + struct k_timer *timer = &arduino_pin_timers[pinNumber]; + const struct gpio_dt_spec *spec = &arduino_pins[pinNumber]; + k_timeout_t timeout; + + pinMode(pinNumber, OUTPUT); + + if (frequency == 0) { + gpio_pin_set_dt(spec, 0); + return; + } + + timeout = K_NSEC(NSEC_PER_SEC / (2 * frequency)); + + k_timer_init(timer, tone_expiry_cb, NULL); + k_timer_user_data_set(timer, (void*)spec); + gpio_pin_set_dt(spec, 1); + k_timer_start(timer, timeout, timeout); + + if(duration > 0) { + timer = &arduino_pin_timers_timeout[pinNumber]; + k_timer_init(timer, tone_timeout_cb, NULL); + k_timer_user_data_set(timer, (void*)(uintptr_t)pinNumber); + k_timer_start(timer, K_MSEC(duration), K_NO_WAIT); + } +} + +void noTone(pin_size_t pinNumber) { + k_timer_stop(&arduino_pin_timers[pinNumber]); + gpio_pin_set_dt(&arduino_pins[pinNumber], 0); +} + void delay(unsigned long ms) { k_sleep(K_MSEC(ms)); } void delayMicroseconds(unsigned int us) { k_sleep(K_USEC(us)); }