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