From f441223b55cdc5da3c0d820b6eda353d810c67a7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2019 19:23:47 +1000 Subject: [PATCH] HAL_ChibiOS: implement updated NeoPixel API allow for N leds per chain, and setting separate colours per led in each chain --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 186 ++++++++++++++++---------- libraries/AP_HAL_ChibiOS/RCOutput.h | 35 +++-- 2 files changed, 142 insertions(+), 79 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 3ac015c07d..76323f5b5a 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -238,68 +238,63 @@ void RCOutput::set_default_rate(uint16_t freq_hz) } } -uint16_t RCOutput::get_freq(uint8_t chan) +/* + find pwm_group and index in group given a channel number + */ +RCOutput::pwm_group *RCOutput::find_chan(uint8_t chan, uint8_t &group_idx) { if (chan >= max_channels) { - return 0; + return nullptr; } -#if HAL_WITH_IO_MCU if (chan < chan_offset) { - return iomcu.get_freq(chan); + return nullptr; } -#endif chan -= chan_offset; for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; for (uint8_t j = 0; j < 4; j++) { if (group.chan[j] == chan) { - return group.pwm_drv->config->frequency / group.pwm_drv->period; + group_idx = j; + return &group; } } } + return nullptr; +} + +uint16_t RCOutput::get_freq(uint8_t chan) +{ +#if HAL_WITH_IO_MCU + if (chan < chan_offset) { + return iomcu.get_freq(chan); + } +#endif + uint8_t i; + pwm_group *grp = find_chan(chan, i); + if (grp) { + return grp->pwm_drv->config->frequency / grp->pwm_drv->period; + } // assume 50Hz default return 50; } void RCOutput::enable_ch(uint8_t chan) { - if (chan >= max_channels) { - return; - } - if (chan < chan_offset) { - return; - } - chan -= chan_offset; - - for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { - pwm_group &group = pwm_group_list[i]; - for (uint8_t j = 0; j < 4; j++) { - if ((group.chan[j] == chan) && !(en_mask & 1<= max_channels) { - return; - } - if (chan < chan_offset) { - return; - } - chan -= chan_offset; - - for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { - pwm_group &group = pwm_group_list[i]; - for (uint8_t j = 0; j < 4; j++) { - if (group.chan[j] == chan) { - pwmDisableChannel(group.pwm_drv, j); - en_mask &= ~(1<pwm_drv, i); + en_mask &= ~(1U<<(chan - chan_offset)); } } @@ -501,11 +496,16 @@ bool RCOutput::mode_requires_dma(enum output_mode mode) const bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length) { #ifndef DISABLE_DSHOT - if (!group.dma_buffer) { + if (!group.dma_buffer || buffer_length != group.dma_buffer_len) { + if (group.dma_buffer) { + hal.util->free_type(group.dma_buffer, group.dma_buffer_len, AP_HAL::Util::MEM_DMA_SAFE); + group.dma_buffer_len = 0; + } group.dma_buffer = (uint32_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE); if (!group.dma_buffer) { return false; } + group.dma_buffer_len = buffer_length; } // for dshot we setup for DMAR based output if (!group.dma_handle) { @@ -602,6 +602,11 @@ void RCOutput::set_group_mode(pwm_group &group) const uint32_t bit_period = 20; // configure timer driver for DMAR at requested rate + const uint8_t pad_bits = 50; + const uint8_t bits_per_pixel = 24; + const uint8_t channels_per_group = 4; + const uint16_t neopixel_bit_length = bits_per_pixel * channels_per_group * group.neopixel_nleds + pad_bits; + const uint16_t neopixel_buffer_length = neopixel_bit_length * sizeof(uint32_t); if (!setup_group_DMA(group, rate, bit_period, true, neopixel_buffer_length)) { group.current_mode = MODE_PWM_NONE; break; @@ -774,8 +779,6 @@ void RCOutput::trigger_groups(void) pwm_group &group = pwm_group_list[i]; if (is_dshot_protocol(group.current_mode)) { dshot_send(group, false); - } else if (group.current_mode == MODE_NEOPIXEL) { - neopixel_send(group); } } } @@ -810,6 +813,16 @@ void RCOutput::timer_tick(void) dshot_send(group, true); } } + if (neopixel_pending && chMtxTryLock(&trigger_mutex)) { + neopixel_pending = false; + for (uint8_t j = 0; j < NUM_GROUPS; j++) { + pwm_group &group = pwm_group_list[j]; + if (group.current_mode == MODE_NEOPIXEL) { + neopixel_send(group); + } + } + chMtxUnlock(&trigger_mutex); + } if (min_pulse_trigger_us == 0 || serial_group != nullptr) { return; @@ -979,23 +992,6 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) } - - -/* - fill in a DMA buffer for Neopixel LED - */ -void RCOutput::fill_DMA_buffer_neopixel(uint32_t *buffer, const uint8_t stride, uint32_t rgb, const uint16_t clockmul) -{ - // TODO: chane clockmul to a define or constant - const uint32_t DSHOT_MOTOR_BIT_0 = 7 * clockmul; - const uint32_t DSHOT_MOTOR_BIT_1 = 14 * clockmul; - - for (uint16_t i=0; i < 24; i++) { - buffer[i * stride] = (rgb & 0x800000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; - rgb <<= 1; - } -} - /* send a set of Neopixel packets for a channel group */ @@ -1007,25 +1003,14 @@ void RCOutput::neopixel_send(pwm_group &group) return; } - memset((uint8_t *)group.dma_buffer, 0, neopixel_buffer_length); - - for (uint8_t i=0; i<4; i++) { - uint8_t chan = group.chan[i]; - if (chan != CHAN_DISABLED) { - fill_DMA_buffer_neopixel(group.dma_buffer + i, 4, neopixel_rgb_data[i], group.bit_width_mul); - } - } - // start sending the pulses out - send_pulses_DMAR(group, neopixel_buffer_length); + send_pulses_DMAR(group, group.dma_buffer_len); group.last_dmar_send_us = AP_HAL::micros64(); #endif //#ifndef DISABLE_DSHOT } - - /* send a series of pulses for a group using DMAR. Pulses must have been encoded into the group dma_buffer with interleaving for the 4 @@ -1612,4 +1597,67 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const return 1; } } + +/* + setup neopixel (WS2812B) output for a given channel number, with + the given max number of LEDs in the chain. +*/ +bool RCOutput::set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) +{ + uint8_t i; + pwm_group *grp = find_chan(chan, i); + if (!grp) { + return false; + } + if (grp->neopixel_nleds == num_leds && grp->current_mode == MODE_NEOPIXEL) { + // no change + return true; + } + grp->neopixel_nleds = MAX(num_leds, grp->neopixel_nleds); + set_output_mode(1U<current_mode == MODE_NEOPIXEL; +} + +/* + setup neopixel (WS2812B) output data for a given output channel + and mask of LEDs on the channel +*/ +void RCOutput::set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue) +{ + uint8_t i; + pwm_group *grp = find_chan(chan, i); + if (!grp) { + return; + } + + // mask out for enabled LEDs + ledmask &= (1U<neopixel_nleds)-1; + + uint8_t led = 0; + while (ledmask) { + if (ledmask & 1) { + const uint8_t neopixel_bit_length = 24; + const uint8_t stride = 4; + uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length) * stride + i; + uint32_t bits = (green<<16) | (red<<8) | blue; + const uint32_t BIT_0 = 7 * grp->bit_width_mul; + const uint32_t BIT_1 = 14 * grp->bit_width_mul; + for (uint16_t b=0; b < 24; b++) { + buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0; + bits <<= 1; + } + } + ledmask >>= 1; + led++; + } +} + +/* + trigger send of neopixel data +*/ +void RCOutput::neopixel_send(void) +{ + neopixel_pending = true; +} + #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 916e14b925..92b73173dc 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -155,7 +155,22 @@ public: reversible_mask |= chanmask; } - void set_neopixel_rgb_data(const uint16_t i, const uint32_t rgb_data) override { neopixel_rgb_data[i] = rgb_data; } + /* + setup neopixel (WS2812B) output for a given channel number, with + the given max number of LEDs in the chain. + */ + bool set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) override; + + /* + setup neopixel (WS2812B) output data for a given output channel + and mask of LEDs on the channel + */ + void set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue) override; + + /* + trigger send of neopixel data + */ + void neopixel_send(void) override; private: struct pwm_group { @@ -177,6 +192,7 @@ private: const stm32_dma_stream_t *dma; Shared_DMA *dma_handle; uint32_t *dma_buffer; + uint16_t dma_buffer_len; bool have_lock; bool pwm_started; uint32_t bit_width_mul; @@ -184,6 +200,7 @@ private: bool in_serial_dma; uint64_t last_dmar_send_us; virtual_timer_t dma_timeout; + uint8_t neopixel_nleds; // serial output struct { @@ -280,6 +297,9 @@ private: // are we using oneshot125 for the iomcu? bool iomcu_oneshot125; + // find a channel group given a channel number + struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx); + // push out values to local PWM void push_local(void); @@ -313,16 +333,11 @@ private: uint32_t dshot_pulse_time_us; uint16_t telem_request_mask; -/* -NeoPixel handling -*/ - uint32_t neopixel_rgb_data[4]; - const uint16_t neopixel_bit_length = 24; - const uint16_t neopixel_buffer_length = (neopixel_bit_length+1)*4*sizeof(uint32_t); + /* + NeoPixel handling. Max of 32 LEDs uses max 12k of memory per group + */ void neopixel_send(pwm_group &group); - void fill_DMA_buffer_neopixel(uint32_t *buffer, const uint8_t stride, uint32_t rgb, const uint16_t clockmul); - - + bool neopixel_pending; void dma_allocate(Shared_DMA *ctx); void dma_deallocate(Shared_DMA *ctx);