x

Author: soburi

cores/arduino/zephyrCommon.cpp

@@ -216,6 +216,8 @@ PinStatus digitalRead(pin_size_t pinNumber) {
 #define MAX_TONE_PINS DT_PROP_LEN(DT_PATH(zephyr_user), digital_pin_gpios)
 #endif
 
+#define TOGGLES_PER_CYCLE 2ULL
+
 struct pin_timer {
   struct k_timer timer;
   uint32_t count;
@@ -241,9 +243,15 @@ void tone_expiry_cb(struct k_timer *timer) {
 void tone(pin_size_t pinNumber, unsigned int frequency,
           unsigned long duration) {
   const struct gpio_dt_spec *spec = &arduino_pins[pinNumber];
-  struct k_timer *timer = &arduino_pin_timers[pinNumber].timer;
+  struct k_timer *timer;
   k_timeout_t timeout;
 
+  if (pinNumber >= MAX_TONE_PINS) {
+    return;
+  }
+
+  timer = &arduino_pin_timers[pinNumber].timer;
+
   pinMode(pinNumber, OUTPUT);
   k_timer_stop(&arduino_pin_timers[pinNumber].timer);
 
@@ -252,13 +260,13 @@ void tone(pin_size_t pinNumber, unsigned int frequency,
     return;
   }
 
-  timeout = K_NSEC(NSEC_PER_SEC / (2 * frequency));
+  timeout = K_NSEC(NSEC_PER_SEC / (TOGGLES_PER_CYCLE * frequency));
   if (timeout.ticks == 0) {
     timeout.ticks = 1;
   }
 
   arduino_pin_timers[pinNumber].infinity = (duration == 0);
-  arduino_pin_timers[pinNumber].count = ((uint64_t)duration * frequency) / 500ULL;
+  arduino_pin_timers[pinNumber].count = (uint64_t)duration * frequency * (MSEC_PER_SEC / TOGGLES_PER_CYCLE);
   arduino_pin_timers[pinNumber].pin = pinNumber;
   k_timer_init(timer, tone_expiry_cb, NULL);