mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2025-12-09 13:21:35 +08:00
[Sponsored by ARK] Bidirectional DShot (#23863)
* Bidirectional DShot Co-authored-by: Julian Oes <julian@oes.ch> * f4/f1 support, not supported * fix f1 build target * sanity check timer_channel value, fix CCxNP ifdef, debug stuff * removed debug code, added define for H7 HAVE_GTIM_CCXNP * round robin sampling for less than 4 DMA * unlimited esc_status logging * dshot: fix formatting * dshot: add define for number of DMA channels to use This allows individual boards to override the number of DShot channels and hence avoid round robin capture of the RPM feedback. * ARK: enable 4 DMA channels for DShot on 6X * dshot: publish when all channels are updated This slows down the ESC_STATUS publication in the case of round robin capture. E.g. for 800 Hz output with one DMA channel, the ESC_STATUS is now published at 200 Hz. * dshot: avoid duplicate publications for bidir and telem Instead of publishing both bidirectional dshot updates as well as telemetry updates, we now combine the data from both streams, and publish whenever we get RPM updates, as the latter arrives with higher rate, e.g. 200 Hz with round robin, or faster otherwise. When combining the data, we take RPM from bidirectional dshot, and the rest from telemetry. When we have only one of the two, either telemetry or bidirectional dshot, we just publish that one. * boards: add ark fpv and pi6x BOARD_DMA_NUM_DSHOT_CHANNELS * dshot: turn off debug build --------- Co-authored-by: Julian Oes <julian@oes.ch> Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Jacob Dahl
GitHub
@@ -420,6 +420,9 @@
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board has 4 DMA channels available for bidirectional dshot */
|
||||
#define BOARD_DMA_NUM_DSHOT_CHANNELS 4
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
@@ -307,6 +307,9 @@
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board has 3 DMA channels available for bidirectional dshot */
|
||||
#define BOARD_DMA_NUM_DSHOT_CHANNELS 3
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
@@ -58,7 +58,7 @@
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer8, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer4),
|
||||
};
|
||||
|
||||
constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
|
||||
|
||||
@@ -309,6 +309,9 @@
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board has 4 DMA channels available for bidirectional dshot */
|
||||
#define BOARD_DMA_NUM_DSHOT_CHANNELS 4
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
@@ -17,7 +17,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
|
||||
CONFIG_DRIVERS_OSD_ATXXXX=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
@@ -47,6 +46,6 @@ CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
|
||||
# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
# CONFIG_SYSTEMCMDS_DMESG is not set
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
# CONFIG_SYSTEMCMDS_TOP is not set
|
||||
|
||||
@@ -418,6 +418,21 @@ void up_bdshot_erpm(void)
|
||||
}
|
||||
|
||||
|
||||
int up_bdshot_num_erpm_ready(void)
|
||||
{
|
||||
int num_ready = 0;
|
||||
|
||||
for (unsigned i = 0; i < DSHOT_TIMERS; ++i) {
|
||||
// We only check that data has been received, rather than if it's valid.
|
||||
// This ensures data is published even if one channel has bit errors.
|
||||
if (bdshot_recv_mask & (1 << i)) {
|
||||
++num_ready;
|
||||
}
|
||||
}
|
||||
|
||||
return num_ready;
|
||||
}
|
||||
|
||||
|
||||
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
|
||||
{
|
||||
|
||||
@@ -151,9 +151,6 @@ __EXPORT int io_timer_free_channel(unsigned channel);
|
||||
__EXPORT int io_timer_get_channel_mode(unsigned channel);
|
||||
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
|
||||
__EXPORT extern void io_timer_trigger(void);
|
||||
__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
|
||||
|
||||
__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
|
||||
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as GPIO output.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
* Author: Igor Misic <igy1000mb@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
@@ -58,5 +58,6 @@
|
||||
*/
|
||||
typedef struct dshot_conf_t {
|
||||
uint32_t dma_base;
|
||||
uint32_t dmamap;
|
||||
uint32_t dma_map_up;
|
||||
uint32_t dma_map_ch[4];
|
||||
} dshot_conf_t;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -81,6 +81,8 @@ typedef enum io_timer_channel_mode_t {
|
||||
IOTimerChanMode_PPS = 8,
|
||||
IOTimerChanMode_RPM = 9,
|
||||
IOTimerChanMode_Other = 10,
|
||||
IOTimerChanMode_DshotInverted = 11,
|
||||
IOTimerChanMode_CaptureDMA = 12,
|
||||
IOTimerChanModeSize
|
||||
} io_timer_channel_mode_t;
|
||||
|
||||
@@ -159,7 +161,12 @@ __EXPORT int io_timer_unallocate_channel(unsigned channel);
|
||||
__EXPORT int io_timer_get_channel_mode(unsigned channel);
|
||||
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
|
||||
__EXPORT extern void io_timer_trigger(unsigned channels_mask);
|
||||
|
||||
__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
|
||||
__EXPORT int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
|
||||
|
||||
__EXPORT void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable);
|
||||
__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, unsigned dshot_pwm_freq);
|
||||
|
||||
/**
|
||||
* Reserve a timer
|
||||
@@ -169,7 +176,6 @@ __EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mod
|
||||
|
||||
__EXPORT int io_timer_unallocate_timer(unsigned timer);
|
||||
|
||||
__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
|
||||
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as GPIO output.
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -103,7 +103,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannelOutputClear(const
|
||||
}
|
||||
|
||||
|
||||
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {})
|
||||
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {})
|
||||
{
|
||||
bool nuttx_config_timer_enabled = false;
|
||||
io_timers_t ret{};
|
||||
@@ -268,9 +268,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
|
||||
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)");
|
||||
|
||||
// DShot
|
||||
if (dshot_dma.index != DMA::Invalid) {
|
||||
ret.dshot.dma_base = getDMABaseRegister(dshot_dma);
|
||||
ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma);
|
||||
if (dma.index != DMA::Invalid) {
|
||||
ret.dshot.dma_base = getDMABaseRegister(dma);
|
||||
ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma);
|
||||
getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch);
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -365,8 +365,10 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
|
||||
rv = -ENXIO;
|
||||
|
||||
/* Any pins in capture mode */
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
|
||||
if (mode == IOTimerChanMode_Capture ||
|
||||
mode == IOTimerChanMode_CaptureDMA) {
|
||||
|
||||
uint16_t edge_bits = 0xffff;
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -61,7 +61,6 @@
|
||||
#include <stm32_gpio.h>
|
||||
#include <stm32_tim.h>
|
||||
|
||||
|
||||
static int io_timer_handler0(int irq, void *context, void *arg);
|
||||
static int io_timer_handler1(int irq, void *context, void *arg);
|
||||
static int io_timer_handler2(int irq, void *context, void *arg);
|
||||
@@ -73,8 +72,14 @@ static int io_timer_handler7(int irq, void *context, void *arg);
|
||||
|
||||
#if defined(HAVE_GTIM_CCXNP)
|
||||
#define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP
|
||||
#define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP
|
||||
#define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP
|
||||
#define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP
|
||||
#else
|
||||
# define HW_GTIM_CCER_CC1NP 0
|
||||
# define HW_GTIM_CCER_CC2NP 0
|
||||
# define HW_GTIM_CCER_CC3NP 0
|
||||
# define HW_GTIM_CCER_CC4NP 0
|
||||
#endif
|
||||
|
||||
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
|
||||
@@ -96,41 +101,42 @@ static int io_timer_handler7(int irq, void *context, void *arg);
|
||||
|
||||
#define MAX_CHANNELS_PER_TIMER 4
|
||||
|
||||
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
|
||||
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
|
||||
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
|
||||
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
|
||||
|
||||
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
|
||||
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
|
||||
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
|
||||
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
|
||||
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
|
||||
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
|
||||
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
|
||||
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
|
||||
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
|
||||
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
|
||||
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
|
||||
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
|
||||
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
|
||||
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
|
||||
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
|
||||
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
|
||||
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
|
||||
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
|
||||
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
|
||||
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
|
||||
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
|
||||
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
|
||||
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
|
||||
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
|
||||
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
|
||||
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
|
||||
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
|
||||
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
|
||||
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
|
||||
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
|
||||
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
|
||||
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
|
||||
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
|
||||
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
|
||||
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
|
||||
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
|
||||
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
|
||||
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
|
||||
|
||||
#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF)
|
||||
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
|
||||
|
||||
#define CCMR_C1_RESET 0x00ff
|
||||
#define CCMR_C1_NUM_BITS 8
|
||||
#define CCER_C1_NUM_BITS 4
|
||||
#define CCMR_C1_RESET 0x00ff
|
||||
#define CCMR_C1_NUM_BITS 8
|
||||
#define CCER_C1_NUM_BITS 4
|
||||
|
||||
#define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \
|
||||
(GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \
|
||||
(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
|
||||
|
||||
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
|
||||
#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
|
||||
|
||||
#define CCMR_C1_PWMIN_INIT 0 // TBD
|
||||
|
||||
@@ -141,15 +147,15 @@ static int io_timer_handler7(int irq, void *context, void *arg);
|
||||
#endif
|
||||
|
||||
/* The transfer is done to 1 register starting from TIMx_CR1 + TIMx_DCR.DBA */
|
||||
#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U
|
||||
#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U
|
||||
/* The transfer is done to 2 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
|
||||
#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U
|
||||
#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U
|
||||
/* The transfer is done to 3 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
|
||||
#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U
|
||||
#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U
|
||||
/* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
|
||||
#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U
|
||||
#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U
|
||||
|
||||
// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other
|
||||
// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other
|
||||
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
|
||||
|
||||
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
|
||||
@@ -161,15 +167,15 @@ io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { };
|
||||
/* Stats and handlers are only useful for Capture */
|
||||
|
||||
typedef struct channel_stat_t {
|
||||
uint32_t isr_cout;
|
||||
uint32_t overflows;
|
||||
uint32_t isr_cout;
|
||||
uint32_t overflows;
|
||||
} channel_stat_t;
|
||||
|
||||
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
static struct channel_handler_entry {
|
||||
channel_handler_t callback;
|
||||
void *context;
|
||||
void *context;
|
||||
} channel_handlers[MAX_TIMER_IO_CHANNELS];
|
||||
#endif // !defined(BOARD_HAS_NO_CAPTURE)
|
||||
|
||||
@@ -519,7 +525,57 @@ void io_timer_update_dma_req(uint8_t timer, bool enable)
|
||||
}
|
||||
}
|
||||
|
||||
int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
|
||||
void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable)
|
||||
{
|
||||
if (enable) {
|
||||
switch (timer_channel_index) {
|
||||
case 0:
|
||||
rDIER(timer) |= ATIM_DIER_CC1DE;
|
||||
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
rDIER(timer) |= ATIM_DIER_CC2DE;
|
||||
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC2G);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
rDIER(timer) |= ATIM_DIER_CC3DE;
|
||||
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC3G);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rDIER(timer) |= ATIM_DIER_CC4DE;
|
||||
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC4G);
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
switch (timer_channel_index) {
|
||||
case 0:
|
||||
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G);
|
||||
rDIER(timer) &= ~ATIM_DIER_CC1DE;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC2G);
|
||||
rDIER(timer) &= ~ATIM_DIER_CC2DE;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC3G);
|
||||
rDIER(timer) &= ~ATIM_DIER_CC3DE;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC4G);
|
||||
rDIER(timer) &= ~ATIM_DIER_CC4DE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
|
||||
{
|
||||
int ret_val = OK;
|
||||
uint32_t tim_dma_burst_length;
|
||||
@@ -545,7 +601,7 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
|
||||
rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
|
||||
rEGR(timer) = ATIM_EGR_UG;
|
||||
|
||||
// find the lowest channel index for the timer (they are not necesarily in ascending order)
|
||||
// find the lowest channel index for the timer (they are not necessarily in ascending order)
|
||||
unsigned lowest_timer_channel = 4;
|
||||
uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
|
||||
uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
|
||||
@@ -574,6 +630,47 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
|
||||
return ret_val;
|
||||
}
|
||||
|
||||
int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, unsigned dshot_pwm_freq)
|
||||
{
|
||||
// Timer Autor Reload Register max value
|
||||
rARR(timer) = 0xFFFFFFFF;
|
||||
// Timer Prescalar
|
||||
rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
|
||||
|
||||
switch (timer_channel_index) {
|
||||
case 0:
|
||||
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC1G;
|
||||
rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP);
|
||||
rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT);
|
||||
rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP);
|
||||
|
||||
break;
|
||||
|
||||
case 1:
|
||||
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC2G;
|
||||
rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP);
|
||||
rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT);
|
||||
rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC3G;
|
||||
rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP);
|
||||
rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT);
|
||||
rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC4G;
|
||||
rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP);
|
||||
rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT);
|
||||
rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP);
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void io_timer_set_PWM_mode(unsigned timer)
|
||||
{
|
||||
rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
|
||||
@@ -773,6 +870,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
setbits = CCMR_C1_PWMOUT_INIT;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_DshotInverted:
|
||||
ccer_setbits = 0;
|
||||
dier_setbits = 0;
|
||||
setbits = CCMR_C1_PWMOUT_INVERTED_INIT;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_PWMIn:
|
||||
setbits = CCMR_C1_PWMIN_INIT;
|
||||
gpio = timer_io_channels[channel].gpio_in;
|
||||
@@ -781,6 +884,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
#if !defined(BOARD_HAS_NO_CAPTURE)
|
||||
|
||||
case IOTimerChanMode_Capture:
|
||||
case IOTimerChanMode_CaptureDMA:
|
||||
setbits = CCMR_C1_CAPTURE_INIT;
|
||||
gpio = timer_io_channels[channel].gpio_in;
|
||||
break;
|
||||
@@ -897,10 +1001,12 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_Dshot:
|
||||
case IOTimerChanMode_DshotInverted:
|
||||
dier_bit = 0;
|
||||
|
||||
/* fallthrough */
|
||||
case IOTimerChanMode_Capture:
|
||||
case IOTimerChanMode_CaptureDMA:
|
||||
cr1_bit = state ? GTIM_CR1_CEN : 0;
|
||||
|
||||
/* fallthrough */
|
||||
@@ -946,6 +1052,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
|
||||
(mode == IOTimerChanMode_PWMOut ||
|
||||
mode == IOTimerChanMode_OneShot ||
|
||||
mode == IOTimerChanMode_Dshot ||
|
||||
mode == IOTimerChanMode_DshotInverted ||
|
||||
mode == IOTimerChanMode_Trigger))) {
|
||||
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
|
||||
}
|
||||
@@ -985,7 +1092,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
|
||||
/* arm requires the timer be enabled */
|
||||
rCR1(actions) |= cr1_bit;
|
||||
|
||||
} else {
|
||||
} else {
|
||||
|
||||
rCR1(actions) = 0;
|
||||
}
|
||||
@@ -1006,6 +1113,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
|
||||
if ((mode != IOTimerChanMode_PWMOut) &&
|
||||
(mode != IOTimerChanMode_OneShot) &&
|
||||
(mode != IOTimerChanMode_Dshot) &&
|
||||
(mode != IOTimerChanMode_DshotInverted) &&
|
||||
(mode != IOTimerChanMode_Trigger)) {
|
||||
|
||||
rv = -EIO;
|
||||
|
||||
@@ -35,6 +35,11 @@
|
||||
|
||||
#include "../../../stm32_common/include/px4_arch/hw_description.h"
|
||||
|
||||
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
|
||||
{
|
||||
// Not supported
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
|
||||
{
|
||||
// not used on STM32F1
|
||||
|
||||
@@ -35,6 +35,11 @@
|
||||
|
||||
#include "../../../stm32_common/include/px4_arch/hw_description.h"
|
||||
|
||||
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
|
||||
{
|
||||
// Not supported
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
|
||||
{
|
||||
uint32_t dma_map = 0;
|
||||
|
||||
@@ -35,6 +35,47 @@
|
||||
|
||||
#include "../../../stm32_common/include/px4_arch/hw_description.h"
|
||||
|
||||
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
|
||||
{
|
||||
switch (timer) {
|
||||
case Timer::Timer1:
|
||||
if (dma.index == DMA::Index2) {
|
||||
dma_map_ch[0] = DMAMAP_TIM1_CH1;
|
||||
dma_map_ch[1] = DMAMAP_TIM1_CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_TIM1_CH3_2;
|
||||
dma_map_ch[3] = DMAMAP_TIM1_CH4;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer2:
|
||||
case Timer::Timer3:
|
||||
break;
|
||||
|
||||
case Timer::Timer4:
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_TIM4_CH1;
|
||||
dma_map_ch[1] = DMAMAP_TIM4_CH2;
|
||||
dma_map_ch[2] = DMAMAP_TIM4_CH3;
|
||||
dma_map_ch[3] = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer5:
|
||||
case Timer::Timer6:
|
||||
case Timer::Timer7:
|
||||
case Timer::Timer8:
|
||||
case Timer::Timer9:
|
||||
case Timer::Timer10:
|
||||
case Timer::Timer11:
|
||||
case Timer::Timer12:
|
||||
case Timer::Timer13:
|
||||
case Timer::Timer14:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
|
||||
{
|
||||
uint32_t dma_map = 0;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -35,47 +35,211 @@
|
||||
|
||||
#include "../../../stm32_common/include/px4_arch/hw_description.h"
|
||||
|
||||
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
|
||||
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
|
||||
{
|
||||
uint32_t dma_map = 0;
|
||||
|
||||
switch (timer) {
|
||||
case Timer::Timer1:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1;
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_0;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_1;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer2:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1;
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_0;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_1;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer3:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1;
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_0;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_1;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer4:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1;
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_0;
|
||||
dma_map_ch[3] = 0;
|
||||
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_1;
|
||||
dma_map_ch[3] = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer5:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1;
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_0;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_1;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer6:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1;
|
||||
// No channels available
|
||||
break;
|
||||
|
||||
case Timer::Timer7:
|
||||
// No channels available
|
||||
break;
|
||||
|
||||
case Timer::Timer8:
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_0;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_0;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_0;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_1;
|
||||
dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_1;
|
||||
dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_1;
|
||||
dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer9:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer10:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer11:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer12:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer13:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer14:
|
||||
// Non-existent
|
||||
break;
|
||||
|
||||
case Timer::Timer15:
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer16:
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer17:
|
||||
if (dma.index == DMA::Index1) {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_0;
|
||||
|
||||
} else {
|
||||
dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_1;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
|
||||
{
|
||||
uint32_t dma_map_up = 0;
|
||||
|
||||
switch (timer) {
|
||||
case Timer::Timer1:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1;
|
||||
break;
|
||||
|
||||
case Timer::Timer2:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer3:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer4:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer5:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer6:
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer7:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1;
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1;
|
||||
|
||||
break;
|
||||
|
||||
case Timer::Timer8:
|
||||
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1;
|
||||
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1;
|
||||
|
||||
break;
|
||||
|
||||
@@ -91,6 +255,6 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const
|
||||
break;
|
||||
}
|
||||
|
||||
constexpr_assert(dma_map != 0, "Invalid DMA config for given timer");
|
||||
return dma_map;
|
||||
constexpr_assert(dma_map_up != 0, "Invalid DMA config for given timer");
|
||||
return dma_map_up;
|
||||
}
|
||||
|
||||
@@ -33,3 +33,5 @@
|
||||
#pragma once
|
||||
|
||||
#include "../../../stm32_common/include/px4_arch/io_timer.h"
|
||||
|
||||
#define HAVE_GTIM_CCXNP
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -135,7 +135,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {})
|
||||
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {})
|
||||
{
|
||||
bool nuttx_config_timer_enabled = false;
|
||||
io_timers_t ret{};
|
||||
@@ -306,9 +306,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
|
||||
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)");
|
||||
|
||||
// DShot
|
||||
if (dshot_dma.index != DMA::Invalid) {
|
||||
ret.dshot.dma_base = getDMABaseRegister(dshot_dma);
|
||||
ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma);
|
||||
if (dma.index != DMA::Invalid) {
|
||||
ret.dshot.dma_base = getDMABaseRegister(dma);
|
||||
ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma);
|
||||
getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch);
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -96,7 +96,7 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq
|
||||
/**
|
||||
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
|
||||
*/
|
||||
__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry);
|
||||
__EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
|
||||
|
||||
/**
|
||||
* Set the current dshot throttle value for a channel (motor).
|
||||
@@ -143,6 +143,17 @@ __EXPORT extern int up_dshot_arm(bool armed);
|
||||
__EXPORT extern void up_bdshot_status(void);
|
||||
|
||||
|
||||
/**
|
||||
* Get how many bidirectional erpm channels are ready
|
||||
*
|
||||
* When we get the erpm round-robin style, we need to get
|
||||
* and publish the erpms less often.
|
||||
*
|
||||
* @return <0 on error, OK on succes
|
||||
*/
|
||||
__EXPORT extern int up_bdshot_num_erpm_ready(void);
|
||||
|
||||
|
||||
/**
|
||||
* Get bidrectional dshot erpm for a channel
|
||||
* @param channel Dshot channel
|
||||
|
||||
@@ -167,10 +167,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
|
||||
}
|
||||
|
||||
_outputs_initialized = true;
|
||||
|
||||
if (_bidirectional_dshot_enabled) {
|
||||
init_telemetry(NULL);
|
||||
}
|
||||
}
|
||||
|
||||
if (_outputs_initialized) {
|
||||
@@ -179,28 +175,24 @@ void DShot::enable_dshot_outputs(const bool enabled)
|
||||
}
|
||||
}
|
||||
|
||||
void DShot::update_telemetry_num_motors()
|
||||
void DShot::update_num_motors()
|
||||
{
|
||||
if (!_telemetry) {
|
||||
return;
|
||||
}
|
||||
|
||||
int motor_count = 0;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; ++i) {
|
||||
if (_mixing_output.isFunctionSet(i)) {
|
||||
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
|
||||
_actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
|
||||
++motor_count;
|
||||
}
|
||||
}
|
||||
|
||||
_telemetry->handler.setNumMotors(motor_count);
|
||||
_num_motors = motor_count;
|
||||
}
|
||||
|
||||
void DShot::init_telemetry(const char *device)
|
||||
{
|
||||
if (!_telemetry) {
|
||||
_telemetry = new Telemetry{};
|
||||
_telemetry = new DShotTelemetry{};
|
||||
|
||||
if (!_telemetry) {
|
||||
PX4_ERR("alloc failed");
|
||||
@@ -208,32 +200,35 @@ void DShot::init_telemetry(const char *device)
|
||||
}
|
||||
}
|
||||
|
||||
_telemetry->esc_status_pub.advertise();
|
||||
|
||||
if (device != NULL) {
|
||||
int ret = _telemetry->handler.init(device);
|
||||
int ret = _telemetry->init(device);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("telemetry init failed (%i)", ret);
|
||||
}
|
||||
}
|
||||
|
||||
update_telemetry_num_motors();
|
||||
update_num_motors();
|
||||
}
|
||||
|
||||
int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
|
||||
int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm)
|
||||
{
|
||||
int ret = 0;
|
||||
// fill in new motor data
|
||||
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
|
||||
esc_status_s &esc_status = esc_status_pub.get();
|
||||
|
||||
if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
|
||||
esc_status.esc_online_flags |= 1 << telemetry_index;
|
||||
|
||||
esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
|
||||
esc_status.esc[telemetry_index].timestamp = data.time;
|
||||
esc_status.esc[telemetry_index].esc_rpm = (static_cast<int>(data.erpm) * 100) /
|
||||
(_param_mot_pole_count.get() / 2);
|
||||
esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
|
||||
|
||||
if (!ignore_rpm) {
|
||||
// If we also have bidirectional dshot, we use rpm and timestamps from there.
|
||||
esc_status.esc[telemetry_index].timestamp = data.time;
|
||||
esc_status.esc[telemetry_index].esc_rpm = (static_cast<int>(data.erpm) * 100) /
|
||||
(_param_mot_pole_count.get() / 2);
|
||||
}
|
||||
|
||||
esc_status.esc[telemetry_index].esc_voltage = static_cast<float>(data.voltage) * 0.01f;
|
||||
esc_status.esc[telemetry_index].esc_current = static_cast<float>(data.current) * 0.01f;
|
||||
esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
|
||||
@@ -241,34 +236,34 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
|
||||
}
|
||||
|
||||
// publish when motor index wraps (which is robust against motor timeouts)
|
||||
if (telemetry_index <= _telemetry->last_telemetry_index) {
|
||||
if (telemetry_index <= _last_telemetry_index) {
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
|
||||
esc_status.esc_count = _telemetry->handler.numMotors();
|
||||
esc_status.esc_count = _num_motors;
|
||||
++esc_status.counter;
|
||||
|
||||
ret = 1; // Indicate we wrapped, so we publish data
|
||||
}
|
||||
|
||||
_telemetry->last_telemetry_index = telemetry_index;
|
||||
_last_telemetry_index = telemetry_index;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void DShot::publish_esc_status(void)
|
||||
{
|
||||
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
|
||||
esc_status_s &esc_status = esc_status_pub.get();
|
||||
int telemetry_index = 0;
|
||||
|
||||
// clear data of the esc that are offline
|
||||
for (int index = 0; (index < _telemetry->last_telemetry_index); index++) {
|
||||
for (int index = 0; (index < _last_telemetry_index); index++) {
|
||||
if ((esc_status.esc_online_flags & (1 << index)) == 0) {
|
||||
memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s));
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout
|
||||
esc_status.esc_count = _telemetry->handler.numMotors();
|
||||
esc_status.esc_count = _num_motors;
|
||||
esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
|
||||
esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
|
||||
|
||||
@@ -287,8 +282,12 @@ void DShot::publish_esc_status(void)
|
||||
}
|
||||
}
|
||||
|
||||
// ESC telem wrap around or bdshot update
|
||||
_telemetry->esc_status_pub.update();
|
||||
if (!esc_status_pub.advertised()) {
|
||||
esc_status_pub.advertise();
|
||||
|
||||
} else {
|
||||
esc_status_pub.update();
|
||||
}
|
||||
|
||||
// reset esc online flags
|
||||
esc_status.esc_online_flags = 0;
|
||||
@@ -299,13 +298,18 @@ int DShot::handle_new_bdshot_erpm(void)
|
||||
int num_erpms = 0;
|
||||
int telemetry_index = 0;
|
||||
int erpm;
|
||||
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
|
||||
esc_status_s &esc_status = esc_status_pub.get();
|
||||
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
esc_status.counter = _esc_status_counter++;
|
||||
esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
|
||||
esc_status.esc_armed_flags = _outputs_on;
|
||||
|
||||
// We wait until all are ready.
|
||||
if (up_bdshot_num_erpm_ready() < _num_motors) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
if (_mixing_output.isFunctionSet(i)) {
|
||||
if (up_bdshot_get_erpm(i, &erpm) == 0) {
|
||||
@@ -313,7 +317,7 @@ int DShot::handle_new_bdshot_erpm(void)
|
||||
esc_status.esc_online_flags |= 1 << telemetry_index;
|
||||
esc_status.esc[telemetry_index].timestamp = hrt_absolute_time();
|
||||
esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2);
|
||||
esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
|
||||
esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
|
||||
}
|
||||
|
||||
++telemetry_index;
|
||||
@@ -389,7 +393,7 @@ void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index)
|
||||
|
||||
int DShot::request_esc_info()
|
||||
{
|
||||
_telemetry->handler.redirectOutput(*_request_esc_info.load());
|
||||
_telemetry->redirectOutput(*_request_esc_info.load());
|
||||
_waiting_for_esc_info = true;
|
||||
|
||||
int motor_index = _request_esc_info.load()->motor_index;
|
||||
@@ -405,7 +409,8 @@ int DShot::request_esc_info()
|
||||
|
||||
void DShot::mixerChanged()
|
||||
{
|
||||
update_telemetry_num_motors();
|
||||
update_num_motors();
|
||||
|
||||
}
|
||||
|
||||
bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
@@ -420,11 +425,11 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
if (_telemetry) {
|
||||
// check for an ESC info request. We only process it when we're not expecting other telemetry data
|
||||
if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors
|
||||
&& !_telemetry->handler.expectingData() && !_current_command.valid()) {
|
||||
&& !_telemetry->expectingData() && !_current_command.valid()) {
|
||||
requested_telemetry_index = request_esc_info();
|
||||
|
||||
} else {
|
||||
requested_telemetry_index = _telemetry->handler.getRequestMotorIndex();
|
||||
requested_telemetry_index = _telemetry->getRequestMotorIndex();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -534,8 +539,7 @@ void DShot::Run()
|
||||
}
|
||||
|
||||
if (_telemetry) {
|
||||
int telem_update = _telemetry->handler.update();
|
||||
int need_to_publish = 0;
|
||||
const int telem_update = _telemetry->update(_num_motors);
|
||||
|
||||
// Are we waiting for ESC info?
|
||||
if (_waiting_for_esc_info) {
|
||||
@@ -545,20 +549,24 @@ void DShot::Run()
|
||||
}
|
||||
|
||||
} else if (telem_update >= 0) {
|
||||
need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
|
||||
}
|
||||
const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(),
|
||||
_bidirectional_dshot_enabled);
|
||||
|
||||
if (_bidirectional_dshot_enabled) {
|
||||
// Add bdshot data to esc status
|
||||
need_to_publish += handle_new_bdshot_erpm();
|
||||
}
|
||||
|
||||
if (need_to_publish > 0) {
|
||||
// ESC telem wrap around or bdshot update
|
||||
publish_esc_status();
|
||||
// We don't want to publish twice, once by telemetry and once by bidirectional dishot.
|
||||
if (!_bidirectional_dshot_enabled && need_to_publish) {
|
||||
publish_esc_status();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_bidirectional_dshot_enabled) {
|
||||
// Add bdshot data to esc status
|
||||
const int need_to_publish = handle_new_bdshot_erpm();
|
||||
|
||||
if (need_to_publish) {
|
||||
publish_esc_status();
|
||||
}
|
||||
}
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
update_params();
|
||||
@@ -794,7 +802,7 @@ int DShot::print_status()
|
||||
|
||||
if (_telemetry) {
|
||||
PX4_INFO("telemetry on: %s", _telemetry_device);
|
||||
_telemetry->handler.printStatus();
|
||||
_telemetry->printStatus();
|
||||
}
|
||||
|
||||
/* Print dshot status */
|
||||
|
||||
@@ -118,18 +118,14 @@ private:
|
||||
void clear() { num_repetitions = 0; }
|
||||
};
|
||||
|
||||
struct Telemetry {
|
||||
DShotTelemetry handler{};
|
||||
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
int last_telemetry_index{-1};
|
||||
uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
|
||||
};
|
||||
int _last_telemetry_index{-1};
|
||||
uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
|
||||
|
||||
void enable_dshot_outputs(const bool enabled);
|
||||
|
||||
void init_telemetry(const char *device);
|
||||
|
||||
int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
|
||||
int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
|
||||
|
||||
void publish_esc_status(void);
|
||||
|
||||
@@ -141,14 +137,16 @@ private:
|
||||
|
||||
void update_params();
|
||||
|
||||
void update_telemetry_num_motors();
|
||||
void update_num_motors();
|
||||
|
||||
void handle_vehicle_commands();
|
||||
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
uint32_t _reversible_outputs{};
|
||||
|
||||
Telemetry *_telemetry{nullptr};
|
||||
DShotTelemetry *_telemetry{nullptr};
|
||||
|
||||
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
|
||||
static char _telemetry_device[20];
|
||||
static px4::atomic_bool _request_telemetry_init;
|
||||
@@ -165,6 +163,8 @@ private:
|
||||
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
|
||||
uint32_t _output_mask{0};
|
||||
|
||||
int _num_motors{0};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
Command _current_command{};
|
||||
|
||||
@@ -87,7 +87,7 @@ int DShotTelemetry::redirectOutput(OutputBuffer &buffer)
|
||||
return 0;
|
||||
}
|
||||
|
||||
int DShotTelemetry::update()
|
||||
int DShotTelemetry::update(int num_motors)
|
||||
{
|
||||
if (_uart_fd < 0) {
|
||||
return -1;
|
||||
@@ -120,7 +120,7 @@ int DShotTelemetry::update()
|
||||
++_num_timeouts;
|
||||
}
|
||||
|
||||
requestNextMotor();
|
||||
requestNextMotor(num_motors);
|
||||
return -2;
|
||||
}
|
||||
|
||||
@@ -142,7 +142,7 @@ int DShotTelemetry::update()
|
||||
_redirect_output = nullptr;
|
||||
ret = _current_motor_index_request;
|
||||
_current_motor_index_request = -1;
|
||||
requestNextMotor();
|
||||
requestNextMotor(num_motors);
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -153,7 +153,7 @@ int DShotTelemetry::update()
|
||||
ret = _current_motor_index_request;
|
||||
}
|
||||
|
||||
requestNextMotor();
|
||||
requestNextMotor(num_motors);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -225,9 +225,9 @@ uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
|
||||
}
|
||||
|
||||
|
||||
void DShotTelemetry::requestNextMotor()
|
||||
void DShotTelemetry::requestNextMotor(int num_motors)
|
||||
{
|
||||
_current_motor_index_request = (_current_motor_index_request + 1) % _num_motors;
|
||||
_current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
|
||||
_current_request_start = 0;
|
||||
_frame_position = 0;
|
||||
}
|
||||
|
||||
@@ -64,14 +64,12 @@ public:
|
||||
|
||||
void deinit();
|
||||
|
||||
void setNumMotors(int num_motors) { _num_motors = num_motors; }
|
||||
int numMotors() const { return _num_motors; }
|
||||
|
||||
/**
|
||||
* Read telemetry from the UART (non-blocking) and handle timeouts.
|
||||
* @param num_motors How many DShot enabled motors
|
||||
* @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data.
|
||||
*/
|
||||
int update();
|
||||
int update(int num_motors);
|
||||
|
||||
/**
|
||||
* Redirect everything that is read into a different buffer.
|
||||
@@ -112,7 +110,7 @@ private:
|
||||
*/
|
||||
int setBaudrate(unsigned baud);
|
||||
|
||||
void requestNextMotor();
|
||||
void requestNextMotor(int num_motors);
|
||||
|
||||
/**
|
||||
* Decode a single byte from an ESC feedback frame
|
||||
@@ -126,7 +124,6 @@ private:
|
||||
static uint8_t crc8(const uint8_t *buf, uint8_t len);
|
||||
|
||||
int _uart_fd{-1};
|
||||
int _num_motors{0};
|
||||
uint8_t _frame_buffer[ESC_FRAME_SIZE];
|
||||
int _frame_position{0};
|
||||
|
||||
|
||||
@@ -61,7 +61,8 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("external_ins_attitude");
|
||||
add_optional_topic("external_ins_global_position");
|
||||
add_optional_topic("external_ins_local_position");
|
||||
add_optional_topic("esc_status", 250);
|
||||
// add_optional_topic("esc_status", 250);
|
||||
add_topic("esc_status");
|
||||
add_topic("failure_detector_status", 100);
|
||||
add_topic("failsafe_flags");
|
||||
add_optional_topic("follow_target", 500);
|
||||
|
||||
Reference in New Issue
Block a user