HAL_SITL: implement cork() on RCOutput

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -46,7 +46,11 @@ void RCOutput::disable_ch(uint8_t ch)
 void RCOutput::write(uint8_t ch, uint16_t period_us)
 {
     if (ch < SITL_NUM_CHANNELS) {
-        _sitlState->pwm_output[ch] = period_us;
+        if (_corked) {
+            _pending[ch] = period_us;
+        } else {
+            _sitlState->pwm_output[ch] = period_us;
+        }
     }
 }
 
@@ -63,4 +67,16 @@ void RCOutput::read(uint16_t* period_us, uint8_t len)
     memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
 }
 
+void RCOutput::cork(void)
+{
+    memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS*sizeof(uint16_t));
+    _corked = true;
+}
+
+void RCOutput::push(void)
+{
+    memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS*sizeof(uint16_t));
+    _corked = false;
+}
+
 #endif

libraries/AP_HAL_SITL/RCOutput.h

@@ -18,11 +18,15 @@ public:
     void     write(uint8_t ch, uint16_t period_us) override;
     uint16_t read(uint8_t ch) override;
     void     read(uint16_t* period_us, uint8_t len) override;
+    void     cork(void);
+    void     push(void);
 
 private:
     SITL_State *_sitlState;
     uint16_t _freq_hz;
     uint16_t _enable_mask;
+    bool _corked;
+    uint16_t _pending[SITL_NUM_CHANNELS];
 };
 
 #endif