int PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < _max_actuators; group++) { // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); if (mask == 0) continue; // all channels in the group must be either default or alt-rate uint32_t alt = rate_map & mask; if (pass == 0) { // preflight if ((alt != 0) && (alt != mask)) { warn("rate group %u mask %x bad overlap %x", group, mask, alt); // not a legal map, bail return -EINVAL; } } else { // set it - errors here are unexpected if (alt != 0) { if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { warn("rate group set alt failed"); return -EINVAL; } } else { if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { warn("rate group set default failed"); return -EINVAL; } } } } } _pwm_alt_rate_channels = rate_map; _pwm_default_rate = default_rate; _pwm_alt_rate = alt_rate; return OK; }
int up_pwm_servo_set_rate(unsigned rate) { for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) up_pwm_servo_set_rate_group_update(i, rate); return 0; }
int up_pwm_servo_set_rate(unsigned rate) { for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { up_pwm_servo_set_rate_group_update(i, rate); } return 0; }
/* * Helper function to handle changes to the PWM rate control registers. */ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); if (mask == 0) { continue; } /* all channels in the group must be either default or alt-rate */ uint32_t alt = map & mask; if (pass == 0) { /* preflight */ if ((alt != 0) && (alt != mask)) { /* not a legal map, bail with an alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; return; } } else { /* set it - errors here are unexpected */ if (alt != 0) { if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; } } else { if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; } } } } } r_setup_pwm_rates = map; r_setup_pwm_defaultrate = defaultrate; r_setup_pwm_altrate = altrate; }