forked from Archive/PX4-Autopilot
px4_fmuv6xrt: bidirectional dshot driver
This commit is contained in:
Peter van der Perk
parent
710286da72
commit
45a3020841
|
|
@ -83,7 +83,6 @@ CONFIG_IMXRT_ENET=y
|
|||
CONFIG_IMXRT_FLEXCAN1=y
|
||||
CONFIG_IMXRT_FLEXCAN2=y
|
||||
CONFIG_IMXRT_FLEXCAN3=y
|
||||
CONFIG_IMXRT_FLEXIO1=y
|
||||
CONFIG_IMXRT_FLEXSPI2=y
|
||||
CONFIG_IMXRT_GPIO13_IRQ=y
|
||||
CONFIG_IMXRT_GPIO1_0_15_IRQ=y
|
||||
|
|
|
|||
|
|
@ -33,78 +33,295 @@
|
|||
****************************************************************************/
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/micro_hal.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <imxrt_flexio.h>
|
||||
#include <hardware/imxrt_flexio.h>
|
||||
#include <imxrt_periphclks.h>
|
||||
#include <px4_arch/dshot.h>
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <drivers/drv_dshot.h>
|
||||
#include <stdio.h>
|
||||
#include "barriers.h"
|
||||
|
||||
#include "arm_internal.h"
|
||||
|
||||
#define FLEXIO_BASE IMXRT_FLEXIO1_BASE
|
||||
#define FLEXIO_PREQ 120000000
|
||||
#define DSHOT_TIMERS FLEXIO_SHIFTBUFNIS_COUNT
|
||||
#define DSHOT_THROTTLE_POSITION 5u
|
||||
#define DSHOT_TELEMETRY_POSITION 4u
|
||||
#define NIBBLES_SIZE 4u
|
||||
#define DSHOT_NUMBER_OF_NIBBLES 3u
|
||||
|
||||
static const uint32_t gcr_decode[32] = {
|
||||
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
|
||||
0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF,
|
||||
0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7,
|
||||
0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0
|
||||
};
|
||||
|
||||
uint32_t erpms[DSHOT_TIMERS];
|
||||
|
||||
typedef enum {
|
||||
DSHOT_START = 0,
|
||||
DSHOT_12BIT_TRANSFERRED,
|
||||
DSHOT_TRANSMIT_COMPLETE,
|
||||
BDSHOT_RECEIVE,
|
||||
BDSHOT_RECEIVE_COMPLETE,
|
||||
} dshot_state;
|
||||
|
||||
typedef struct dshot_handler_t {
|
||||
bool init;
|
||||
uint32_t data_seg1;
|
||||
uint32_t irq_data;
|
||||
dshot_state state;
|
||||
bool bdshot;
|
||||
uint32_t raw_response;
|
||||
uint16_t erpm;
|
||||
uint32_t crc_error_cnt;
|
||||
uint32_t frame_error_cnt;
|
||||
uint32_t no_response_cnt;
|
||||
} dshot_handler_t;
|
||||
|
||||
static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {};
|
||||
|
||||
struct flexio_dev_s *flexio_s;
|
||||
static uint32_t dshot_tcmp;
|
||||
static uint32_t bdshot_tcmp;
|
||||
static uint32_t dshot_mask;
|
||||
static uint32_t bdshot_recv_mask;
|
||||
|
||||
static inline uint32_t flexio_getreg32(uint32_t offset)
|
||||
{
|
||||
return getreg32(FLEXIO_BASE + offset);
|
||||
}
|
||||
|
||||
static inline void flexio_modifyreg32(unsigned int offset,
|
||||
uint32_t clearbits,
|
||||
uint32_t setbits)
|
||||
{
|
||||
modifyreg32(FLEXIO_BASE + offset, clearbits, setbits);
|
||||
}
|
||||
|
||||
static inline void flexio_putreg32(uint32_t value, uint32_t offset)
|
||||
{
|
||||
putreg32(value, FLEXIO_BASE + offset);
|
||||
}
|
||||
|
||||
static inline void enable_shifter_status_interrupts(uint32_t mask)
|
||||
{
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask);
|
||||
}
|
||||
|
||||
static inline void disable_shifter_status_interrupts(uint32_t mask)
|
||||
{
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0);
|
||||
}
|
||||
|
||||
static inline uint32_t get_shifter_status_flags(void)
|
||||
{
|
||||
return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
|
||||
}
|
||||
|
||||
static inline void clear_shifter_status_flags(uint32_t mask)
|
||||
{
|
||||
flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
|
||||
}
|
||||
|
||||
static inline void enable_timer_status_interrupts(uint32_t mask)
|
||||
{
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask);
|
||||
}
|
||||
|
||||
static inline void disable_timer_status_interrupts(uint32_t mask)
|
||||
{
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0);
|
||||
}
|
||||
|
||||
static inline uint32_t get_timer_status_flags(void)
|
||||
{
|
||||
return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET);
|
||||
}
|
||||
|
||||
static inline void clear_timer_status_flags(uint32_t mask)
|
||||
{
|
||||
flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET);
|
||||
}
|
||||
|
||||
static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted)
|
||||
{
|
||||
/* Disable Shifter */
|
||||
flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
/* No start bit, stop bit low */
|
||||
flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
|
||||
FLEXIO_SHIFTCFG_PWIDTH(0) |
|
||||
FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_LOW) |
|
||||
FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE),
|
||||
IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Transmit mode, output to FXIO pin, inverted output for bdshot */
|
||||
flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
|
||||
FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
|
||||
FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) |
|
||||
FLEXIO_SHIFTCTL_PINSEL(pin) |
|
||||
FLEXIO_SHIFTCTL_PINPOL(inverted) |
|
||||
FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_TRANSMIT),
|
||||
IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Start transmitting on trigger, disable on compare */
|
||||
flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET) |
|
||||
FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
|
||||
FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_NEVER) |
|
||||
FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
|
||||
FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH) |
|
||||
FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_DISABLED) |
|
||||
FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED),
|
||||
IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
|
||||
|
||||
flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Baud mode, Trigger on shifter write */
|
||||
flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) |
|
||||
FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW) |
|
||||
FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
|
||||
FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
|
||||
FLEXIO_TIMCTL_PINSEL(0) |
|
||||
FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
|
||||
FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
|
||||
IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
}
|
||||
|
||||
static int flexio_irq_handler(int irq, void *context, void *arg)
|
||||
{
|
||||
|
||||
uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s);
|
||||
uint32_t instance;
|
||||
uint32_t flags = get_shifter_status_flags();
|
||||
uint32_t channel;
|
||||
|
||||
for (instance = 0; flags && instance < DSHOT_TIMERS; instance++) {
|
||||
if (flags & (1 << instance)) {
|
||||
flexio_s->ops->disable_shifter_status_interrupts(flexio_s, (1 << instance));
|
||||
flexio_s->ops->disable_timer_status_interrupts(flexio_s, (1 << instance));
|
||||
for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
|
||||
if (flags & (1 << channel)) {
|
||||
disable_shifter_status_interrupts(1 << channel);
|
||||
|
||||
if (dshot_inst[instance].irq_data != 0) {
|
||||
uint32_t buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, instance);
|
||||
putreg32(dshot_inst[instance].irq_data, IMXRT_FLEXIO1_BASE + buf_adr);
|
||||
dshot_inst[instance].irq_data = 0;
|
||||
if (dshot_inst[channel].irq_data != 0) {
|
||||
flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
|
||||
dshot_inst[channel].irq_data = 0;
|
||||
|
||||
} else if (dshot_inst[channel].irq_data == 0 && dshot_inst[channel].state == BDSHOT_RECEIVE) {
|
||||
dshot_inst[channel].state = BDSHOT_RECEIVE_COMPLETE;
|
||||
dshot_inst[channel].raw_response = flexio_getreg32(IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4);
|
||||
|
||||
bdshot_recv_mask |= (1 << channel);
|
||||
|
||||
if (bdshot_recv_mask == dshot_mask) {
|
||||
// Received telemetry on all channels
|
||||
// Schedule workqueue?
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
flags = get_timer_status_flags();
|
||||
|
||||
for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
|
||||
clear_timer_status_flags(1 << channel);
|
||||
|
||||
if (flags & (1 << channel)) {
|
||||
if (dshot_inst[channel].state == DSHOT_START) {
|
||||
dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED;
|
||||
|
||||
} else if (!dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
|
||||
dshot_inst[channel].state = DSHOT_TRANSMIT_COMPLETE;
|
||||
|
||||
} else if (dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
|
||||
disable_shifter_status_interrupts(1 << channel);
|
||||
dshot_inst[channel].state = BDSHOT_RECEIVE;
|
||||
uint8_t timer = timer_io_channels[channel].timer_index;
|
||||
|
||||
/* Transmit done, disable timer and reconfigure to receive*/
|
||||
flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Input data from pin, no start/stop bit*/
|
||||
flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
|
||||
FLEXIO_SHIFTCFG_PWIDTH(0) |
|
||||
FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_DISABLE) |
|
||||
FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_SHIFT),
|
||||
IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Shifter receive mdoe, on FXIO pin input */
|
||||
flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
|
||||
FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
|
||||
FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
|
||||
FLEXIO_SHIFTCTL_PINSEL(io_timers[timer].dshot.flexio_pin) |
|
||||
FLEXIO_SHIFTCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
|
||||
FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_RECEIVE),
|
||||
IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Make sure there no shifter flags high from transmission */
|
||||
clear_shifter_status_flags(1 << channel);
|
||||
|
||||
/* Enable on pin transition, resychronize through reset on rising edge */
|
||||
flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_AFFECTED_BY_RESET) |
|
||||
FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
|
||||
FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_ON_TIMER_PIN_RISING_EDGE) |
|
||||
FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
|
||||
FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_BOTH_EDGE) |
|
||||
FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_ENABLE_ON_TIMER_DISABLE) |
|
||||
FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_ENABLED),
|
||||
IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Enable on pin transition, resychronize through reset on rising edge */
|
||||
flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Trigger on FXIO pin transition, Baud mode */
|
||||
flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * io_timers[timer].dshot.flexio_pin) |
|
||||
FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_HIGH) |
|
||||
FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
|
||||
FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
|
||||
FLEXIO_TIMCTL_PINSEL(0) |
|
||||
FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
|
||||
FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
|
||||
IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
|
||||
|
||||
/* Enable shifter interrupt for receiving data */
|
||||
enable_shifter_status_interrupts(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
|
||||
|
||||
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
|
||||
{
|
||||
uint32_t timer_compare;
|
||||
/* Calculate dshot timings based on dshot_pwm_freq */
|
||||
dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
|
||||
bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);
|
||||
|
||||
if (dshot_pwm_freq == 150000) {
|
||||
timer_compare = 0x2F8A;
|
||||
/* Clock FlexIO peripheral */
|
||||
imxrt_clockall_flexio1();
|
||||
|
||||
} else if (dshot_pwm_freq == 300000) {
|
||||
timer_compare = 0x2F45;
|
||||
/* Reset FlexIO peripheral */
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
|
||||
FLEXIO_CTRL_SWRST_MASK);
|
||||
flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET);
|
||||
|
||||
} else if (dshot_pwm_freq == 600000) {
|
||||
timer_compare = 0x2F22;
|
||||
/* Initialize FlexIO peripheral */
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET,
|
||||
(FLEXIO_CTRL_DOZEN_MASK |
|
||||
FLEXIO_CTRL_DBGE_MASK |
|
||||
FLEXIO_CTRL_FASTACC_MASK |
|
||||
FLEXIO_CTRL_FLEXEN_MASK),
|
||||
(FLEXIO_CTRL_DBGE(1) |
|
||||
FLEXIO_CTRL_FASTACC(0) |
|
||||
FLEXIO_CTRL_FLEXEN(0)));
|
||||
|
||||
} else if (dshot_pwm_freq == 1200000) {
|
||||
timer_compare = 0x2F11;
|
||||
|
||||
} else {
|
||||
// Not supported Dshot frequency
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Init FlexIO peripheral */
|
||||
|
||||
flexio_s = imxrt_flexio_initialize(1);
|
||||
/* FlexIO IRQ handling */
|
||||
up_enable_irq(IMXRT_IRQ_FLEXIO1);
|
||||
irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s);
|
||||
irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0);
|
||||
|
||||
dshot_mask = 0x0;
|
||||
|
||||
for (unsigned channel = 0; (channel_mask != 0) && (channel < DSHOT_TIMERS); channel++) {
|
||||
if (channel_mask & (1 << channel)) {
|
||||
|
|
@ -114,64 +331,107 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
|
|||
continue;
|
||||
}
|
||||
|
||||
imxrt_config_gpio(io_timers[timer].dshot.pinmux);
|
||||
imxrt_config_gpio(io_timers[timer].dshot.pinmux | IOMUX_PULL_UP);
|
||||
|
||||
struct flexio_shifter_config_s shft_cfg;
|
||||
shft_cfg.timer_select = channel;
|
||||
shft_cfg.timer_polarity = FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE;
|
||||
shft_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT;
|
||||
shft_cfg.pin_select = io_timers[timer].dshot.flexio_pin;
|
||||
shft_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_HIGH;
|
||||
shft_cfg.shifter_mode = FLEXIO_SHIFTER_MODE_TRANSMIT;
|
||||
shft_cfg.parallel_width = 0;
|
||||
shft_cfg.input_source = FLEXIO_SHIFTER_INPUT_FROM_PIN;
|
||||
shft_cfg.shifter_stop = FLEXIO_SHIFTER_STOP_BIT_LOW;
|
||||
shft_cfg.shifter_start = FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE;
|
||||
dshot_inst[channel].bdshot = enable_bidirectional_dshot;
|
||||
|
||||
flexio_s->ops->set_shifter_config(flexio_s, channel, &shft_cfg);
|
||||
|
||||
struct flexio_timer_config_s tmr_cfg;
|
||||
tmr_cfg.trigger_select = (4 * channel) + 1;
|
||||
tmr_cfg.trigger_polarity = FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW;
|
||||
tmr_cfg.trigger_source = FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL;
|
||||
tmr_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT_DISABLED;
|
||||
tmr_cfg.pin_select = 0;
|
||||
tmr_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_LOW;
|
||||
tmr_cfg.timer_mode = FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT;
|
||||
tmr_cfg.timer_output = FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET;
|
||||
tmr_cfg.timer_decrement = FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT;
|
||||
tmr_cfg.timer_reset = FLEXIO_TIMER_RESET_NEVER;
|
||||
tmr_cfg.timer_disable = FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE;
|
||||
tmr_cfg.timer_enable = FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH;
|
||||
tmr_cfg.timer_stop = FLEXIO_TIMER_STOP_BIT_DISABLED;
|
||||
tmr_cfg.timer_start = FLEXIO_TIMER_START_BIT_DISABLED;
|
||||
tmr_cfg.timer_compare = timer_compare;
|
||||
flexio_s->ops->set_timer_config(flexio_s, channel, &tmr_cfg);
|
||||
flexio_dshot_output(channel, io_timers[timer].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
|
||||
|
||||
dshot_inst[channel].init = true;
|
||||
|
||||
// Mask channel to be active on dshot
|
||||
dshot_mask |= (1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
flexio_s->ops->enable(flexio_s, true);
|
||||
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
|
||||
FLEXIO_CTRL_FLEXEN_MASK);
|
||||
|
||||
return channel_mask;
|
||||
}
|
||||
|
||||
|
||||
void up_bdshot_erpm(void)
|
||||
{
|
||||
uint32_t value;
|
||||
uint32_t erpm;
|
||||
uint32_t csum_data;
|
||||
|
||||
// Decode each individual channel
|
||||
for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
|
||||
if (bdshot_recv_mask & (1 << channel)) {
|
||||
value = ~dshot_inst[channel].raw_response & 0xFFFFF;
|
||||
|
||||
/* if lowest significant isn't 1 we've got a framing error */
|
||||
if (value & 0x1) {
|
||||
/* Decode RLL */
|
||||
value = (value ^ (value >> 1));
|
||||
|
||||
/* Decode GCR */
|
||||
erpm = gcr_decode[value & 0x1fU];
|
||||
erpm |= gcr_decode[(value >> 5U) & 0x1fU] << 4U;
|
||||
erpm |= gcr_decode[(value >> 10U) & 0x1fU] << 8U;
|
||||
erpm |= gcr_decode[(value >> 15U) & 0x1fU] << 12U;
|
||||
|
||||
/* Calculate checksum */
|
||||
csum_data = erpm;
|
||||
csum_data = csum_data ^ (csum_data >> 8U);
|
||||
csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE);
|
||||
|
||||
if ((csum_data & 0xFU) != 0xFU) {
|
||||
dshot_inst[channel].crc_error_cnt++;
|
||||
|
||||
} else {
|
||||
dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF;
|
||||
//TODO store this or foward this
|
||||
}
|
||||
|
||||
} else {
|
||||
dshot_inst[channel].frame_error_cnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void up_bdshot_status(void)
|
||||
{
|
||||
/* Call this function to calculate last ERPM ideally a workqueue does this.
|
||||
For now this to debug using the dshot status cli command */
|
||||
up_bdshot_erpm();
|
||||
|
||||
for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
|
||||
|
||||
if (dshot_inst[channel].init) {
|
||||
PX4_INFO("Channel %i Last erpm %i value", channel, dshot_inst[channel].erpm);
|
||||
PX4_INFO("CRC errors Frame error No response");
|
||||
PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt,
|
||||
dshot_inst[channel].no_response_cnt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void up_dshot_trigger(void)
|
||||
{
|
||||
uint32_t buf_adr;
|
||||
clear_timer_status_flags(0xFF);
|
||||
|
||||
for (uint8_t motor_number = 0; (motor_number < DSHOT_TIMERS); motor_number++) {
|
||||
if (dshot_inst[motor_number].init && dshot_inst[motor_number].data_seg1 != 0) {
|
||||
buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, motor_number);
|
||||
putreg32(dshot_inst[motor_number].data_seg1, IMXRT_FLEXIO1_BASE + buf_adr);
|
||||
for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
|
||||
if ((bdshot_recv_mask & (1 << channel)) == 0) {
|
||||
dshot_inst[channel].no_response_cnt++;
|
||||
}
|
||||
|
||||
if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) {
|
||||
flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
|
||||
}
|
||||
}
|
||||
|
||||
flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF);
|
||||
flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF);
|
||||
bdshot_recv_mask = 0x0;
|
||||
|
||||
clear_timer_status_flags(0xFF);
|
||||
enable_shifter_status_interrupts(0xFF);
|
||||
enable_timer_status_interrupts(0xFF);
|
||||
}
|
||||
|
||||
/* Expand packet from 16 bits 48 to get T0H and T1H timing */
|
||||
uint64_t dshot_expand_data(uint16_t packet)
|
||||
{
|
||||
unsigned int mask;
|
||||
|
|
@ -197,16 +457,24 @@ uint64_t dshot_expand_data(uint16_t packet)
|
|||
* bit 12 - dshot telemetry enable/disable
|
||||
* bits 13-16 - XOR checksum
|
||||
**/
|
||||
void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
|
||||
void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
|
||||
{
|
||||
if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) {
|
||||
uint8_t timer = timer_io_channels[channel].timer_index;
|
||||
|
||||
if (channel < DSHOT_TIMERS && dshot_inst[channel].init) {
|
||||
uint16_t csum_data;
|
||||
uint16_t packet = 0;
|
||||
uint16_t checksum = 0;
|
||||
|
||||
packet |= throttle << DSHOT_THROTTLE_POSITION;
|
||||
packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
|
||||
|
||||
uint16_t csum_data = packet;
|
||||
if (dshot_inst[channel].bdshot) {
|
||||
csum_data = ~packet;
|
||||
|
||||
} else {
|
||||
csum_data = packet;
|
||||
}
|
||||
|
||||
/* XOR checksum calculation */
|
||||
csum_data >>= NIBBLES_SIZE;
|
||||
|
|
@ -219,8 +487,19 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
|
|||
packet |= (checksum & 0x0F);
|
||||
|
||||
uint64_t dshot_expanded = dshot_expand_data(packet);
|
||||
dshot_inst[motor_number].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
|
||||
dshot_inst[motor_number].irq_data = (uint32_t)(dshot_expanded >> 24);
|
||||
dshot_inst[channel].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
|
||||
dshot_inst[channel].irq_data = (uint32_t)(dshot_expanded >> 24);
|
||||
dshot_inst[channel].state = DSHOT_START;
|
||||
|
||||
if (dshot_inst[channel].bdshot) {
|
||||
|
||||
flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
|
||||
disable_shifter_status_interrupts(1 << channel);
|
||||
|
||||
flexio_dshot_output(channel, io_timers[timer].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
|
||||
|
||||
clear_timer_status_flags(0xFF);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue