From c8179a088710faed458338e45c09da9248ad224a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 12 Oct 2016 11:05:17 +1100
Subject: [PATCH] HAL_Linux: implement cork/push for RCOutput_AioPRU

---
 libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp | 23 +++++++++++++++++++++-
 libraries/AP_HAL_Linux/RCOutput_AioPRU.h   |  5 +++++
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
index eaa9b9839e..fdd6cff9a2 100644
--- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
@@ -98,7 +98,12 @@ void RCOutput_AioPRU::disable_ch(uint8_t ch)
 void RCOutput_AioPRU::write(uint8_t ch, uint16_t period_us)
 {
    if(ch < PWM_CHAN_COUNT) {
-      pwm->channel[ch].time_high = TICK_PER_US * period_us;
+       if (corked) {
+           pending_mask |= (1U << ch);
+           pending[ch] = period_us;
+       } else {
+           pwm->channel[ch].time_high = TICK_PER_US * period_us;
+       }
    }
 }
 
@@ -125,3 +130,19 @@ void RCOutput_AioPRU::read(uint16_t* period_us, uint8_t len)
       period_us[i] = pwm->channel[i].time_high / TICK_PER_US;
    }
 }
+
+void RCOutput_AioPRU::cork(void)
+{
+    corked = true;
+}
+
+void RCOutput_AioPRU::push(void)
+{
+    corked = false;
+    for (uint8_t i=0; i<PWM_CHAN_COUNT; i++) {
+        if (pending_mask & (1U<<i)) {
+            write(i, pending[i]);
+        }
+    }
+    pending_mask = 0;
+}
diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.h b/libraries/AP_HAL_Linux/RCOutput_AioPRU.h
index b041a76806..d61e178f80 100644
--- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.h
+++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.h
@@ -27,6 +27,8 @@ class RCOutput_AioPRU : public AP_HAL::RCOutput {
     void     write(uint8_t ch, uint16_t period_us);
     uint16_t read(uint8_t ch);
     void     read(uint16_t* period_us, uint8_t len);
+    void     cork(void) override;
+    void     push(void) override;
 
 private:
    static const uint32_t TICK_PER_US = 200;
@@ -41,6 +43,9 @@ private:
     };
 
     volatile struct pwm *pwm;
+    uint16_t pending[PWM_CHAN_COUNT];
+    uint32_t pending_mask;
+    bool corked;
 };
 
 }