mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_ChibiOS: ensure that serial LED setup goes via an intermediate buffer
protect serial LED buffer transfers with mutex protect serial LED buffer size with mutex prevent serial LED interactions before rcout is ready allocate a serial led buffer per channel that needs it setup clock mask when needed
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
ecfcd98f8a
commit
2b885cd79f
|
|
@ -106,6 +106,8 @@ void RCOutput::init()
|
|||
hal.gpio->pinMode(56, 1);
|
||||
hal.gpio->pinMode(57, 1);
|
||||
#endif
|
||||
|
||||
_initialised = true;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -664,11 +666,11 @@ void RCOutput::set_group_mode(pwm_group &group)
|
|||
const uint8_t channels_per_group = 4;
|
||||
const uint16_t bit_length = bits_per_pixel * channels_per_group * group.serial_nleds + (pad_start_bits + pad_end_bits) * channels_per_group;
|
||||
const uint16_t buffer_length = bit_length * sizeof(uint32_t);
|
||||
|
||||
if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, true)) {
|
||||
group.current_mode = MODE_PWM_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
// calculate min time between pulses
|
||||
group.dshot_pulse_time_us = 1000000UL * bit_length / rate;
|
||||
break;
|
||||
|
|
@ -931,23 +933,22 @@ void RCOutput::timer_tick(void)
|
|||
// like low rates
|
||||
if (!serial_group) {
|
||||
dshot_send_groups(true);
|
||||
}
|
||||
|
||||
if (serial_led_pending && chMtxTryLock(&trigger_mutex)) {
|
||||
serial_led_pending = false;
|
||||
for (auto &group : pwm_group_list) {
|
||||
if (group.serial_led_pending && (group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED)) {
|
||||
group.serial_led_pending = !serial_led_send(group);
|
||||
group.prepared_send = group.serial_led_pending;
|
||||
serial_led_pending |= group.serial_led_pending;
|
||||
}
|
||||
}
|
||||
chMtxUnlock(&trigger_mutex);
|
||||
}
|
||||
if (min_pulse_trigger_us == 0 ||
|
||||
serial_group != nullptr) {
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
// if we have enough time left send out LED data
|
||||
if (serial_led_pending) {
|
||||
serial_led_pending = false;
|
||||
for (auto &group : pwm_group_list) {
|
||||
serial_led_pending |= !serial_led_send(group);
|
||||
}
|
||||
}
|
||||
|
||||
if (min_pulse_trigger_us == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (now > min_pulse_trigger_us &&
|
||||
now - min_pulse_trigger_us > 4000) {
|
||||
// trigger at a minimum of 250Hz
|
||||
|
|
@ -1183,12 +1184,27 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
|
|||
*/
|
||||
bool RCOutput::serial_led_send(pwm_group &group)
|
||||
{
|
||||
if (!group.serial_led_pending
|
||||
|| (group.current_mode != MODE_NEOPIXEL && group.current_mode != MODE_PROFILED)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifndef DISABLE_DSHOT
|
||||
if (irq.waiter || !group.dma_handle->lock_nonblock()) {
|
||||
// doing serial output, don't send Serial LED pulses
|
||||
return false;
|
||||
}
|
||||
|
||||
{
|
||||
WITH_SEMAPHORE(group.serial_led_mutex);
|
||||
|
||||
group.serial_led_pending = false;
|
||||
group.prepared_send = false;
|
||||
|
||||
// fill the DMA buffer while we have the lock
|
||||
fill_DMA_buffer_serial_led(group);
|
||||
}
|
||||
|
||||
// start sending the pulses out
|
||||
send_pulses_DMAR(group, group.dma_buffer_len);
|
||||
|
||||
|
|
@ -1781,20 +1797,33 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode)
|
|||
*/
|
||||
bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint16_t clock_mask)
|
||||
{
|
||||
uint8_t i;
|
||||
if (!_initialised) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t i = 0;
|
||||
pwm_group *grp = find_chan(chan, i);
|
||||
if (!grp) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// we must hold the LED mutex while resizing the array
|
||||
WITH_SEMAPHORE(grp->serial_led_mutex);
|
||||
// nothing is as nothing does
|
||||
if (grp->serial_nleds == num_leds
|
||||
&& mode == grp->current_mode
|
||||
&& grp->serial_led_data[i] != nullptr) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (mode) {
|
||||
case MODE_NEOPIXEL: {
|
||||
grp->serial_nleds = MAX(num_leds, grp->serial_nleds);
|
||||
num_leds = MAX(num_leds, grp->serial_nleds);
|
||||
break;
|
||||
}
|
||||
case MODE_PROFILED: {
|
||||
// ProfiLED requires two dummy LED's to mark end of transmission
|
||||
grp->serial_nleds = MAX(num_leds + 2, grp->serial_nleds);
|
||||
num_leds = MAX(num_leds + 2, grp->serial_nleds);
|
||||
|
||||
// Enable any clock channels in the same group
|
||||
grp->clock_mask = 0;
|
||||
|
|
@ -1806,9 +1835,24 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
|
|||
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate the data storage array
|
||||
if (grp->serial_nleds != num_leds || grp->serial_led_data[i] == nullptr) {
|
||||
if (grp->serial_led_data[i] != nullptr) {
|
||||
delete[] grp->serial_led_data[i];
|
||||
grp->serial_led_data[i] = nullptr;
|
||||
}
|
||||
if (num_leds > 0) {
|
||||
grp->serial_led_data[i] = new SerialLed[num_leds];
|
||||
if (grp->serial_led_data[i] == nullptr) {
|
||||
num_leds = 0;
|
||||
}
|
||||
}
|
||||
|
||||
grp->serial_nleds = num_leds;
|
||||
}
|
||||
|
||||
set_output_mode(1U<<chan, mode);
|
||||
|
|
@ -1816,6 +1860,43 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
|
|||
return grp->current_mode == mode;
|
||||
}
|
||||
|
||||
#pragma GCC push_options
|
||||
#pragma GCC optimize("O2")
|
||||
// Fill the group DMA buffer with data to be output
|
||||
void RCOutput::fill_DMA_buffer_serial_led(pwm_group& group)
|
||||
{
|
||||
memset(group.dma_buffer, 0, group.dma_buffer_len);
|
||||
for (uint8_t j = 0; j < 4; j++) {
|
||||
if (group.serial_led_data[j] == nullptr) {
|
||||
if (group.current_mode == MODE_PROFILED && (group.clock_mask & 1U<<j) != 0) {
|
||||
for (uint8_t i = 0; i < group.serial_nleds; i++) {
|
||||
_set_profiled_clock(&group, j, i);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < group.serial_nleds; i++) {
|
||||
const SerialLed& led = group.serial_led_data[j][i];
|
||||
switch (group.current_mode) {
|
||||
case MODE_NEOPIXEL:
|
||||
_set_neopixel_rgb_data(&group, j, i, led.red, led.green, led.blue);
|
||||
break;
|
||||
case MODE_PROFILED: {
|
||||
if (i < group.serial_nleds - 2) {
|
||||
_set_profiled_rgb_data(&group, j, i, led.red, led.green, led.blue);
|
||||
} else {
|
||||
_set_profiled_blank_frame(&group, j, i);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
setup neopixel (WS2812B) output data for a given output channel
|
||||
and a LED number. LED -1 is all LEDs
|
||||
|
|
@ -1883,6 +1964,7 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
|
|||
buf[b * stride] = BIT_1;
|
||||
}
|
||||
}
|
||||
#pragma GCC pop_options
|
||||
|
||||
/*
|
||||
setup serial LED output data for a given output channel
|
||||
|
|
@ -1890,52 +1972,55 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
|
|||
*/
|
||||
void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue)
|
||||
{
|
||||
uint8_t i;
|
||||
pwm_group *grp = find_chan(chan, i);
|
||||
if (!grp) {
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (led >= grp->serial_nleds || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
|
||||
uint8_t i = 0;
|
||||
pwm_group *grp = find_chan(chan, i);
|
||||
|
||||
WITH_SEMAPHORE(grp->serial_led_mutex);
|
||||
|
||||
if (!grp || grp->serial_nleds == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (led == -1) {
|
||||
grp->prepared_send = true;
|
||||
for (uint8_t n=0; n<grp->serial_nleds; n++) {
|
||||
set_serial_led_rgb_data(chan, n, red, green, blue);
|
||||
serial_led_set_single_rgb_data(*grp, i, n, red, green, blue);
|
||||
}
|
||||
return;
|
||||
} else if (!grp->prepared_send) {
|
||||
// if not ouput clock and trailing frames, run through all LED's to do it now
|
||||
set_serial_led_rgb_data(chan, -1, 0, 0, 0);
|
||||
}
|
||||
|
||||
switch (grp->current_mode) {
|
||||
case MODE_NEOPIXEL: {
|
||||
_set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue);
|
||||
// if not ouput clock and trailing frames, run through all LED's to do it now
|
||||
if (!grp->prepared_send) {
|
||||
for (uint8_t n=0; n<grp->serial_nleds; n++) {
|
||||
serial_led_set_single_rgb_data(*grp, i, n, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
serial_led_set_single_rgb_data(*grp, i, uint8_t(led), red, green, blue);
|
||||
}
|
||||
|
||||
/*
|
||||
setup serial LED output data for a given output channel
|
||||
and a LED number. LED -1 is all LEDs
|
||||
*/
|
||||
void RCOutput::serial_led_set_single_rgb_data(pwm_group& group, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue)
|
||||
{
|
||||
if (led >= group.serial_nleds || (group.current_mode != MODE_NEOPIXEL && group.current_mode != MODE_PROFILED)) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (group.current_mode) {
|
||||
case MODE_PROFILED:
|
||||
case MODE_NEOPIXEL:
|
||||
group.serial_led_data[idx][led].red = red;
|
||||
group.serial_led_data[idx][led].green = green;
|
||||
group.serial_led_data[idx][led].blue = blue;
|
||||
break;
|
||||
}
|
||||
|
||||
case MODE_PROFILED: {
|
||||
if (led < grp->serial_nleds - 2) {
|
||||
_set_profiled_rgb_data(grp, i, uint8_t(led), red, green, blue);
|
||||
} else {
|
||||
_set_profiled_blank_frame(grp, i, uint8_t(led));
|
||||
}
|
||||
|
||||
for (uint8_t j = 0; j < 4; j++) {
|
||||
if ((grp->clock_mask & 1U<<j) != 0) {
|
||||
_set_profiled_clock(grp, j, uint8_t(led));
|
||||
}
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1944,17 +2029,25 @@ void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t
|
|||
*/
|
||||
void RCOutput::serial_led_send(const uint16_t chan)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t i;
|
||||
pwm_group *grp = find_chan(chan, i);
|
||||
if (!grp) {
|
||||
return;
|
||||
}
|
||||
if (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED) {
|
||||
|
||||
WITH_SEMAPHORE(grp->serial_led_mutex);
|
||||
|
||||
if (grp->serial_nleds == 0 || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (grp->prepared_send) {
|
||||
serial_led_pending = true;
|
||||
grp->serial_led_pending = true;
|
||||
serial_led_pending = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_HAL_ChibiOS.h"
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include "shared_dma.h"
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
|
|
@ -199,6 +200,12 @@ private:
|
|||
RECV_COMPLETE = 4
|
||||
};
|
||||
|
||||
struct PACKED SerialLed {
|
||||
uint8_t red;
|
||||
uint8_t green;
|
||||
uint8_t blue;
|
||||
};
|
||||
|
||||
/*
|
||||
DShot handling
|
||||
*/
|
||||
|
|
@ -248,10 +255,16 @@ private:
|
|||
uint32_t dshot_pulse_time_us;
|
||||
uint32_t dshot_pulse_send_time_us;
|
||||
virtual_timer_t dma_timeout;
|
||||
uint8_t serial_nleds;
|
||||
|
||||
// serial LED support
|
||||
volatile uint8_t serial_nleds;
|
||||
uint8_t clock_mask;
|
||||
bool serial_led_pending;
|
||||
bool prepared_send;
|
||||
volatile bool serial_led_pending;
|
||||
volatile bool prepared_send;
|
||||
HAL_Semaphore serial_led_mutex;
|
||||
// structure to hold serial LED data until it can be transferred
|
||||
// to the DMA buffer
|
||||
SerialLed* serial_led_data[4];
|
||||
|
||||
// serial output
|
||||
struct {
|
||||
|
|
@ -412,6 +425,8 @@ private:
|
|||
// iomcu output mode (pwm, oneshot or oneshot125)
|
||||
enum output_mode iomcu_mode = MODE_PWM_NORMAL;
|
||||
|
||||
volatile bool _initialised;
|
||||
|
||||
bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
|
||||
|
||||
// find a channel group given a channel number
|
||||
|
|
@ -446,7 +461,9 @@ private:
|
|||
return true if send was successful
|
||||
*/
|
||||
bool serial_led_send(pwm_group &group);
|
||||
bool serial_led_pending;
|
||||
void serial_led_set_single_rgb_data(pwm_group& group, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
|
||||
void fill_DMA_buffer_serial_led(pwm_group& group);
|
||||
volatile bool serial_led_pending;
|
||||
|
||||
void dma_allocate(Shared_DMA *ctx);
|
||||
void dma_deallocate(Shared_DMA *ctx);
|
||||
|
|
|
|||
Loading…
Reference in New Issue