diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index df98122f70..b4e287711f 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -58,10 +58,11 @@ const struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = } }; +#define NUM_GROUPS ARRAY_SIZE(pwm_group_list) + void ChibiRCOutput::init() { - _num_groups = sizeof(pwm_group_list)/sizeof(pwm_group); - for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { //Start Pwm groups pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg); } @@ -96,7 +97,7 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) return; } - for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { grp_ch_mask = 0XF; if ((grp_ch_mask & chmask) == grp_ch_mask) { update_mask |= grp_ch_mask; @@ -118,7 +119,7 @@ uint16_t ChibiRCOutput::get_freq(uint8_t chan) #endif chan -= chan_offset; - for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { for (uint8_t j = 0; j < 4; j++) { if (pwm_group_list[i].chan[j] == chan) { return pwm_group_list[i].pwm_drv->config->frequency / pwm_group_list[i].pwm_drv->period; @@ -136,7 +137,7 @@ void ChibiRCOutput::enable_ch(uint8_t chan) } chan -= chan_offset; - for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { for (uint8_t j = 0; j < 4; j++) { if ((pwm_group_list[i].chan[j] == chan) && !(en_mask & 1<