HAL_ChibiOS: ensure RC in channels are bounded

This commit is contained in:
Andrew Tridgell 2018-11-02 22:23:53 +11:00
parent 793ff201b4
commit 49ce9d02b1
1 changed files with 2 additions and 0 deletions

View File

@ -127,6 +127,7 @@ void RCInput::_timer_tick(void)
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
_rcin_timestamp_last_signal = AP_HAL::micros();
_num_channels = rcin_prot.num_channels();
_num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
for (uint8_t i=0; i<_num_channels; i++) {
_rc_values[i] = rcin_prot.read(i);
}
@ -140,6 +141,7 @@ void RCInput::_timer_tick(void)
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
_rcin_timestamp_last_signal = last_radio_us;
_num_channels = radio->num_channels();
_num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
for (uint8_t i=0; i<_num_channels; i++) {
_rc_values[i] = radio->read(i);
}