int led_pwm_servo_set(unsigned channel, uint8_t cvalue) { if (channel >= arraySize(led_pwm_channels)) { return -1; } unsigned timer = led_pwm_channels[channel].timer_index; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || (led_pwm_channels[channel].gpio_out == 0)) { return -1; } unsigned period = led_pwm_timer_get_period(timer); #if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) unsigned value = period - (unsigned)cvalue * period / 255; #else unsigned value = (unsigned)cvalue * period / 255; #endif /* configure the channel */ if (value > 0) { value--; } switch (led_pwm_channels[channel].timer_channel) { case 1: rCCR1(timer) = value; break; case 2: rCCR2(timer) = value; break; case 3: rCCR3(timer) = value; break; case 4: rCCR4(timer) = value; break; default: return -1; } return 0; }
unsigned led_pwm_servo_get(unsigned channel) { if (channel >= 3) { return 0; } unsigned timer = led_pwm_channels[channel].timer_index; servo_position_t value = 0; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || (led_pwm_channels[channel].timer_channel == 0)) { return 0; } /* configure the channel */ switch (led_pwm_channels[channel].timer_channel) { case 1: value = rCCR1(timer); break; case 2: value = rCCR2(timer); break; case 3: value = rCCR3(timer); break; case 4: value = rCCR4(timer); break; } unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); }