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

Added additional PWM/ServoCluster constructors

This commit is contained in:
ZodiusInfuser 2022-02-21 00:04:36 +00:00
parent d94a7c0718
commit 8fc1270404
6 changed files with 143 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
drivers/pwm/pwm_cluster.cpp
View File

@ -77,14 +77,81 @@ interrupt is fired, and the handler reconfigures channel A so that it is ready f
* */
PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm), channel_mask(channel_mask) {
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask) : pio(pio), sm(sm), pin_mask(pin_mask) {
channel_polarities = 0x00000000;
wrap_level = 0;
// Initialise all the channels this PWM will control
for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
channel_levels[channel] = 0u;
channel_offsets[channel] = 0u;
}
}
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count) : pio(pio), sm(sm) {
pin_mask = 0x00000000;
channel_polarities = 0x00000000;
wrap_level = 0;
// Initialise all the channels this PWM will control
uint pin_end = MIN(pin_count, NUM_BANK0_GPIOS);
for(uint channel = pin_base; channel < pin_end; channel++) {
pin_mask |= (1u << channel);
}
// Initialise all the channels this PWM will control
for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
channel_levels[channel] = 0u;
channel_offsets[channel] = 0u;
}
}
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins) : pio(pio), sm(sm) {
pin_mask = 0x00000000;
channel_polarities = 0x00000000;
wrap_level = 0;
for(auto pin : pins) {
if(pin < NUM_BANK0_GPIOS) {
pin_mask |= (1u << pin);
}
}
// Initialise all the channels this PWM will control
for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
channel_levels[channel] = 0u;
channel_offsets[channel] = 0u;
}
}
PWMCluster::~PWMCluster() {
dma_channel_unclaim(data_dma_channel);
dma_channel_unclaim(ctrl_dma_channel);
pio_sm_set_enabled(pio, sm, false);
#ifdef DEBUG_MULTI_PWM
pio_remove_program(pio, &debug_pwm_cluster_program, pio_program_offset);
#else
pio_remove_program(pio, &pwm_cluster_program, pio_program_offset);
#endif
#ifndef MICROPY_BUILD_TYPE
// pio_sm_unclaim seems to hardfault in MicroPython
pio_sm_unclaim(pio, sm);
#endif
// Reset all the pins this PWM will control back to an unused state
for(uint pin = 0; pin < 32; pin++) { // 32 is number of bits
if((1u << pin) != 0) {
gpio_set_function(pin, GPIO_FUNC_NULL);
}
}
}
bool PWMCluster::init() {
#ifdef DEBUG_MULTI_PWM
pio_program_offset = pio_add_program(pio, &debug_pwm_cluster_program);
#else
pio_program_offset = pio_add_program(pio, &pwm_cluster_program);
#endif
channel_polarities = 0x00000000;
wrap_level = 0;
gpio_init(irq_gpio);
gpio_set_dir(irq_gpio, GPIO_OUT);
@ -93,16 +160,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm),
// Initialise all the channels this PWM will control
for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
if(bit_in_mask(channel, channel_mask)) {
if(bit_in_mask(channel, pin_mask)) {
pio_gpio_init(pio, channel);
channel_levels[channel] = 0u;
channel_offsets[channel] = 0u;
}
}
// Set their default state and direction
pio_sm_set_pins_with_mask(pio, sm, 0x00, channel_mask);
pio_sm_set_pindirs_with_mask(pio, sm, channel_mask, channel_mask);
pio_sm_set_pins_with_mask(pio, sm, 0x00, pin_mask);
pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
#ifdef DEBUG_MULTI_PWM
pio_gpio_init(pio, DEBUG_SIDESET);
@ -189,31 +254,11 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm),
pwm_dma_handler();
//dma_start_channel_mask(1u << ctrl_dma_channel);
return true;
}
PWMCluster::~PWMCluster() {
dma_channel_unclaim(data_dma_channel);
dma_channel_unclaim(ctrl_dma_channel);
pio_sm_set_enabled(pio, sm, false);
#ifdef DEBUG_MULTI_PWM
pio_remove_program(pio, &debug_pwm_cluster_program, pio_program_offset);
#else
pio_remove_program(pio, &pwm_cluster_program, pio_program_offset);
#endif
#ifndef MICROPY_BUILD_TYPE
// pio_sm_unclaim seems to hardfault in MicroPython
pio_sm_unclaim(pio, sm);
#endif
// Reset all the pins this PWM will control back to an unused state
for(uint pin = 0; pin < 32; pin++) { // 32 is number of bits
if((1u << pin) != 0) {
gpio_set_function(pin, GPIO_FUNC_NULL);
}
}
}
uint PWMCluster::get_chan_mask() const {
return channel_mask;
uint PWMCluster::get_pin_mask() const {
return pin_mask;
}
void PWMCluster::set_wrap(uint32_t wrap, bool load) {
@ -223,7 +268,7 @@ void PWMCluster::set_wrap(uint32_t wrap, bool load) {
}
void PWMCluster::set_chan_level(uint8_t channel, uint32_t level, bool load) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
channel_levels[channel] = level;
if(load)
load_pwm();
@ -231,7 +276,7 @@ void PWMCluster::set_chan_level(uint8_t channel, uint32_t level, bool load) {
}
void PWMCluster::set_chan_offset(uint8_t channel, uint32_t offset, bool load) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
channel_offsets[channel] = offset;
if(load)
load_pwm();
@ -239,7 +284,7 @@ void PWMCluster::set_chan_offset(uint8_t channel, uint32_t offset, bool load) {
}
void PWMCluster::set_chan_polarity(uint8_t channel, bool polarity, bool load) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
if(polarity)
channel_polarities |= (1u << channel);
else
@ -272,7 +317,7 @@ void PWMCluster::load_pwm() {
// Go through each channel that we are assigned to
for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
if(bit_in_mask(channel, channel_mask)) {
if(bit_in_mask(channel, pin_mask)) {
// Get the channel polarity, remembering that true means inverted
bool polarity = bit_in_mask(channel, channel_polarities);

14
drivers/pwm/pwm_cluster.hpp
View File

@ -4,6 +4,7 @@
#include "hardware/pio.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include <initializer_list>
namespace pimoroni {
@ -22,9 +23,14 @@ namespace pimoroni {
class PWMCluster {
public:
PWMCluster(PIO pio, uint sm, uint channel_mask);
PWMCluster(PIO pio, uint sm, uint pin_mask);
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
~PWMCluster();
uint get_chan_mask() const;
bool init();
uint get_pin_mask() const;
void set_wrap(uint32_t wrap, bool load = true);
void set_chan_level(uint8_t channel, uint32_t level, bool load = true);
void set_chan_offset(uint8_t channel, uint32_t offset, bool load = true);
@ -34,14 +40,14 @@ namespace pimoroni {
//void set_phase_correct(bool phase_correct);
//void set_enabled(bool enabled);
void load_pwm();
private:
static bool bit_in_mask(uint bit, uint mask);
private:
static void sorted_insert(TransitionData array[], uint &size, const TransitionData &data);
private:
PIO pio;
uint sm;
uint pio_program_offset;
uint channel_mask;
uint pin_mask;
uint channel_levels[NUM_BANK0_GPIOS];
uint channel_offsets[NUM_BANK0_GPIOS];
uint channel_polarities;

61
drivers/servo/servo_cluster.cpp
View File

@ -3,50 +3,53 @@
#include <cstdio>
namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint channel_mask)
: pwms(pio, sm, channel_mask) {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask)
: pwms(pio, sm, pin_mask) {
}
// 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)) {
pwm_period = period;
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
: pwms(pio, sm, pin_base, pin_count) {
}
// Update the pwm before setting the new wrap
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
pwms.set_chan_level(servo, 0, false);
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwms.set_wrap(pwm_period); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
// 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;
pwms.set_clkdiv_int_frac(div, mod);
}
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
: pwms(pio, sm, pins) {
}
ServoCluster::~ServoCluster() {
}
bool ServoCluster::init() {
// pwm_cfg = pwm_get_default_config();
// pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
bool success = false;
// float div = clock_get_hz(clk_sys) / 1000000;
// pwm_config_set_clkdiv(&pwm_cfg, div);
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)) {
pwm_period = period;
// pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
// gpio_set_function(pin, GPIO_FUNC_PWM);
// Update the pwm before setting the new wrap
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
pwms.set_chan_level(servo, 0, false);
}
// pwm_set_gpio_level(pin, 0);
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwms.set_wrap(pwm_period); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
return true;
// 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;
pwms.set_clkdiv_int_frac(div, mod);
success = true;
}
}
return success;
}
uint ServoCluster::get_pin_mask() const {
return pwms.get_chan_mask();
return pwms.get_pin_mask();
}
void ServoCluster::enable(uint servo, bool load) {

4
drivers/servo/servo_cluster.hpp
View File

@ -23,7 +23,9 @@ namespace servo {
// Constructors/Destructor
//--------------------------------------------------
public:
ServoCluster(PIO pio, uint sm, uint channel_mask);
ServoCluster(PIO pio, uint sm, uint pin_mask);
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
~ServoCluster();
//--------------------------------------------------

4
examples/servo2040/servo2040_functions.cpp
View File

@ -58,7 +58,9 @@ int main() {
//PWMCluster pwms(pio1, 0, 0b111111111111111);
//pwms.set_wrap(20000);
ServoCluster cluster(pio1, 0, 0b111100);
//ServoCluster cluster(pio1, 0, 0b111100);
ServoCluster cluster(pio1, 0, {2, 3, 4, 5});
cluster.init();
int speed = DEFAULT_SPEED;
float offset = 0.0f;

20
micropython/modules/servo/servo.cpp
View File

@ -819,14 +819,23 @@ typedef struct _ServoCluster_obj_t {
/***** Print *****/
void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
(void)kind; //Unused input parameter
//_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
mp_print_str(print, "ServoCluster(");
// TODO
//mp_print_str(print, "num_leds = ");
//mp_obj_print_helper(print, mp_obj_new_int(self->apa102->num_leds), PRINT_REPR);
mp_print_str(print, "pins = {");
uint pin_mask = self->cluster->get_pin_mask();
bool first = true;
for(uint pin = 0; pin < NUM_BANK0_GPIOS; pin++) {
if(pimoroni::PWMCluster::bit_in_mask(pin, pin_mask)) {
if(!first) {
mp_print_str(print, ", ");
}
mp_obj_print_helper(print, mp_obj_new_int(pin), PRINT_REPR);
first = false;
}
}
mp_print_str(print, ")");
mp_print_str(print, "})");
}
/***** Destructor ******/
@ -893,6 +902,7 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
self->base.type = &ServoCluster_type;
self->cluster = new ServoCluster(pio1, 0, 0b11111100); //TODO Expose parameters
self->cluster->init();
return MP_OBJ_FROM_PTR(self);
}