Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: implement updated NeoPixel API 

allow for N leds per chain, and setting separate colours per led in
each chain
This commit is contained in:
Andrew Tridgell 2019-09-09 19:23:47 +10:00
parent 1cb608a9f4
commit f441223b55
2 changed files with 142 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

186
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -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<<chan)) {
en_mask |= 1<<chan;
}
}
uint8_t i;
pwm_group *grp = find_chan(chan, i);
if (grp) {
en_mask |= 1U << (chan - chan_offset);
}
}
void RCOutput::disable_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) {
pwmDisableChannel(group.pwm_drv, j);
en_mask &= ~(1<<chan);
}
}
uint8_t i;
pwm_group *grp = find_chan(chan, i);
if (grp) {
pwmDisableChannel(grp->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<<chan, MODE_NEOPIXEL);
return grp->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<<grp->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

35
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -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);