AP_HAL_ChibiOS: convert back to full range on IOMCU ONESHOT125 read

This commit is contained in:
Iampete1 2021-09-06 18:14:48 +01:00 committed by Andrew Tridgell
parent ed83edc2a6
commit 529a3a9122
1 changed files with 10 additions and 1 deletions

View File

@ -620,7 +620,12 @@ uint16_t RCOutput::read(uint8_t chan)
} }
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
if (chan < chan_offset) { if (chan < chan_offset) {
return iomcu.read_channel(chan); uint16_t period_us = iomcu.read_channel(chan);
if ((iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & io_fast_channel_mask)) {
// convert back to 1000 - 2000 range
period_us *= 8;
}
return period_us;
} }
#endif #endif
chan -= chan_offset; chan -= chan_offset;
@ -635,6 +640,10 @@ void RCOutput::read(uint16_t* period_us, uint8_t len)
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
for (uint8_t i=0; i<MIN(len, chan_offset); i++) { for (uint8_t i=0; i<MIN(len, chan_offset); i++) {
period_us[i] = iomcu.read_channel(i); period_us[i] = iomcu.read_channel(i);
if ((iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<i) & io_fast_channel_mask)) {
// convert back to 1000 - 2000 range
period_us[i] *= 8;
}
} }
#endif #endif
if (len <= chan_offset) { if (len <= chan_offset) {