diff --git a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp index 51ee34608c..30f476a302 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp @@ -68,7 +68,12 @@ void RCOutput_PRU::disable_ch(uint8_t ch) void RCOutput_PRU::write(uint8_t ch, uint16_t period_us) { - sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us; + if (corked) { + pending[ch] = period_us; + pending_mask |= (1U << ch); + } else { + sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us; + } } uint16_t RCOutput_PRU::read(uint8_t ch) @@ -86,3 +91,19 @@ void RCOutput_PRU::read(uint16_t* period_us, uint8_t len) period_us[i] = sharedMem_cmd->hilo_read[chan_pru_map[i]][1]/TICK_PER_US; } } + +void RCOutput_PRU::cork(void) +{ + corked = true; +} + +void RCOutput_PRU::push(void) +{ + corked = false; + for (uint8_t i=0; iperiodhi[ch].hi = TICK_PER_US*period_us; + if (corked) { + pending[ch] = period_us; + pending_mask |= (1U << ch); + } else { + sharedMem_cmd->periodhi[ch].hi = TICK_PER_US*period_us; + } } uint16_t RCOutput_ZYNQ::read(uint8_t ch) @@ -94,3 +99,19 @@ void RCOutput_ZYNQ::read(uint16_t* period_us, uint8_t len) period_us[i] = sharedMem_cmd->periodhi[i].hi/TICK_PER_US; } } + +void RCOutput_ZYNQ::cork(void) +{ + corked = true; +} + +void RCOutput_ZYNQ::push(void) +{ + corked = false; + for (uint8_t i=0; i