From 710cbacef7cd7010ac0c1913016d25baa5cc73f4 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Mon, 29 Aug 2022 11:30:19 -0300
Subject: [PATCH] RCOutput_PC9685: refactor write/write_gpio

---
 libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 17 ++++++++---------
 libraries/AP_HAL_Linux/RCOutput_PCA9685.h   |  1 +
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
index 647c2b7478..d6c1fed519 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
@@ -199,13 +199,7 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
     if (_is_gpio_mask & (1U << ch)) {
         return;
     }
-    _pulses_buffer[ch] = period_us;
-    _pending_write_mask |= (1U << ch);
-
-    if (!_corking) {
-        _corking = true;
-        push();
-    }
+    write_raw(ch, period_us);
 }
 
 void RCOutput_PCA9685::write_gpio(uint8_t chan, bool active)
@@ -213,8 +207,13 @@ void RCOutput_PCA9685::write_gpio(uint8_t chan, bool active)
     if (chan >= (PWM_CHAN_COUNT - _channel_offset)) {
         return;
     }
-    _is_relay_mask |= (1U << ch);
-    _pulses_buffer[ch] = active;
+    _is_gpio_mask |= (1U << chan);
+    write_raw(chan, active);
+}
+
+void RCOutput_PCA9685::write_raw(uint8_t ch, uint16_t period_us) {
+    /* Common code used by both write() and write_gpio() */
+    _pulses_buffer[ch] = period_us;
     _pending_write_mask |= (1U << ch);
 
     if (!_corking) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
index 1e06ac96ea..5d057203f2 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
+++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
@@ -38,6 +38,7 @@ public:
 
 private:
     void reset();
+    void write_raw(uint8_t ch, uint16_t period_us);
 
     AP_HAL::DigitalSource *_enable_pin;
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;