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

Various changes to handle class cleanup

This commit is contained in:
ZodiusInfuser 2022-03-18 12:06:35 +00:00
parent 6c16611e88
commit 6f41834082
5 changed files with 337 additions and 249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

389
drivers/pwm/pwm_cluster.cpp
View File

@ -5,7 +5,7 @@
#include "pwm_cluster.pio.h"
// Uncomment the below line to enable debugging
#define DEBUG_MULTI_PWM
//#define DEBUG_MULTI_PWM
namespace pimoroni {
@ -13,57 +13,19 @@ namespace pimoroni {
static const uint DEBUG_SIDESET = 17;
#endif
int data_dma_channel;
int ctrl_dma_channel;
Sequence sequences[NUM_BUFFERS];
Sequence loop_sequences[NUM_BUFFERS];
uint sequence_index = 0;
volatile uint read_index = 0;
volatile uint last_written_index = 0;
uint irq_gpio = 15;
uint write_gpio = 16;
void __isr pwm_dma_handler() {
// Clear the interrupt request.
dma_hw->ints0 = 1u << data_dma_channel;
gpio_put(irq_gpio, 1); //TOREMOVE Just for debug
////////////////////////////////////////////////////////////////////////////////////////////////////
// STATICS
////////////////////////////////////////////////////////////////////////////////////////////////////
PWMCluster* PWMCluster::clusters[] = { nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr };
uint8_t PWMCluster::claimed_sms[] = { 0x0, 0x0 };
uint PWMCluster::pio_program_offset = 0;
Sequence* seq;
// If new data been written since the last time, switch to reading that buffer
if(last_written_index != read_index) {
read_index = last_written_index;
seq = &sequences[read_index];
}
else {
seq = &loop_sequences[read_index];
}
dma_channel_set_trans_count(data_dma_channel, seq->size << 1, false);
dma_channel_set_read_addr(data_dma_channel, seq->data, true);
gpio_put(irq_gpio, 0); //TOREMOVE Just for debug
}
/***
* From RP2040 datasheet
* *
* One disadvantage of this technique is that we don’t start to reconfigure the channel until some time after the channel
makes its last transfer. If there is heavy interrupt activity on the processor, this may be quite a long time, and therefore
quite a large gap in transfers, which is problematic if we need to sustain a high data throughput.
This is solved by using two channels, with their CHAIN_TO fields crossed over, so that channel A triggers channel B when it
completes, and vice versa. At any point in time, one of the channels is transferring data, and the other is either already
configured to start the next transfer immediately when the current one finishes, or it is in the process of being
reconfigured. When channel A completes, it immediately starts the cued-up transfer on channel B. At the same time, the
interrupt is fired, and the handler reconfigures channel A so that it is ready for when channel B completes.
* */
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer, TransitionData *dat_buffer)
: pio(pio)
, sm(sm)
, pin_mask(pin_mask & ((1u << NUM_BANK0_GPIOS) - 1))
@ -79,14 +41,11 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
}
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
constructor_common(seq_buffer, dat_buffer);
}
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer, TransitionData *dat_buffer)
: pio(pio)
, sm(sm)
, pin_mask(0x00000000)
@ -102,13 +61,10 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
channel_count++;
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
constructor_common(seq_buffer, dat_buffer);
}
PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length)
PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer)
: pio(pio)
, sm(sm)
, pin_mask(0x00000000)
@ -126,13 +82,10 @@ PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length)
}
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
constructor_common(seq_buffer, dat_buffer);
}
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer, TransitionData *dat_buffer)
: pio(pio)
, sm(sm)
, pin_mask(0x00000000)
@ -149,143 +102,217 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
}
}
constructor_common(seq_buffer, dat_buffer);
}
void PWMCluster::constructor_common(Sequence *seq_buffer, TransitionData *dat_buffer) {
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
if(seq_buffer == nullptr) {
sequences = new Sequence[NUM_BUFFERS];
loop_sequences = new Sequence[NUM_BUFFERS];
managed_seq_buffer = true;
}
else {
sequences = seq_buffer;
loop_sequences = seq_buffer + NUM_BUFFERS;
managed_seq_buffer = false;
}
if(dat_buffer == nullptr) {
transitions = new TransitionData[BUFFER_SIZE];
looping_transitions = new TransitionData[BUFFER_SIZE];
managed_dat_buffer = true;
}
else {
transitions = dat_buffer;
looping_transitions = dat_buffer + BUFFER_SIZE;
managed_dat_buffer = false;
}
// Set up the transition buffers
for(uint i = 0; i < NUM_BUFFERS; i++) {
Sequence& seq = sequences[i];
Sequence& looping_seq = loop_sequences[i];
seq = Sequence();
looping_seq = Sequence();
// Need to set a delay otherwise a lockup occurs when first changing frequency
seq.data[0].delay = 10;
looping_seq.data[0].delay = 10;
}
}
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 channel = 0; channel < channel_count; channel++) {
gpio_set_function(channel_to_pin_map[channel], GPIO_FUNC_NULL);
if(initialised) {
pio_sm_set_enabled(pio, sm, false);
dma_channel_abort(dma_channel);
dma_channel_set_irq0_enabled(dma_channel, false);
//dma_channel_unclaim(dma_channel); // This does not seem to work
clusters[dma_channel] = nullptr;
pio_sm_unclaim(pio, sm);
uint pio_idx = pio_get_index(pio);
claimed_sms[pio_idx] &= ~(1u << sm);
//If there are no more SMs using the encoder program, then we can remove it from the PIO
if(claimed_sms[pio_idx] == 0) {
#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
}
if(claimed_sms[0] == 0 && claimed_sms[1] == 0) {
irq_remove_handler(DMA_IRQ_0, dma_interrupt_handler);
}
// Reset all the pins this PWM will control back to an unused state
for(uint channel = 0; channel < channel_count; channel++) {
gpio_set_function(channel_to_pin_map[channel], GPIO_FUNC_NULL);
}
}
if(managed_seq_buffer) {
delete[] sequences;
delete[] loop_sequences;
}
if(managed_dat_buffer) {
delete[] transitions;
delete[] looping_transitions;
}
delete[] channels;
}
void PWMCluster::dma_interrupt_handler() {
// Go through each dma channel to see which triggered this interrupt,
// and if there's an associated cluster, have it advance to the next sequence
for(uint8_t channel = 0; channel < NUM_DMA_CHANNELS; channel++) {
if(dma_channel_get_irq0_status(channel) && clusters[channel] != nullptr) {
clusters[channel]->next_dma_sequence();
}
}
}
void PWMCluster::next_dma_sequence() {
gpio_put(irq_gpio, 1); //TOREMOVE Just for debug
// Clear any interrupt request caused by our channel
dma_channel_acknowledge_irq0(dma_channel);
// If new data been written since the last time, switch to reading
// that sequence, otherwise continue with the looping sequence
Sequence* seq;
if(last_written_index != read_index) {
read_index = last_written_index;
seq = &sequences[read_index];
}
else {
seq = &loop_sequences[read_index];
}
// Let the dma channel know the sequence size and data location
dma_channel_set_trans_count(dma_channel, seq->size << 1, false);
dma_channel_set_read_addr(dma_channel, seq->data, true);
gpio_put(irq_gpio, 0); //TOREMOVE Just for debug
}
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
if(!initialised && !pio_sm_is_claimed(pio, sm)) {
dma_channel = dma_claim_unused_channel(false);
if(dma_channel >= 0) {
pio_sm_claim(pio, sm);
uint pio_idx = pio_get_index(pio);
gpio_init(irq_gpio);
gpio_set_dir(irq_gpio, GPIO_OUT);
gpio_init(write_gpio);
gpio_set_dir(write_gpio, GPIO_OUT);
// If this is the first time using a cluster on this PIO, add the program to the PIO memory
if(claimed_sms[pio_idx] == 0) {
#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
}
// Initialise all the channels this PWM will control
for(uint channel = 0; channel < channel_count; channel++) {
pio_gpio_init(pio, channel_to_pin_map[channel]);
gpio_init(irq_gpio);
gpio_set_dir(irq_gpio, GPIO_OUT);
gpio_init(write_gpio);
gpio_set_dir(write_gpio, GPIO_OUT);
// Initialise all the pins this PWM will control
for(uint channel = 0; channel < channel_count; channel++) {
pio_gpio_init(pio, channel_to_pin_map[channel]);
}
// Set their default state and direction
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);
pio_sm_set_consecutive_pindirs(pio, sm, DEBUG_SIDESET, 1, true);
#endif
#ifdef DEBUG_MULTI_PWM
pio_sm_config c = debug_pwm_cluster_program_get_default_config(pio_program_offset);
#else
pio_sm_config c = pwm_cluster_program_get_default_config(pio_program_offset);
#endif
sm_config_set_out_pins(&c, 0, irq_gpio); //TODO change this to be 32
#ifdef DEBUG_MULTI_PWM
sm_config_set_sideset_pins(&c, DEBUG_SIDESET);
#endif
sm_config_set_out_shift(&c, false, true, 32);
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); // We actively do not want a joined FIFO even though we are not needing the RX
float div = clock_get_hz(clk_sys) / 500000;
sm_config_set_clkdiv(&c, div);
dma_channel_config data_config = dma_channel_get_default_config(dma_channel);
channel_config_set_bswap(&data_config, false);
channel_config_set_dreq(&data_config, pio_get_dreq(pio, sm, true));
channel_config_set_transfer_data_size(&data_config, DMA_SIZE_32);
channel_config_set_read_increment(&data_config, true);
dma_channel_configure(
dma_channel,
&data_config,
&pio->txf[sm],
NULL,
0,
false);
dma_channel_set_irq0_enabled(dma_channel, true);
pio_sm_init(pio, sm, pio_program_offset, &c);
pio_sm_set_enabled(pio, sm, true);
if(claimed_sms[0] == 0 && claimed_sms[1] == 0) {
// Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
irq_add_shared_handler(DMA_IRQ_0, dma_interrupt_handler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
irq_set_enabled(DMA_IRQ_0, true);
}
//Keep a record of this cluster for the interrupt callback
clusters[dma_channel] = this;
claimed_sms[pio_idx] |= 1u << sm;
// Manually set the next dma sequence to trigger the first transfer
next_dma_sequence();
initialised = true;
}
}
// Set their default state and direction
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);
pio_sm_set_consecutive_pindirs(pio, sm, DEBUG_SIDESET, 1, true);
#endif
#ifdef DEBUG_MULTI_PWM
pio_sm_config c = debug_pwm_cluster_program_get_default_config(pio_program_offset);
#else
pio_sm_config c = pwm_cluster_program_get_default_config(pio_program_offset);
#endif
sm_config_set_out_pins(&c, 0, irq_gpio); //TODO change this to be 32
#ifdef DEBUG_MULTI_PWM
sm_config_set_sideset_pins(&c, DEBUG_SIDESET);
#endif
sm_config_set_out_shift(&c, false, true, 32);
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); // We actively do not want a joined FIFO even though we are not needing the RX
float div = clock_get_hz(clk_sys) / 5000000;
sm_config_set_clkdiv(&c, div);
pio_sm_init(pio, sm, pio_program_offset, &c);
pio_sm_set_enabled(pio, sm, true);
data_dma_channel = dma_claim_unused_channel(true);
/*ctrl_dma_channel = dma_claim_unused_channel(true);
dma_channel_config ctrl_config = dma_channel_get_default_config(ctrl_dma_channel);
channel_config_set_transfer_data_size(&ctrl_config, DMA_SIZE_32);
//channel_config_set_read_increment(&ctrl_config, false);
//channel_config_set_write_increment(&ctrl_config, false);
channel_config_set_read_increment(&ctrl_config, true);
channel_config_set_write_increment(&ctrl_config, true);
channel_config_set_ring(&ctrl_config, true, 3); // 1 << 3 byte boundary on write ptr
channel_config_set_ring(&ctrl_config, false, 3); // 1 << 3 byte boundary on read ptr
dma_channel_configure(
ctrl_dma_channel,
&ctrl_config,
//The below two work
//&dma_hw->ch[data_dma_channel].al1_transfer_count_trig,
//&transfer_count,
//1,
//These two do not
//&dma_hw->ch[data_dma_channel].al3_read_addr_trig,
//&((uint32_t *)buffer),
&dma_hw->ch[data_dma_channel].al3_transfer_count, // Initial write address
&control_blocks[0],
2,
false
);*/
dma_channel_config data_config = dma_channel_get_default_config(data_dma_channel);
channel_config_set_bswap(&data_config, false);
channel_config_set_dreq(&data_config, pio_get_dreq(pio, sm, true));
channel_config_set_transfer_data_size(&data_config, DMA_SIZE_32);
channel_config_set_read_increment(&data_config, true);
//channel_config_set_chain_to(&data_config, ctrl_dma_channel);
//channel_config_set_ring(&data_config, false, 7);
dma_channel_configure(
data_dma_channel,
&data_config,
&pio->txf[sm],
NULL,
0,
false);
dma_channel_set_irq0_enabled(data_dma_channel, true);
// Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
irq_set_exclusive_handler(DMA_IRQ_0, pwm_dma_handler);
irq_set_enabled(DMA_IRQ_0, true);
// Set up the transition buffers
for(uint i = 0; i < NUM_BUFFERS; i++) {
Sequence& seq = sequences[i];
seq = Sequence();
seq.data[0].delay = 10; // Need to set a delay otherwise a lockup occurs when first changing frequency
Sequence& looping_seq = loop_sequences[i];
looping_seq = Sequence();
looping_seq.data[0].delay = 10; // Need to set a delay otherwise a lockup occurs when first changing frequency
}
// Manually call the handler once, to trigger the first transfer
pwm_dma_handler();
//dma_start_channel_mask(1u << ctrl_dma_channel);
return true;
return initialised;
}
uint8_t PWMCluster::get_chan_count() const {

142
drivers/pwm/pwm_cluster.hpp
View File

@ -8,42 +8,72 @@
namespace pimoroni {
static const uint BUFFER_SIZE = 64; // Set to 64, the maximum number of single rises and falls for 32 channels within a looping time period
struct Transition {
uint32_t mask;
uint32_t delay;
Transition() : mask(0), delay(0) {};
};
static const uint NUM_BUFFERS = 3;
struct Sequence {
uint32_t size;
Transition data[BUFFER_SIZE];
Sequence() : size(1), data({Transition()}) {};
};
struct TransitionData {
//--------------------------------------------------
// Variables
//--------------------------------------------------
uint8_t channel;
uint32_t level;
bool state;
bool dummy;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
TransitionData() : channel(0), level(0), state(false), dummy(false) {};
TransitionData(uint8_t channel, uint32_t level, bool new_state) : channel(channel), level(level), state(new_state), dummy(false) {};
TransitionData(uint32_t level) : channel(0), level(level), state(false), dummy(true) {};
};
class PWMCluster {
//--------------------------------------------------
// Constants
//--------------------------------------------------
private:
static const uint64_t MAX_PWM_CLUSTER_WRAP = UINT16_MAX; // UINT32_MAX works too, but seems to produce less accurate counters
static const uint32_t LOADING_ZONE_SIZE = 3; // The number of dummy transitions to insert into the data to delay the DMA interrupt (if zero then no zone is used)
static const uint32_t LOADING_ZONE_POSITION = 55; // The number of levels before the wrap level to insert the load zone
// Smaller values will make the DMA interrupt trigger closer to the time the data is needed,
// but risks stalling the PIO if the interrupt takes longer due to other processes
public:
static const uint BUFFER_SIZE = 64; // Set to 64, the maximum number of single rises and falls for 32 channels within a looping time period
static const uint NUM_BUFFERS = 3;
//--------------------------------------------------
// Substructures
//--------------------------------------------------
public:
struct Transition {
//--------------------------------------------------
// Variables
//--------------------------------------------------
uint32_t mask;
uint32_t delay;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
Transition() : mask(0), delay(0) {};
};
struct Sequence {
//--------------------------------------------------
// Variables
//--------------------------------------------------
uint32_t size;
Transition data[BUFFER_SIZE];
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
Sequence() : size(1), data({Transition()}) {};
};
struct TransitionData {
//--------------------------------------------------
// Variables
//--------------------------------------------------
uint8_t channel;
uint32_t level;
bool state;
bool dummy;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
TransitionData() : channel(0), level(0), state(false), dummy(false) {};
TransitionData(uint8_t channel, uint32_t level, bool new_state) : channel(channel), level(level), state(new_state), dummy(false) {};
TransitionData(uint32_t level) : channel(0), level(level), state(false), dummy(true) {};
};
private:
struct ChannelState {
//--------------------------------------------------
@ -63,43 +93,55 @@ struct Sequence {
};
//--------------------------------------------------
// Constants
//--------------------------------------------------
private:
static const uint64_t MAX_PWM_CLUSTER_WRAP = UINT16_MAX; // UINT32_MAX works too, but seems to produce less accurate counters
static const uint32_t LOADING_ZONE_SIZE = 3; // The number of dummy transitions to insert into the data to delay the DMA interrupt (if zero then no zone is used)
static const uint32_t LOADING_ZONE_POSITION = 55; // The number of levels before the wrap level to insert the load zone
// Smaller values will make the DMA interrupt trigger closer to the time the data is needed,
// but risks stalling the PIO if the interrupt takes longer due to other processes
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
PIO pio;
uint sm;
uint pio_program_offset;
int dma_channel;
uint pin_mask;
uint8_t channel_count;
ChannelState* channels;
uint8_t channel_to_pin_map[NUM_BANK0_GPIOS];
uint wrap_level;
TransitionData transitions[64];
TransitionData looping_transitions[64];
Sequence *sequences;
Sequence *loop_sequences;
bool managed_seq_buffer = false;
TransitionData *transitions;
TransitionData *looping_transitions;
bool managed_dat_buffer = false;
volatile uint read_index = 0;
volatile uint last_written_index = 0;
bool initialised = false;
//--------------------------------------------------
// Statics
//--------------------------------------------------
static PWMCluster* clusters[NUM_DMA_CHANNELS];
static uint8_t claimed_sms[NUM_PIOS];
static uint pio_program_offset;
static void dma_interrupt_handler();
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
PWMCluster(PIO pio, uint sm, uint pin_mask);
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length);
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
~PWMCluster();
private:
void constructor_common(Sequence *seq_buffer, TransitionData *dat_buffer);
//--------------------------------------------------
// Methods
@ -134,5 +176,7 @@ struct Sequence {
static bool bit_in_mask(uint bit, uint mask);
static void sorted_insert(TransitionData array[], uint &size, const TransitionData &data);
void populate_sequence(const TransitionData transitions[], const uint &data_size, Sequence &seq_out, uint &pin_states_in_out) const;
void next_dma_sequence();
};
}

16
drivers/servo/servo_cluster.cpp
View File

@ -3,23 +3,23 @@
#include <cstdio>
namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pin_mask), pwm_frequency(freq) {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pin_base, pin_count), pwm_frequency(freq) {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pins, length), pwm_frequency(freq) {
ServoCluster::ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pins), pwm_frequency(freq) {
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}

12
drivers/servo/servo_cluster.hpp
View File

@ -4,6 +4,8 @@
#include "pwm_cluster.hpp"
#include "servo_state.hpp"
using namespace pimoroni;
namespace servo {
class ServoCluster {
@ -11,7 +13,7 @@ namespace servo {
// Variables
//--------------------------------------------------
private:
pimoroni::PWMCluster pwms;
PWMCluster pwms;
uint32_t pwm_period;
float pwm_frequency;
ServoState* states;
@ -22,10 +24,10 @@ namespace servo {
// Constructors/Destructor
//--------------------------------------------------
public:
ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
~ServoCluster();

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

@ -4,6 +4,7 @@
#define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o))
using namespace pimoroni;
using namespace servo;
extern "C" {
@ -880,6 +881,8 @@ extern mp_obj_t Servo_calibration(size_t n_args, const mp_obj_t *pos_args, mp_ma
typedef struct _ServoCluster_obj_t {
mp_obj_base_t base;
ServoCluster* cluster;
PWMCluster::Sequence *seq_buf;
PWMCluster::TransitionData *dat_buf;
} _ServoCluster_obj_t;
@ -991,19 +994,31 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
bool auto_phase = args[ARG_auto_phase].u_bool;
self = m_new_obj_with_finaliser(_ServoCluster_obj_t);
self->base.type = &ServoCluster_type;
ServoCluster *cluster;
PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2);
PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2);
if(mask_provided)
self->cluster = new ServoCluster(pio, sm, pin_mask, calibration_type, freq, auto_phase);
cluster = new ServoCluster(pio, sm, pin_mask, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
else
self->cluster = new ServoCluster(pio, sm, pins, pin_count, calibration_type, freq, auto_phase);
self->cluster->init();
cluster = new ServoCluster(pio, sm, pins, pin_count, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
// Cleanup the pins array
if(pins != nullptr)
delete[] pins;
if(!cluster->init()) {
delete cluster;
m_del(PWMCluster::Sequence, seq_buffer, PWMCluster::NUM_BUFFERS * 2);
m_del(PWMCluster::TransitionData, dat_buffer, PWMCluster::BUFFER_SIZE * 2);
mp_raise_msg(&mp_type_RuntimeError, "unable to allocate the hardware resources needed to initialise this ServoCluster. Try running `import gc` followed by `gc.collect()`, then create this ServoCluster");
}
self = m_new_obj_with_finaliser(_ServoCluster_obj_t);
self->base.type = &ServoCluster_type;
self->cluster = cluster;
self->seq_buf = seq_buffer;
self->dat_buf = dat_buffer;
return MP_OBJ_FROM_PTR(self);
}