diff --git a/drivers/pwm/pwm_cluster.cpp b/drivers/pwm/pwm_cluster.cpp index af624405..6016d18d 100644 --- a/drivers/pwm/pwm_cluster.cpp +++ b/drivers/pwm/pwm_cluster.cpp @@ -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 pins) +PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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 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 { diff --git a/drivers/pwm/pwm_cluster.hpp b/drivers/pwm/pwm_cluster.hpp index f78ccd57..0674e834 100644 --- a/drivers/pwm/pwm_cluster.hpp +++ b/drivers/pwm/pwm_cluster.hpp @@ -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 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 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(); }; } \ No newline at end of file diff --git a/drivers/servo/servo_cluster.cpp b/drivers/servo/servo_cluster.cpp index efcd25cd..8a1e0d47 100644 --- a/drivers/servo/servo_cluster.cpp +++ b/drivers/servo/servo_cluster.cpp @@ -3,23 +3,23 @@ #include 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 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 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); } diff --git a/drivers/servo/servo_cluster.hpp b/drivers/servo/servo_cluster.hpp index 816cc41c..15867944 100644 --- a/drivers/servo/servo_cluster.hpp +++ b/drivers/servo/servo_cluster.hpp @@ -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 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 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(); diff --git a/micropython/modules/servo/servo.cpp b/micropython/modules/servo/servo.cpp index b8086010..c3bcac94 100644 --- a/micropython/modules/servo/servo.cpp +++ b/micropython/modules/servo/servo.cpp @@ -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); }