STM32F1: Servo "soft" PWM via timer interrupt (#14187)

Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp

@@ -30,6 +30,7 @@
 uint8_t ServoCount = 0;
 
 #include "HAL_Servo_STM32F1.h"
+#include "HAL_timers_STM32F1.h"
 
 //#include "Servo.h"
 
@@ -46,7 +47,7 @@ uint8_t ServoCount = 0;
  *
  * This uses the smallest prescaler that allows an overflow < 2^16.
  */
-#define MAX_OVERFLOW    ((1 << 16) - 1)
+#define MAX_OVERFLOW    UINT16_MAX //((1 << 16) - 1)
 #define CYC_MSEC        (1000 * CYCLES_PER_MICROSECOND)
 #define TAU_MSEC        20
 #define TAU_USEC        (TAU_MSEC * 1000)
@@ -62,22 +63,45 @@ uint8_t ServoCount = 0;
 #define US_TO_ANGLE(us)   ((int16_t)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW,  \
                                        this->minAngle, this->maxAngle)))
 
+void libServo::servoWrite(uint8_t pin, uint16_t duty_cycle) {
+  #ifdef SERVO0_TIMER_NUM
+    if (this->servoIndex == 0) {
+      this->pwmSetDuty(duty_cycle);
+      return;
+    }
+  #endif
+
+  timer_dev *tdev = PIN_MAP[pin].timer_device;
+  uint8_t tchan = PIN_MAP[pin].timer_channel;
+  if (tdev) timer_set_compare(tdev, tchan, duty_cycle);
+}
+
 libServo::libServo() {
   this->servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO;
 }
 
 bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) {
   if (this->servoIndex >= MAX_SERVOS) return false;
-  if (!PWM_PIN(pin)) return false;
+  if (pin >= BOARD_NR_GPIO_PINS) return false;
 
   this->minAngle = minAngle;
   this->maxAngle = maxAngle;
+  this->angle = -1;
+
+  #ifdef SERVO0_TIMER_NUM
+    if (this->servoIndex == 0 && this->setupSoftPWM(pin)) {
+      this->pin = pin; // set attached()
+      return true;
+    }
+  #endif
+
+  if (!PWM_PIN(pin)) return false;
 
   timer_dev *tdev = PIN_MAP[pin].timer_device;
   uint8_t tchan = PIN_MAP[pin].timer_channel;
 
-  pinMode(pin, PWM);
+  SET_PWM(pin);
-  pwmWrite(pin, 0);
+  servoWrite(pin, 0);
 
   timer_pause(tdev);
   timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
@@ -92,12 +116,16 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m
 
 bool libServo::detach() {
   if (!this->attached()) return false;
-  pwmWrite(this->pin, 0);
+  this->angle = -1;
+  servoWrite(this->pin, 0);
   return true;
 }
 
 int32_t libServo::read() const {
   if (this->attached()) {
+    #ifdef SERVO0_TIMER_NUM
+      if (this->servoIndex == 0) return this->angle;
+    #endif
     timer_dev *tdev = PIN_MAP[this->pin].timer_device;
     uint8_t tchan = PIN_MAP[this->pin].timer_channel;
     return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan)));
@@ -110,13 +138,95 @@ void libServo::move(const int32_t value) {
   static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
 
   if (this->attached()) {
-    pwmWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(constrain(value, this->minAngle, this->maxAngle))));
+    this->angle = constrain(value, this->minAngle, this->maxAngle);
+    servoWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(this->angle)));
     safe_delay(servo_delay[this->servoIndex]);
     #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
       this->detach();
     #endif
   }
 }
+
+#ifdef SERVO0_TIMER_NUM
+  extern "C" void Servo_IRQHandler(void) {
+    static timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
+    uint16_t SR = timer_get_status(tdev);
+    if (SR & TIMER_SR_CC1IF) { // channel 1 off
+      #ifdef SERVO0_PWM_OD
+        OUT_WRITE_OD(SERVO0_PIN, 1); // off
+      #else
+        OUT_WRITE(SERVO0_PIN, 0);
+      #endif
+      timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT);
+    }
+    if (SR & TIMER_SR_CC2IF) { // channel 2 resume
+      #ifdef SERVO0_PWM_OD
+        OUT_WRITE_OD(SERVO0_PIN, 0); // on
+      #else
+        OUT_WRITE(SERVO0_PIN, 1);
+      #endif
+      timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT);
+    }
+  }
+
+  bool libServo::setupSoftPWM(const int32_t pin) {
+    timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
+    if (!tdev) return false;
+    #ifdef SERVO0_PWM_OD
+      OUT_WRITE_OD(pin, 1);
+    #else
+      OUT_WRITE(pin, 0);
+    #endif
+
+    timer_pause(tdev);
+    timer_set_mode(tdev, 1, TIMER_OUTPUT_COMPARE); // counter with isr
+    timer_oc_set_mode(tdev, 1, TIMER_OC_MODE_FROZEN, 0); // no pin output change
+    timer_oc_set_mode(tdev, 2, TIMER_OC_MODE_FROZEN, 0); // no pin output change
+    timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
+    timer_set_reload(tdev, SERVO_OVERFLOW);
+    timer_set_compare(tdev, 1, SERVO_OVERFLOW);
+    timer_set_compare(tdev, 2, SERVO_OVERFLOW);
+    timer_attach_interrupt(tdev, 1, Servo_IRQHandler);
+    timer_attach_interrupt(tdev, 2, Servo_IRQHandler);
+    timer_generate_update(tdev);
+    timer_resume(tdev);
+
+    return true;
+  }
+
+  void libServo::pwmSetDuty(const uint16_t duty_cycle) {
+    timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
+    timer_set_compare(tdev, 1, duty_cycle);
+    timer_generate_update(tdev);
+    if (duty_cycle) {
+      timer_enable_irq(tdev, 1);
+      timer_enable_irq(tdev, 2);
+    }
+    else {
+      timer_disable_irq(tdev, 1);
+      timer_disable_irq(tdev, 2);
+      #ifdef SERVO0_PWM_OD
+        OUT_WRITE_OD(this->pin, 1); // off
+      #else
+        OUT_WRITE(this->pin, 0);
+      #endif
+    }
+  }
+
+  void libServo::pauseSoftPWM() { // detach
+    timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
+    timer_pause(tdev);
+    pwmSetDuty(0);
+  }
+
+#else
+
+  bool libServo::setupSoftPWM(const int32_t pin) {}
+  void libServo::pwmSetDuty(const uint16_t duty_cycle) {}
+  void libServo::pauseSoftPWM() {}
+
+#endif
+
 #endif // HAS_SERVOS
 
 #endif // __STM32F1__

Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h

@@ -46,8 +46,15 @@ class libServo {
     void move(const int32_t value);
     int32_t read() const;
   private:
+    void servoWrite(uint8_t pin, const uint16_t duty_cycle);
+
     uint8_t servoIndex;               // index into the channel data for this servo
     int32_t pin = NOT_ATTACHED;
     int32_t minAngle;
     int32_t maxAngle;
+    int32_t angle;
+
+    bool setupSoftPWM(const int32_t pin);
+    void pauseSoftPWM();
+    void pwmSetDuty(const uint16_t duty_cycle);
 };

Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h

@@ -49,7 +49,7 @@
 #define SET_PWM_OD(IO)        pinMode(IO, PWM_OPEN_DRAIN)
 
 #define IS_INPUT(IO)          (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD)
-#define IS_OUTPUT(IO)         (_GET_MODE(IO) == GPIO_OUTPUT_PP)
+#define IS_OUTPUT(IO)         (_GET_MODE(IO) == GPIO_OUTPUT_PP || _GET_MODE(IO) == GPIO_OUTPUT_OD)
 
 #define PWM_PIN(IO)           (PIN_MAP[IO].timer_device != nullptr)