HAL_Linux: implement cork/push for RCOutput_AioPRU

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;
+}

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;
 };
 
 }