From bedfbda372306fd4e723c0ae473f63c61ba01e9f Mon Sep 17 00:00:00 2001
From: "Earle F. Philhower, III" <earlephilhower@yahoo.com>
Date: Fri, 11 Jun 2021 12:27:48 -0700
Subject: [PATCH] Clear PIO FIFOs when updating Tone and Servos (#206)

The PIO programs that generate tone() and Servo() use the TX FIFO to
receive updates to the period/duty cycle.

The original code would push into the FIFO (potentially blocking the
app if the FIFO was full) and generate at least one cycle of every
value written into the control.  Basically, the output would
lag the changes by 1 cycle or more (which could be 20ms+ on Servo).

Fix this by clearing any old, ungrabbed values from the FIFO before
sending a new one to the program.  Instead of a FIFO, there is
effectively now just a control register and updates will be immediate.

Update the Siren.ino example with delays because now the tone() calls
will not block and run 10x+ faster.
---
 cores/rp2040/Tone.cpp                     | 1 +
 libraries/Servo/src/Servo.cpp             | 1 +
 libraries/rp2040/examples/Siren/Siren.ino | 3 ++-
 3 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/cores/rp2040/Tone.cpp b/cores/rp2040/Tone.cpp
index c5b59018..2fde8254 100644
--- a/cores/rp2040/Tone.cpp
+++ b/cores/rp2040/Tone.cpp
@@ -88,6 +88,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
             newTone->alarm = 0;
         }
     }
+    pio_sm_clear_fifos(newTone->pio, newTone->sm); // Remove any old updates that haven't yet taken effect
     pio_sm_put_blocking(newTone->pio, newTone->sm, RP2040::usToPIOCycles(us));
     pio_sm_set_enabled(newTone->pio, newTone->sm, true);
 
diff --git a/libraries/Servo/src/Servo.cpp b/libraries/Servo/src/Servo.cpp
index fd38625b..7c9844e9 100644
--- a/libraries/Servo/src/Servo.cpp
+++ b/libraries/Servo/src/Servo.cpp
@@ -135,6 +135,7 @@ void Servo::writeMicroseconds(int value) {
     value = constrain(value, _minUs, _maxUs);
     _valueUs = value;
     if (_attached) {
+        pio_sm_clear_fifos(_pio, _smIdx); // Remove any old updates that haven't yet taken effect
         pio_sm_put_blocking(_pio, _smIdx, RP2040::usToPIOCycles(value) / 3);
     }
 }
diff --git a/libraries/rp2040/examples/Siren/Siren.ino b/libraries/rp2040/examples/Siren/Siren.ino
index d395a73d..014a7629 100644
--- a/libraries/rp2040/examples/Siren/Siren.ino
+++ b/libraries/rp2040/examples/Siren/Siren.ino
@@ -7,7 +7,8 @@ void setup() {
 }
 
 void loop() {
-  for (int i = 100; i < 10000; i += 5) {
+  for (int i = 100; i < 10000; i += 1) {
     tone(TONEPIN, i);
+    delayMicroseconds(20);
   }
 }