HAL_ChibiOS: switch base freq when needed

timers can't do below 123 Hz with a clock of 8MHz, so we need to
change clock frequency based on the target period
This commit is contained in:
Andrew Tridgell 2018-01-13 14:53:29 +11:00
parent ff3b0ec1bd
commit 25b68dc150
3 changed files with 61 additions and 8 deletions

View File

@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
extern AP_IOMCU iomcu;
#endif
const struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
#define NUM_GROUPS ARRAY_SIZE_SIMPLE(pwm_group_list)
@ -91,16 +91,61 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
multicopter properly
*/
update_mask |= grp_ch_mask;
uint16_t freq_set = freq_hz;
if (!pwm_group_list[i].advanced_timer && freq_set > 400) {
freq_set = 400;
}
bool changed_clock = false;
if (freq_set > 400 && pwm_group_list[i].pwm_cfg.frequency == 1000000) {
// need to change to an 8MHz clock
pwm_group_list[i].pwm_cfg.frequency = 8000000;
changed_clock = true;
} else if (freq_set <= 400 && pwm_group_list[i].pwm_cfg.frequency == 8000000) {
// need to change to an 1MHz clock
pwm_group_list[i].pwm_cfg.frequency = 1000000;
}
if (changed_clock) {
pwmStop(pwm_group_list[i].pwm_drv);
pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg);
}
pwmChangePeriod(pwm_group_list[i].pwm_drv,
pwm_group_list[i].pwm_cfg.frequency/freq_hz);
pwm_group_list[i].pwm_cfg.frequency/freq_set);
}
}
if (freq_hz > 50) {
fast_channel_mask |= update_mask;
}
if (chmask != update_mask) {
hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
}
}
/*
set default output rate
*/
void ChibiRCOutput::set_default_rate(uint16_t freq_hz)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
iomcu.set_default_rate(freq_hz);
}
#endif
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
uint16_t grp_ch_mask = 0;
for (uint8_t j=0; j<4; j++) {
if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
}
}
if (grp_ch_mask & fast_channel_mask) {
// don't change fast channels
continue;
}
pwmChangePeriod(pwm_group_list[i].pwm_drv,
pwm_group_list[i].pwm_cfg.frequency/freq_hz);
}
}
uint16_t ChibiRCOutput::get_freq(uint8_t chan)
{
if (chan >= total_channels) {

View File

@ -53,15 +53,22 @@ public:
bool enable_px4io_sbus_out(uint16_t rate_hz) override;
/*
set default update rate
*/
void set_default_rate(uint16_t rate_hz) override;
private:
struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
bool advanced_timer;
uint8_t chan[4]; // chan number, zero based, 255 for disabled
PWMConfig pwm_cfg;
PWMDriver* pwm_drv;
};
enum output_mode _output_mode = MODE_PWM_NORMAL;
static const pwm_group pwm_group_list[];
static pwm_group pwm_group_list[];
uint16_t _esc_pwm_min;
uint16_t _esc_pwm_max;

View File

@ -536,11 +536,12 @@ def write_PWM_config(f):
groups.append('HAL_PWM_GROUP%u' % group)
if n in [1, 8]:
# only the advanced timers do 8MHz clocks
pwm_clock = 8000000
advanced_timer = 'true'
else:
advanced_timer = 'false'
pwm_clock = 1000000
period = 20000 * pwm_clock / 1000000
f.write('''#define HAL_PWM_GROUP%u { \\
f.write('''#define HAL_PWM_GROUP%u { %s, \\
{%u, %u, %u, %u}, \\
/* Group Initial Config */ \\
{ \\
@ -553,7 +554,7 @@ def write_PWM_config(f):
{%s, NULL}, \\
{%s, NULL}, \\
{%s, NULL} \\
}, 0, 0}, &PWMD%u}\n''' % (group,
}, 0, 0}, &PWMD%u}\n''' % (group, advanced_timer,
chan_list[0], chan_list[1], chan_list[2], chan_list[3],
pwm_clock,
period,