Ensure Servo detach()es only on idle state

Avoid any short pulses which could cause servo twitches or damage by
adding a shutdown command to the PIO program and checking it's in that
safe part of the loop before detaching the servo.

libraries/Servo/src/Servo.cpp

@@ -24,10 +24,8 @@
 #include <hardware/clocks.h>
 #include <hardware/pio.h>
 
-// The PWM example from theSDK is general purpose enough to use here
-// with no PIO code changes whatsoever.  RP2040 is awesome!
-#include "pwm.pio.h"
-static PIOProgram _servoPgm(&pwm_program);
+#include "servo.pio.h"
+static PIOProgram _servoPgm(&servo_program);
 
 // similiar to map but will have increased accuracy that provides a more
 // symmetrical api (call it and use result to reverse will provide the original value)
@@ -80,21 +78,23 @@ int Servo::attach(pin_size_t pin, int minUs, int maxUs, int value)
     _minUs = max(200, min(_maxUs, minUs));
 
     if (!_attached) {
-        int off;
         digitalWrite(pin, LOW);
         pinMode(pin, OUTPUT);
-        if (!_servoPgm.prepare(&_pio, &_smIdx, &off)) {
+        _pin = pin;
+        if (!_servoPgm.prepare(&_pio, &_smIdx, &_pgmOffset)) {
             // ERROR, no free slots
             return -1;
         }
-        pwm_program_init(_pio, _smIdx, off, pin);
+	_attached = true;
+        servo_program_init(_pio, _smIdx, _pgmOffset, pin);
         pio_sm_set_enabled(_pio, _smIdx, false);
         pio_sm_put_blocking(_pio, _smIdx, RP2040::usToPIOCycles(REFRESH_INTERVAL) / 3);
         pio_sm_exec(_pio, _smIdx, pio_encode_pull(false, false));
         pio_sm_exec(_pio, _smIdx, pio_encode_out(pio_isr, 32));
+        write(value);
+        pio_sm_exec(_pio, _smIdx, pio_encode_pull(false, false));
+        pio_sm_exec(_pio, _smIdx, pio_encode_mov(pio_x, pio_osr));
         pio_sm_set_enabled(_pio, _smIdx, true);
-        _pin = pin;
-	_attached = true;
     }
 
     write(value);
@@ -105,6 +105,12 @@ int Servo::attach(pin_size_t pin, int minUs, int maxUs, int value)
 void Servo::detach()
 {
     if (_attached) {
+        // Set a 0 for the width and then wait for the halt loop
+        pio_sm_put_blocking(_pio, _smIdx, 0);
+        delayMicroseconds(5);  // Avoid race condition
+        do {
+            // Do nothing until we are stuck in the halt loop (avoid short pulses
+        } while (pio_sm_get_pc(_pio, _smIdx) != servo_offset_halt + _pgmOffset);
         pio_sm_set_enabled(_pio, _smIdx, false);
         pio_sm_unclaim(_pio, _smIdx);
         _attached = false;

libraries/Servo/src/pwm.pio → libraries/Servo/src/servo.pio

+; Servo.PIO - Generate Servo pulses
 ;
-; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+; Based on PWM.PIO Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 ;
 ; SPDX-License-Identifier: BSD-3-Clause
 ;
 
 ; Side-set pin 0 is used for PWM output
 
-.program pwm
+; A servo count of 0 will cause an infinite loop allowing for
+; the RP2040 to determine when it is safe to shut down this
+; PIO program without causing any short pulses.
+; RP2040 code can check the PIO PC and determine when to stop it.
+
+.program servo
 .side_set 1 opt
 
     pull noblock    side 0 ; Pull from FIFO to OSR if available, else copy X to OSR.
     mov x, osr             ; Copy most-recently-pulled value back to scratch X
-    mov y, isr             ; ISR contains PWM period. Y used as counter.
+public halt:
+    jmp !x halt            ; If x == 0 then a halt was requested
+    mov y, isr             ; ISR contains servo period. Y used as counter.
 countloop:
     jmp x!=y noset         ; Set pin high if X == Y, keep the two paths length matched
     jmp skip        side 1
 noset:
     nop                    ; Single dummy cycle to keep the two paths the same length
 skip:
-    jmp y-- countloop      ; Loop until Y hits 0, then pull a fresh PWM value from FIFO
+    jmp y-- countloop      ; Loop until Y hits 0, then pull a fresh servo value from FIFO
 
 % c-sdk {
-static inline void pwm_program_init(PIO pio, uint sm, uint offset, uint pin) {
+static inline void servo_program_init(PIO pio, uint sm, uint offset, uint pin) {
    pio_gpio_init(pio, pin);
    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
-   pio_sm_config c = pwm_program_get_default_config(offset);
+   pio_sm_config c = servo_program_get_default_config(offset);
    sm_config_set_sideset_pins(&c, pin);
    pio_sm_init(pio, sm, offset, &c);
 }

libraries/Servo/src/pwm.pio.h → libraries/Servo/src/servo.pio.h

@@ -6,43 +6,46 @@
 #include "hardware/pio.h"
 #endif
 
-// --- //
-// pwm //
-// --- //
+// ----- //
+// servo //
+// ----- //
 
-#define pwm_wrap_target 0
-#define pwm_wrap 6
+#define servo_wrap_target 0
+#define servo_wrap 7
 
-static const uint16_t pwm_program_instructions[] = {
+#define servo_offset_halt 2u
+
+static const uint16_t servo_program_instructions[] = {
             //     .wrap_target
     0x9080, //  0: pull   noblock         side 0     
     0xa027, //  1: mov    x, osr                     
-    0xa046, //  2: mov    y, isr                     
-    0x00a5, //  3: jmp    x != y, 5                  
-    0x1806, //  4: jmp    6               side 1     
-    0xa042, //  5: nop                               
-    0x0083, //  6: jmp    y--, 3                     
+    0x0022, //  2: jmp    !x, 2                      
+    0xa046, //  3: mov    y, isr                     
+    0x00a6, //  4: jmp    x != y, 6                  
+    0x1807, //  5: jmp    7               side 1     
+    0xa042, //  6: nop                               
+    0x0084, //  7: jmp    y--, 4                     
             //     .wrap
 };
 
 #if !PICO_NO_HARDWARE
-static const struct pio_program pwm_program = {
-    .instructions = pwm_program_instructions,
-    .length = 7,
+static const struct pio_program servo_program = {
+    .instructions = servo_program_instructions,
+    .length = 8,
     .origin = -1,
 };
 
-static inline pio_sm_config pwm_program_get_default_config(uint offset) {
+static inline pio_sm_config servo_program_get_default_config(uint offset) {
     pio_sm_config c = pio_get_default_sm_config();
-    sm_config_set_wrap(&c, offset + pwm_wrap_target, offset + pwm_wrap);
+    sm_config_set_wrap(&c, offset + servo_wrap_target, offset + servo_wrap);
     sm_config_set_sideset(&c, 2, true, false);
     return c;
 }
 
-static inline void pwm_program_init(PIO pio, uint sm, uint offset, uint pin) {
+static inline void servo_program_init(PIO pio, uint sm, uint offset, uint pin) {
    pio_gpio_init(pio, pin);
    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
-   pio_sm_config c = pwm_program_get_default_config(offset);
+   pio_sm_config c = servo_program_get_default_config(offset);
    sm_config_set_sideset_pins(&c, pin);
    pio_sm_init(pio, sm, offset, &c);
 }