From 603bc5ecce57c724b3ef86b039ea244c33250c09 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Fri, 27 May 2022 16:52:08 -0300
Subject: [PATCH] RCOutput_PCA9685: implement gpio methods

---
 libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 38 ++++++++++++++++++---
 libraries/AP_HAL_Linux/RCOutput_PCA9685.h   |  3 ++
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
index 3a6b7dab2f..647c2b7478 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
@@ -42,6 +42,9 @@
 #define PCA9685_MODE2_OUTNE1_BIT   (1 << 1)
 #define PCA9685_MODE2_OUTNE0_BIT   (1 << 0)
 
+#define PCA9685_LED_ON_H_ALWAYS_ON_BIT  (1 << 4)
+#define PCA9685_LED_OFF_H_ALWAYS_OFF_BIT (1 << 4)
+
 /*
  * Drift for internal oscillator
  * see: https://github.com/ArduPilot/ardupilot/commit/50459bdca0b5a1adf95
@@ -193,7 +196,9 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
     if (ch >= (PWM_CHAN_COUNT - _channel_offset)) {
         return;
     }
-
+    if (_is_gpio_mask & (1U << ch)) {
+        return;
+    }
     _pulses_buffer[ch] = period_us;
     _pending_write_mask |= (1U << ch);
 
@@ -203,6 +208,21 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
     }
 }
 
+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;
+    _pending_write_mask |= (1U << ch);
+
+    if (!_corking) {
+        _corking = true;
+        push();
+    }
+}
+
 void RCOutput_PCA9685::cork()
 {
     _corking = true;
@@ -240,10 +260,18 @@ void RCOutput_PCA9685::push()
         }
 
         uint8_t *d = &pwm_values.data[(ch - min_ch) * 4];
-        *d++ = 0;
-        *d++ = 0;
-        *d++ = length & 0xFF;
-        *d++ = length >> 8;
+
+        if (_is_gpio_mask & (1 << ch)) {
+            *d++ = 0;                                                // LEDn_ON_L
+            *d++ = period_us ? PCA9685_LED_ON_H_ALWAYS_ON_BIT : 0;   // LEDn_ON_H
+            *d++ = 0;                                                // LEDn_OFF_L
+            *d++ = period_us ? 0 : PCA9685_LED_OFF_H_ALWAYS_OFF_BIT; // LEDn_OFF_H
+        } else {
+            *d++ = 0;               // LEDn_ON_L
+            *d++ = 0;               // LEDn_ON_H
+            *d++ = length & 0xFF;   // LEDn_OFF_L
+            *d++ = length >> 8;     // LEDn_OFF_H
+        }
     }
 
     if (!_dev || !_dev->get_semaphore()->take_nonblocking()) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
index 367d346309..1e06ac96ea 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
+++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h
@@ -33,6 +33,8 @@ public:
     void     push() override;
     uint16_t read(uint8_t ch) override;
     void     read(uint16_t* period_us, uint8_t len) override;
+    bool     supports_gpio() override { return true; };
+    void     write_gpio(uint8_t chan, bool active) override;
 
 private:
     void reset();
@@ -49,6 +51,7 @@ private:
     uint8_t _channel_offset;
     int16_t _oe_pin_number;
     uint32_t _pending_write_mask;
+    uint32_t _is_gpio_mask;
 };
 
 }