diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index 9f7a5aa0ee..3fd9941951 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/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 diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h index 85f9ab14ab..c777de06d8 100644 --- a/libraries/AP_HAL_SITL/RCOutput.h +++ b/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