aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: only set freq on valid IO channels

Browse Source
This commit is contained in:
Andrew Tridgell 2018-07-28 13:44:06 +10:00
parent 16c7ab81dc
commit 1eef29e525
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -80,7 +80,7 @@ void RCOutput::init()
chMtxObjectInit(&trigger_mutex);
// setup default output rate of 50Hz
set_freq(0xFFFF, 50);
set_freq(0xFFFF ^ ((1U<<chan_offset)-1), 50);
#ifdef HAL_GPIO_PIN_SAFETY_IN
safety_state = AP_HAL::Util::SAFETY_DISARMED;
@ -169,10 +169,15 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
// change frequency on IOMCU
uint16_t io_chmask = chmask & 0xFF;
if (freq_hz > 50) {
io_fast_channel_mask = chmask;
io_fast_channel_mask |= io_chmask;
} else {
io_fast_channel_mask &= ~io_chmask;
}
if (io_chmask) {
iomcu.set_freq(io_fast_channel_mask, freq_hz);
}
iomcu.set_freq(chmask, freq_hz);
}
#endif
@ -206,9 +211,6 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
fast_channel_mask |= group.ch_mask;
}
}
if (chmask != update_mask) {
hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
}
}
/*