mirrors
/
pimoroni-pico
mirror of https://github.com/pimoroni/pimoroni-pico.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix for servo cluster frequency being inaccurate at the upper end

This commit is contained in:
ZodiusInfuser 2022-03-10 06:58:26 +00:00
parent 65268098c6
commit c352a795a8
3 changed files with 27 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
drivers/pwm/pwm_cluster.cpp
View File

@ -544,27 +544,35 @@ void PWMCluster::load_pwm() {
}
// Derived from the rp2 Micropython implementation: https://github.com/micropython/micropython/blob/master/ports/rp2/machine_pwm.c
bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out) {
bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint32_t& div256_out) {
bool success = false;
uint32_t source_hz = clock_get_hz(clk_sys) / PWM_CLUSTER_CYCLES;
// Check the provided frequency is valid
if((freq >= 0.01f) && (freq <= (float)(source_hz >> 1))) {
uint32_t div16_top = (uint32_t)((float)(source_hz << 4) / freq);
uint64_t div256_top = (uint64_t)((float)((uint64_t)source_hz << 8) / freq);
uint64_t top = 1;
while(true) {
// Try a few small prime factors to get close to the desired frequency.
if((div16_top >= (5 << 4)) && (div16_top % 5 == 0) && (top * 5 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 5;
if((div256_top >= (11 << 8)) && (div256_top % 11 == 0) && (top * 11 <= MAX_PWM_CLUSTER_WRAP)) {
div256_top /= 11;
top *= 11;
}
else if((div256_top >= (7 << 8)) && (div256_top % 7 == 0) && (top * 7 <= MAX_PWM_CLUSTER_WRAP)) {
div256_top /= 7;
top *= 7;
}
else if((div256_top >= (5 << 8)) && (div256_top % 5 == 0) && (top * 5 <= MAX_PWM_CLUSTER_WRAP)) {
div256_top /= 5;
top *= 5;
}
else if((div16_top >= (3 << 4)) && (div16_top % 3 == 0) && (top * 3 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 3;
else if((div256_top >= (3 << 8)) && (div256_top % 3 == 0) && (top * 3 <= MAX_PWM_CLUSTER_WRAP)) {
div256_top /= 3;
top *= 3;
}
else if((div16_top >= (2 << 4)) && (top * 2 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 2;
else if((div256_top >= (2 << 8)) && (top * 2 <= MAX_PWM_CLUSTER_WRAP)) {
div256_top /= 2;
top *= 2;
}
else {
@ -573,9 +581,9 @@ bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t&
}
// Only return valid factors if the divisor is actually achievable
if(div16_top >= 16 && div16_top <= (UINT8_MAX << 4)) {
if(div256_top >= 256 && div256_top <= (UINT8_MAX << 8)) {
top_out = top;
div16_out = div16_top;
div256_out = div256_top;
success = true;
}

2
drivers/pwm/pwm_cluster.hpp
View File

@ -117,7 +117,7 @@ namespace pimoroni {
//--------------------------------------------------
public:
static bool calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out);
static bool calculate_pwm_factors(float freq, uint32_t& top_out, uint32_t& div256_out);
private:
static bool bit_in_mask(uint bit, uint mask);
static void sorted_insert(TransitionData array[], uint &size, const TransitionData &data);

16
drivers/servo/servo_cluster.cpp
View File

@ -33,8 +33,8 @@ namespace servo {
if(pwms.init()) {
// Calculate a suitable pwm wrap period for this frequency
uint32_t period; uint16_t div16;
if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div16)) {
uint32_t period; uint32_t div256;
if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div256)) {
pwm_period = period;
// Update the pwm before setting the new wrap
@ -49,8 +49,8 @@ namespace servo {
// Apply the new divider
// This is done after loading new PWM values to avoid a lockup condition
uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16;
uint8_t div = div256 >> 8;
uint8_t mod = div256 % 256;
pwms.set_clkdiv_int_frac(div, mod);
success = true;
@ -127,8 +127,8 @@ namespace servo {
if((freq >= ServoState::MIN_FREQUENCY) && (freq <= ServoState::MAX_FREQUENCY)) {
// Calculate a suitable pwm wrap period for this frequency
uint32_t period; uint16_t div16;
if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div16)) {
uint32_t period; uint32_t div256;
if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div256)) {
pwm_period = period;
pwm_frequency = freq;
@ -146,8 +146,8 @@ namespace servo {
pwms.set_wrap(pwm_period, true);
// Apply the new divider
uint16_t div = div16 >> 4;
uint8_t mod = div16 % 16;
uint16_t div = div256 >> 8;
uint8_t mod = div256 % 256;
pwms.set_clkdiv_int_frac(div, mod);
success = true;