diff --git a/conf/airframes/ENAC/quadrotor/cobraV2.xml b/conf/airframes/ENAC/quadrotor/cobraV2.xml
index 30c8bbfc301..29c7d86f9e2 100644
--- a/conf/airframes/ENAC/quadrotor/cobraV2.xml
+++ b/conf/airframes/ENAC/quadrotor/cobraV2.xml
@@ -30,7 +30,9 @@
-
+
+
+
diff --git a/conf/boards/tawaki_2.0.makefile b/conf/boards/tawaki_2.0.makefile
index df6980534bc..47b92f35841 100644
--- a/conf/boards/tawaki_2.0.makefile
+++ b/conf/boards/tawaki_2.0.makefile
@@ -99,3 +99,6 @@ SBUS2_PORT ?= UART6
#
ACTUATORS ?= actuators_pwm
+
+# Bidirectionnal DSHOT timer for input capture timeout
+DSHOT1_GPT_TIM ?= 7
diff --git a/conf/modules/actuators_dshot.xml b/conf/modules/actuators_dshot.xml
index b566789f430..038def8ecc5 100644
--- a/conf/modules/actuators_dshot.xml
+++ b/conf/modules/actuators_dshot.xml
@@ -8,8 +8,15 @@
Beware that servo output from the same timer cannot mix PWM and DSHOT.
It might be required to disable by hand some PWM output to avoid conflicts when they are activated by default on a board.
Currently only implemented over ChibiOS.
+
+ Configure DSHOT_BIDIR to TRUE to enable bidirectionnal DSHOT.
+ It overrides the rpm values from the UART dshot telemetry.
+ UART dshot telemetry is still usefull for current and voltage telemetry.
+
+
+
actuators,@commands
@@ -19,10 +26,21 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/chibios/halconf.h b/sw/airborne/arch/chibios/halconf.h
index b80a11f99a8..e543fb289fc 100644
--- a/sw/airborne/arch/chibios/halconf.h
+++ b/sw/airborne/arch/chibios/halconf.h
@@ -91,8 +91,13 @@
* @brief Enables the GPT subsystem.
*/
#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__)
+#if USE_GPT5 || USE_GPT7 || USE_GPT8 || USE_GPT12 || USE_GPT13
+#define HAL_USE_GPT TRUE
+#define GPT_DRIVER_EXT_FIELDS void *user_data;
+#else
#define HAL_USE_GPT FALSE
#endif
+#endif
/**
* @brief Enables the I2C subsystem.
diff --git a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c
index 7614c21dd76..162f3471482 100644
--- a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c
+++ b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.c
@@ -58,6 +58,10 @@ void gpio_setup_input_pulldown(ioportid_t port, uint16_t gpios)
chSysUnlock();
}
+void gpio_setup_pin_af_pullup(ioportid_t port, uint16_t pin, uint8_t af) {
+ palSetPadMode(port, pin, PAL_MODE_ALTERNATE(af) | PAL_STM32_PUPDR_PULLUP);
+}
+
void gpio_setup_pin_af(ioportid_t port, uint16_t pin, uint8_t af, bool is_output)
{
chSysLock();
diff --git a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h
index f9258468377..f4207c22b7e 100644
--- a/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h
+++ b/sw/airborne/arch/chibios/mcu_periph/gpio_arch.h
@@ -72,6 +72,12 @@ extern void gpio_setup_input_pulldown(ioportid_t port, uint16_t gpios);
*/
extern void gpio_setup_pin_af(ioportid_t port, uint16_t pin, uint8_t af, bool is_output);
+/**
+ * Setup a gpio for input pullup with alternate function.
+ * This is an STM32 specific helper funtion and should only be used in stm32 code.
+*/
+void gpio_setup_pin_af_pullup(ioportid_t port, uint16_t pin, uint8_t af);
+
/**
* Setup a gpio for analog use.
* @param[in] port
diff --git a/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.c b/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.c
index 3933e4a27df..4114f72eeb4 100644
--- a/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.c
+++ b/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.c
@@ -27,6 +27,7 @@
*/
#include "hal_stm32_dma.h"
+#include "string.h"
/*
TODO :
@@ -42,6 +43,15 @@ TODO :
*/
+#if (! STM32_DMA_ADVANCED) && STM32_DMA_USE_DOUBLE_BUFFER
+#error "STM32_DMA_USE_DOUBLE_BUFFER only available on DMAv2"
+#endif
+
+#if (STM32_DMA_USE_ASYNC_TIMOUT) && STM32_DMA_USE_DOUBLE_BUFFER
+#error "STM32_DMA_USE_DOUBLE_BUFFER only not yet compatible with STM32_DMA_USE_ASYNC_TIMOUT"
+#endif
+
+
/**
* @brief DMA ISR service routine.
*
@@ -49,9 +59,12 @@ TODO :
* @param[in] flags pre-shifted content of the ISR register
*/
static void dma_lld_serve_interrupt(DMADriver *dmap, uint32_t flags);
+
+#if STM32_DMA_ADVANCED
static inline uint32_t getFCR_FS(const DMADriver *dmap) {
return (dmap->dmastream->stream->FCR & DMA_SxFCR_FS_Msk);
}
+#endif
void dmaObjectInit(DMADriver *dmap)
{
@@ -92,13 +105,53 @@ void dmaObjectInit(DMADriver *dmap)
bool dmaStart(DMADriver *dmap, const DMAConfig *cfg)
{
osalDbgCheck((dmap != NULL) && (cfg != NULL));
+ #if STM32_DMA_USE_DOUBLE_BUFFER
+ osalDbgAssert((cfg->op_mode != DMA_CONTINUOUS_DOUBLE_BUFFER) || (!STM32_DMA_USE_ASYNC_TIMOUT),
+ "STM32_DMA_USE_ASYNC_TIMOUT not yet implemented in DMA_CONTINUOUS_DOUBLE_BUFFER mode");
+ osalDbgAssert((cfg->op_mode != DMA_CONTINUOUS_DOUBLE_BUFFER) || (cfg->next_cb != NULL),
+ "DMA_CONTINUOUS_DOUBLE_BUFFER mode implies next_cb not NULL");
+#endif
osalSysLock();
osalDbgAssert((dmap->state == DMA_STOP) || (dmap->state == DMA_READY),
"invalid state");
dmap->config = cfg;
- const bool statusOk = dma_lld_start(dmap);
+ const bool statusOk = dma_lld_start(dmap, true);
dmap->state = DMA_READY;
+ #if STM32_DMA_USE_DOUBLE_BUFFER
+ dmap->next_cb_errors = 0U;
+#endif
+ osalSysUnlock();
+ return statusOk;
+}
+
+/**
+ * @brief Configures and activates the DMA peripheral.
+ *
+ * @param[in] dmap pointer to the @p DMADriver object
+ * @param[in] config pointer to the @p DMAConfig object.
+ * @return The operation result.
+ * @retval true dma driver is OK
+ * @retval false incoherencies has been found in config
+ * @api
+ */
+bool dmaReloadConf(DMADriver *dmap, const DMAConfig *cfg)
+{
+ osalDbgCheck((dmap != NULL) && (cfg != NULL));
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ osalDbgAssert((cfg->op_mode != DMA_CONTINUOUS_DOUBLE_BUFFER) || (!STM32_DMA_USE_ASYNC_TIMOUT),
+ "STM32_DMA_USE_ASYNC_TIMOUT not yet implemented in DMA_CONTINUOUS_DOUBLE_BUFFER mode");
+
+ osalDbgAssert((cfg->op_mode != DMA_CONTINUOUS_DOUBLE_BUFFER) || (cfg->next_cb != NULL),
+ "DMA_CONTINUOUS_DOUBLE_BUFFER mode implies next_cb not NULL");
+#endif
+ osalSysLock();
+ osalDbgAssert(dmap->state == DMA_READY, "invalid state");
+ dmap->config = cfg;
+ const bool statusOk = dma_lld_start(dmap, false);
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ dmap->next_cb_errors = 0U;
+#endif
osalSysUnlock();
return statusOk;
}
@@ -115,16 +168,15 @@ void dmaStop(DMADriver *dmap)
{
osalDbgCheck(dmap != NULL);
- osalSysLock();
-
osalDbgAssert((dmap->state == DMA_STOP) || (dmap->state == DMA_READY),
"invalid state");
dma_lld_stop(dmap);
+
+ osalSysLock();
dmap->config = NULL;
dmap->state = DMA_STOP;
dmap->mem0p = NULL;
-
osalSysUnlock();
}
@@ -174,10 +226,21 @@ bool dmaStartTransfert(DMADriver *dmap, volatile void *periphp, void * mem0p, c
bool dmaStartTransfertI(DMADriver *dmap, volatile void *periphp, void * mem0p, const size_t size)
{
osalDbgCheckClassI();
+
+ #if STM32_DMA_USE_DOUBLE_BUFFER
+ if (dmap->config->op_mode == DMA_CONTINUOUS_DOUBLE_BUFFER) {
+ osalDbgAssert(mem0p == NULL,
+ "in double buffer mode memory pointer is dynamically completed by next_cb callback");
+ mem0p = dmap->config->next_cb(dmap, size);
+ }
+#endif
+
#if (CH_DBG_ENABLE_ASSERTS != FALSE)
if (size != dmap->size) {
osalDbgCheck((dmap != NULL) && (mem0p != NULL) && (periphp != NULL) &&
- (size > 0U) && ((size == 1U) || ((size & 1U) == 0U)));
+ (size > 0U) && ((size == 1U) ||
+ ((dmap->config->op_mode != DMA_CONTINUOUS_HALF_BUFFER) ||
+ (((size & 1U) == 0U)))));
const DMAConfig *cfg = dmap->config;
osalDbgAssert((dmap->state == DMA_READY) ||
@@ -223,12 +286,15 @@ bool dmaStartTransfertI(DMADriver *dmap, volatile void *periphp, void * mem0p,
osalDbgAssert((((uint32_t) periphp) % cfg->pburst) == 0,
"peripheral address alignment rule not respected");
}
-
-
+ if (cfg->periph_inc_size_4) {
+ osalDbgAssert(cfg->inc_peripheral_addr,
+ "periph_inc_size_4 implies enabling inc_peripheral_addr");
+ osalDbgAssert(cfg->fifo,
+ "periph_inc_size_4 implies enabling fifo");
+ }
# endif // STM32_DMA_ADVANCED
}
-
#endif // CH_DBG_ENABLE_ASSERTS != FALSE
dmap->state = DMA_ACTIVE;
@@ -243,6 +309,76 @@ bool dmaStartTransfertI(DMADriver *dmap, volatile void *periphp, void * mem0p,
return dma_lld_start_transfert(dmap, periphp, mem0p, size);
}
+/**
+ * @brief copy the dma register to memory.
+ * @details mainly used to preapare mdma linked list chained
+ * transferts
+ *
+ * @param[in] dmap pointer to the @p DMADriver object
+ * @param[in,out] periphp pointer to a @p peripheral register address
+ * @param[in,out] mem0p pointer to the data buffer
+ * @param[in] size buffer size. The buffer size
+ * must be one or an even number.
+ * @param[out] registers pointer to structure representing
+ a DMA stream set of registers
+ *
+ * @iclass
+ */
+#ifndef DMA_request_TypeDef
+void dmaGetRegisters(DMADriver *dmap, volatile void *periphp, void *mem0p,
+ const size_t size,
+ DMA_Stream_TypeDef *registers)
+{
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ if (dmap->config->op_mode == DMA_CONTINUOUS_DOUBLE_BUFFER) {
+ osalDbgAssert(mem0p == NULL,
+ "in double buffer mode memory pointer is dynamically completed by next_cb callback");
+ mem0p = dmap->config->next_cb(dmap, size);
+ }
+#endif
+
+#if (CH_DBG_ENABLE_ASSERTS != FALSE)
+ osalDbgCheck((dmap != NULL) && (mem0p != NULL) && (periphp != NULL) &&
+ (size > 0U) && ((size == 1U) ||
+ ((dmap->config->op_mode != DMA_CONTINUOUS_HALF_BUFFER) ||
+ (((size & 1U) == 0U)))));
+
+ const DMAConfig *cfg = dmap->config;
+ osalDbgAssert(dmap->state == DMA_READY, "not ready");
+ osalDbgAssert((uint32_t) periphp % cfg->psize == 0,
+ "peripheral address not aligned");
+
+ osalDbgAssert((uint32_t) mem0p % cfg->msize == 0,
+ "memory address not aligned");
+
+# if STM32_DMA_ADVANCED
+ if (cfg->mburst) {
+ osalDbgAssert((size % (cfg->mburst * cfg->msize / cfg->psize)) == 0,
+ "mburst alignment rule not respected");
+ osalDbgAssert((size % (cfg->mburst * cfg->msize)) == 0,
+ "mburst alignment rule not respected");
+ osalDbgAssert((((uint32_t) mem0p) % cfg->mburst) == 0,
+ "memory address alignment rule not respected");
+ }
+ if (cfg->pburst) {
+ osalDbgAssert((size % (cfg->pburst * cfg->psize)) == 0,
+ "pburst alignment rule not respected");
+ osalDbgAssert((((uint32_t) periphp) % cfg->pburst) == 0,
+ "peripheral address alignment rule not respected");
+ }
+ if (cfg->periph_inc_size_4) {
+ osalDbgAssert(cfg->inc_peripheral_addr,
+ "periph_inc_size_4 implies enabling inc_peripheral_addr");
+ osalDbgAssert(cfg->fifo,
+ "periph_inc_size_4 implies enabling fifo");
+ }
+
+# endif // STM32_DMA_ADVANCED
+#endif // CH_DBG_ENABLE_ASSERTS != FALSE
+
+ dma_lld_get_registers(dmap, periphp, mem0p, size, registers);
+}
+#endif
/**
* @brief Stops an ongoing transaction.
@@ -297,7 +433,15 @@ void dmaStopTransfertI(DMADriver *dmap)
dmap->state = DMA_READY;
_dma_reset_i(dmap);
}
+}
+uint8_t dmaGetStreamIndex(DMADriver *dmap)
+{
+ for(uint8_t i = 0; i < 16; i++) {
+ if (dmap->dmastream == STM32_DMA_STREAM(i))
+ return i;
+ }
+ return 0xff;
}
#if (STM32_DMA_USE_WAIT == TRUE) || defined(__DOXYGEN__)
@@ -332,7 +476,7 @@ msg_t dmaTransfertTimeout(DMADriver *dmap, volatile void *periphp, void *mem0p,
osalSysLock();
osalDbgAssert(dmap->thread == NULL, "already waiting");
- osalDbgAssert(dmap->config->circular == false, "blocking API is incompatible with circular mode");
+ osalDbgAssert(dmap->config->op_mode == DMA_ONESHOT, "blocking API is incompatible with circular modes");
dmaStartTransfertI(dmap, periphp, mem0p, size);
msg = osalThreadSuspendTimeoutS(&dmap->thread, timeout);
if (msg != MSG_OK) {
@@ -398,7 +542,6 @@ void dmaReleaseBus(DMADriver *dmap)
# |_____/ |_| |_| \_/ \___| |_|
*/
-
/**
* @brief Configures and activates the DMA peripheral.
*
@@ -406,12 +549,13 @@ void dmaReleaseBus(DMADriver *dmap)
*
* @notapi
*/
-bool dma_lld_start(DMADriver *dmap)
+bool dma_lld_start(DMADriver *dmap, bool allocate_stream)
{
uint32_t psize_msk, msize_msk;
const DMAConfig *cfg = dmap->config;
-
+ osalDbgAssert(PORT_IRQ_IS_VALID_KERNEL_PRIORITY(cfg->irq_priority),
+ "illegal IRQ priority");
switch (cfg->psize) {
case 1 : psize_msk = STM32_DMA_CR_PSIZE_BYTE; break;
case 2 : psize_msk = STM32_DMA_CR_PSIZE_HWORD; break;
@@ -435,14 +579,15 @@ bool dma_lld_start(DMADriver *dmap)
default: osalDbgAssert(false, "direction not set or incorrect");
}
- uint32_t isr_flags = cfg->circular ? 0UL : STM32_DMA_CR_TCIE;
-
- if (cfg->direction != DMA_DIR_M2M) {
- if (cfg->end_cb) {
- isr_flags |= STM32_DMA_CR_TCIE;
- if (cfg->circular) {
- isr_flags |= STM32_DMA_CR_HTIE;
- }
+ uint32_t isr_flags = cfg->op_mode == DMA_ONESHOT ? STM32_DMA_CR_TCIE : 0UL;
+
+ // in M2M mode, half buffer transfert ISR is disabled, but
+ // full buffer transfert complete ISR is enabled
+ if (cfg->end_cb) {
+ isr_flags |= STM32_DMA_CR_TCIE;
+ if ((cfg->direction != DMA_DIR_M2M) &&
+ (cfg->op_mode == DMA_CONTINUOUS_HALF_BUFFER)) {
+ isr_flags |= STM32_DMA_CR_HTIE;
}
}
@@ -464,10 +609,13 @@ bool dma_lld_start(DMADriver *dmap)
#endif
dmap->dmamode = STM32_DMA_CR_PL(cfg->dma_priority) |
- dir_msk | psize_msk | msize_msk | isr_flags |
- (cfg->circular ? STM32_DMA_CR_CIRC : 0UL) |
- (cfg->inc_peripheral_addr ? STM32_DMA_CR_PINC : 0UL) |
- (cfg->inc_memory_addr ? STM32_DMA_CR_MINC : 0UL)
+ dir_msk | psize_msk | msize_msk | isr_flags |
+ (cfg->op_mode == DMA_CONTINUOUS_HALF_BUFFER ? STM32_DMA_CR_CIRC : 0UL) |
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ (cfg->op_mode == DMA_CONTINUOUS_DOUBLE_BUFFER ? STM32_DMA_CR_DBM : 0UL) |
+#endif
+ (cfg->inc_peripheral_addr ? STM32_DMA_CR_PINC : 0UL) |
+ (cfg->inc_memory_addr ? STM32_DMA_CR_MINC : 0UL)
#if STM32_DMA_SUPPORTS_CSELR
| STM32_DMA_CR_CHSEL(cfg->request)
@@ -476,7 +624,7 @@ bool dma_lld_start(DMADriver *dmap)
| STM32_DMA_CR_CHSEL(cfg->channel)
#endif
| (cfg->periph_inc_size_4 ? STM32_DMA_CR_PINCOS : 0UL) |
- (cfg->transfert_end_ctrl_by_periph ? STM32_DMA_CR_PFCTRL : 0UL)
+ (cfg->transfert_end_ctrl_by_periph? STM32_DMA_CR_PFCTRL : 0UL)
# endif
;
@@ -504,7 +652,7 @@ bool dma_lld_start(DMADriver *dmap)
case 1 : fifo_msk = STM32_DMA_FCR_FTH_1Q; break;
case 2 : fifo_msk = STM32_DMA_FCR_FTH_HALF; break;
case 3 : fifo_msk = STM32_DMA_FCR_FTH_3Q; break;
- case 4 : fifo_msk = STM32_DMA_FCR_FTH_FULL; ; break;
+ case 4 : fifo_msk = STM32_DMA_FCR_FTH_FULL; break;
default: osalDbgAssert(false, "fifo threshold should be 1(/4) or 2(/4) or 3(/4) or 4(/4)");
return false;
}
@@ -515,103 +663,108 @@ bool dma_lld_start(DMADriver *dmap)
osalDbgAssert(dmap->config->timeout != 0,
"timeout cannot be 0 if STM32_DMA_USE_ASYNC_TIMOUT is enabled");
osalDbgAssert(!((dmap->config->timeout != TIME_INFINITE) && (dmap->config->fifo != 0)),
- "timeout should be dynamicly disabled (dmap->config->timeout = TIME_INFINITE) "
+ "timeout should be dynamically disabled (dmap->config->timeout = TIME_INFINITE) "
"if STM32_DMA_USE_ASYNC_TIMOUT is enabled and fifo is enabled (fifo != 0)");
# endif
// lot of combination of parameters are forbiden, and some conditions must be meet
- if (!cfg->msize != !cfg->psize) {
- osalDbgAssert(false, "psize and msize should be enabled or disabled together");
+ if (!cfg->mburst != !cfg->pburst) {
+ osalDbgAssert(false, "pburst and mburst should be enabled or disabled together");
return false;
}
- if (cfg->fifo) {
- switch (cfg->msize) {
- case 1: // msize 1
- switch (cfg->mburst) {
- case 4 : // msize 1 mburst 4
- switch (cfg->fifo) {
- case 1: break; // msize 1 mburst 4 fifo 1/4
- case 2: break; // msize 1 mburst 4 fifo 2/4
- case 3: break; // msize 1 mburst 4 fifo 3/4
- case 4: break; // msize 1 mburst 4 fifo 4/4
- }
- break;
- case 8 : // msize 1 mburst 8
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 1 mburst 8 fifo 1/4
- case 2: break; // msize 1 mburst 8 fifo 2/4
- case 3: goto forbiddenCombination; // msize 1 mburst 8 fifo 3/4
- case 4: break; // msize 1 mburst 8 fifo 4/4
- }
- break;
- case 16 : // msize 1 mburst 16
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 1 mburst 16 fifo 1/4
- case 2: goto forbiddenCombination; // msize 1 mburst 16 fifo 2/4
- case 3: goto forbiddenCombination; // msize 1 mburst 16 fifo 3/4
- case 4: break; // msize 1 mburst 16 fifo 4/4
- }
- break;
- }
- break;
- case 2: // msize 2
- switch (cfg->mburst) {
- case 4 : // msize 2 mburst 4
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 2 mburst 4 fifo 1/4
- case 2: break; // msize 2 mburst 4 fifo 2/4
- case 3: goto forbiddenCombination; // msize 2 mburst 4 fifo 3/4
- case 4: break; // msize 2 mburst 4 fifo 4/4
- }
- break;
- case 8 :
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 2 mburst 8 fifo 1/4
- case 2: goto forbiddenCombination; // msize 2 mburst 8 fifo 2/4
- case 3: goto forbiddenCombination; // msize 2 mburst 8 fifo 3/4
- case 4: break; // msize 2 mburst 8 fifo 4/4
- }
- break;
- case 16 :
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 2 mburst 16 fifo 1/4
- case 2: goto forbiddenCombination; // msize 2 mburst 16 fifo 2/4
- case 3: goto forbiddenCombination; // msize 2 mburst 16 fifo 3/4
- case 4: goto forbiddenCombination; // msize 2 mburst 16 fifo 4/4
- }
- }
- break;
- case 4:
- switch (cfg->mburst) {
- case 4 :
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 4 mburst 4 fifo 1/4
- case 2: goto forbiddenCombination; // msize 4 mburst 4 fifo 2/4
- case 3: goto forbiddenCombination; // msize 4 mburst 4 fifo 3/4
- case 4: break; // msize 4 mburst 4 fifo 4/4
- }
- break;
- case 8 :
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 4 mburst 8 fifo 1/4
- case 2: goto forbiddenCombination; // msize 4 mburst 8 fifo 2/4
- case 3: goto forbiddenCombination; // msize 4 mburst 8 fifo 3/4
- case 4: goto forbiddenCombination; // msize 4 mburst 8 fifo 4/4
- }
- break;
- case 16 :
- switch (cfg->fifo) {
- case 1: goto forbiddenCombination; // msize 4 mburst 16 fifo 1/4
- case 2: goto forbiddenCombination; // msize 4 mburst 16 fifo 2/4
- case 3: goto forbiddenCombination; // msize 4 mburst 16 fifo 3/4
- case 4: goto forbiddenCombination; // msize 4 mburst 16 fifo 4/4
- }
- }
- }
+ // from RM0090, Table 48. FIFO threshold configurations
+ if (cfg->fifo && cfg->mburst) {
+ const size_t fifo_level = cfg->fifo * 4U;
+ osalDbgAssert(fifo_level % (cfg->mburst * cfg->msize) == 0, "threshold combination forbidden");
}
+ // if (cfg->fifo) {
+ // switch (cfg->msize) {
+ // case 1: // msize 1
+ // switch (cfg->mburst) {
+ // case 4 : // msize 1 mburst 4
+ // switch (cfg->fifo) {
+ // case 1: break; // msize 1 mburst 4 fifo 1/4
+ // case 2: break; // msize 1 mburst 4 fifo 2/4
+ // case 3: break; // msize 1 mburst 4 fifo 3/4
+ // case 4: break; // msize 1 mburst 4 fifo 4/4
+ // }
+ // break;
+ // case 8 : // msize 1 mburst 8
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 1 mburst 8 fifo 1/4
+ // case 2: break; // msize 1 mburst 8 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 1 mburst 8 fifo 3/4
+ // case 4: break; // msize 1 mburst 8 fifo 4/4
+ // }
+ // break;
+ // case 16 : // msize 1 mburst 16
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 1 mburst 16 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 1 mburst 16 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 1 mburst 16 fifo 3/4
+ // case 4: break; // msize 1 mburst 16 fifo 4/4
+ // }
+ // break;
+ // }
+ // break;
+ // case 2: // msize 2
+ // switch (cfg->mburst) {
+ // case 4 : // msize 2 mburst 4
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 2 mburst 4 fifo 1/4
+ // case 2: break; // msize 2 mburst 4 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 2 mburst 4 fifo 3/4
+ // case 4: break; // msize 2 mburst 4 fifo 4/4
+ // }
+ // break;
+ // case 8 :
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 2 mburst 8 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 2 mburst 8 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 2 mburst 8 fifo 3/4
+ // case 4: break; // msize 2 mburst 8 fifo 4/4
+ // }
+ // break;
+ // case 16 :
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 2 mburst 16 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 2 mburst 16 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 2 mburst 16 fifo 3/4
+ // case 4: goto forbiddenCombination; // msize 2 mburst 16 fifo 4/4
+ // }
+ // }
+ // break;
+ // case 4:
+ // switch (cfg->mburst) {
+ // case 4 :
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 4 mburst 4 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 4 mburst 4 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 4 mburst 4 fifo 3/4
+ // case 4: break; // msize 4 mburst 4 fifo 4/4
+ // }
+ // break;
+ // case 8 :
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 4 mburst 8 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 4 mburst 8 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 4 mburst 8 fifo 3/4
+ // case 4: goto forbiddenCombination; // msize 4 mburst 8 fifo 4/4
+ // }
+ // break;
+ // case 16 :
+ // switch (cfg->fifo) {
+ // case 1: goto forbiddenCombination; // msize 4 mburst 16 fifo 1/4
+ // case 2: goto forbiddenCombination; // msize 4 mburst 16 fifo 2/4
+ // case 3: goto forbiddenCombination; // msize 4 mburst 16 fifo 3/4
+ // case 4: goto forbiddenCombination; // msize 4 mburst 16 fifo 4/4
+ // }
+ // }
+ // }
+ // }
# endif
dmap->dmamode |= (pburst_msk | mburst_msk);
@@ -625,8 +778,7 @@ bool dma_lld_start(DMADriver *dmap)
avoid permanent underrun or overrun conditions, depending on the DMA stream direction:
If (PBURST × PSIZE) = FIFO_SIZE (4 words), FIFO_Threshold = 3/4 is forbidden
*/
-
- if (((cfg->pburst * cfg->psize) == STM32_DMA_FIFO_SIZE) && (cfg->fifo == 3)) {
+ if ( ((cfg->pburst * cfg->psize) == STM32_DMA_FIFO_SIZE) && (cfg->fifo == 3)) {
goto forbiddenCombination;
}
@@ -634,15 +786,12 @@ bool dma_lld_start(DMADriver *dmap)
When memory-to-memory mode is used, the Circular and direct modes are not allowed.
Only the DMA2 controller is able to perform memory-to-memory transfers.
*/
-
#if STM32_DMA_SUPPORTS_DMAMUX == 0
if (cfg->direction == DMA_DIR_M2M) {
osalDbgAssert(dmap->controller == 2, "M2M not available on DMA1");
- osalDbgAssert(cfg->circular == false, "M2M not available in circular mode");
}
#endif
-
# endif
#endif
@@ -650,30 +799,30 @@ bool dma_lld_start(DMADriver *dmap)
if (cfg->fifo) {
dmap->fifomode = STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FEIE | fifo_msk;
} else {
- osalDbgAssert(cfg->direction != DMA_DIR_M2M, "fifo mode mandatory for M2M");
osalDbgAssert(cfg->psize == cfg->msize,
"msize == psize is mandatory when fifo is disabled");
dmap->fifomode = 0U;
}
#endif
+ if (allocate_stream == true) {
#if CH_KERNEL_MAJOR < 6
- const bool error = dmaStreamAllocate( dmap->dmastream,
- cfg->irq_priority,
- (stm32_dmaisr_t) &dma_lld_serve_interrupt,
- (void *) dmap );
+ const bool error = dmaStreamAllocate( dmap->dmastream,
+ cfg->irq_priority,
+ (stm32_dmaisr_t) &dma_lld_serve_interrupt,
+ (void *) dmap );
#else
- dmap->dmastream = dmaStreamAllocI(dmap->config->stream,
- cfg->irq_priority,
- (stm32_dmaisr_t) &dma_lld_serve_interrupt,
- (void *) dmap );
- bool error = dmap->dmastream == NULL;
-#endif
- if (error) {
- osalDbgAssert(false, "stream already allocated");
- return false;
+ dmap->dmastream = dmaStreamAllocI(dmap->config->stream,
+ cfg->irq_priority,
+ (stm32_dmaisr_t) &dma_lld_serve_interrupt,
+ (void *) dmap );
+ const bool error = dmap->dmastream == NULL;
+#endif
+ if (error) {
+ osalDbgAssert(false, "stream already allocated");
+ return false;
+ }
}
-
return true;
#if (CH_DBG_ENABLE_ASSERTS != FALSE)
@@ -686,7 +835,58 @@ bool dma_lld_start(DMADriver *dmap)
#endif
}
+static inline size_t getCrossCacheBoundaryAwareSize(const void *memp,
+ const size_t dsize)
+{
+ // L1 cache on F7 and H7 is organised of line of 32 bytes
+ // returned size is not 32 bytes aligned by a mask operation
+ // because cache management does internal mask and this operation
+ // would be useless
+
+#if defined CACHE_LINE_SIZE && CACHE_LINE_SIZE != 0
+ const uint32_t endp = ((uint32_t) memp % CACHE_LINE_SIZE +
+ dsize % CACHE_LINE_SIZE );
+ return endp < CACHE_LINE_SIZE ? dsize + CACHE_LINE_SIZE :
+ dsize + CACHE_LINE_SIZE *2U;
+#else
+ (void) memp;
+ return dsize;
+#endif
+}
+/**
+ * @brief Copy the register of a ready stream
+ *
+ * @param[in] dmap pointer to the @p DMADriver object
+ * @note : main use is preparing link list of transactions for mdma use
+ * @notapi
+ */
+#ifndef DMA_request_TypeDef
+void dma_lld_get_registers(DMADriver *dmap, volatile void *periphp,
+ void *mem0p, const size_t size,
+ DMA_Stream_TypeDef *registers)
+{
+ dmaStreamSetPeripheral(dmap->dmastream, periphp);
+#if STM32_DMA_SUPPORTS_DMAMUX
+ dmaSetRequestSource(dmap->dmastream, dmap->config->dmamux);
+#endif
+
+ dmaStreamSetMemory0(dmap->dmastream, mem0p);
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ if (dmap->config->op_mode == DMA_CONTINUOUS_DOUBLE_BUFFER) {
+ dmaStreamSetMemory1(dmap->dmastream, dmap->config->next_cb(dmap, size));
+ }
+#endif
+ dmaStreamSetTransactionSize(dmap->dmastream, size);
+ dmaStreamSetMode(dmap->dmastream, dmap->dmamode);
+#if STM32_DMA_ADVANCED
+ dmaStreamSetFIFO(dmap->dmastream, dmap->fifomode);
+#endif
+
+ memcpy(registers, dmap->dmastream->stream, sizeof(DMA_Stream_TypeDef));
+ registers->CR |= STM32_DMA_CR_EN;
+}
+#endif
/**
* @brief Starts a DMA transaction.
*
@@ -696,12 +896,12 @@ bool dma_lld_start(DMADriver *dmap)
*/
bool dma_lld_start_transfert(DMADriver *dmap, volatile void *periphp, void *mem0p, const size_t size)
{
- const DMAConfig *cfg = dmap->config;
- osalDbgAssert(PORT_IRQ_IS_VALID_KERNEL_PRIORITY(cfg->irq_priority), "illegal IRQ priority");
#if __DCACHE_PRESENT
- if (dmap->config->dcache_memory_in_use &&
+ if (dmap->config->activate_dcache_sync &&
(dmap->config->direction != DMA_DIR_P2M)) {
- cacheBufferFlush(mem0p, size * dmap->config->msize);
+ const size_t cacheSize = getCrossCacheBoundaryAwareSize(mem0p,
+ size * dmap->config->msize);
+ cacheBufferFlush(mem0p, cacheSize);
}
#endif
dmap->mem0p = mem0p;
@@ -713,7 +913,13 @@ bool dma_lld_start_transfert(DMADriver *dmap, volatile void *periphp, void *mem0
#if STM32_DMA_SUPPORTS_DMAMUX
dmaSetRequestSource(dmap->dmastream, dmap->config->dmamux);
#endif
+
dmaStreamSetMemory0(dmap->dmastream, mem0p);
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ if (dmap->config->op_mode == DMA_CONTINUOUS_DOUBLE_BUFFER) {
+ dmaStreamSetMemory1(dmap->dmastream, dmap->config->next_cb(dmap, size));
+ }
+#endif
dmaStreamSetTransactionSize(dmap->dmastream, size);
dmaStreamSetMode(dmap->dmastream, dmap->dmamode);
#if STM32_DMA_ADVANCED
@@ -767,21 +973,29 @@ static void dma_lld_serve_interrupt(DMADriver *dmap, uint32_t flags)
{
/* DMA errors handling.*/
-#if CH_DBG_SYSTEM_STATE_CHECK
+#if CH_DBG_SYSTEM_STATE_CHECK && STM32_DMA_ADVANCED
const uint32_t feif_msk = dmap->config->fifo != 0U ? STM32_DMA_ISR_FEIF : 0U;
#else
- static const uint32_t feif_msk = 0U;
+ const uint32_t feif_msk = 0U;
#endif
//const uint32_t feif_msk = STM32_DMA_ISR_FEIF;
if ((flags & (STM32_DMA_ISR_TEIF | STM32_DMA_ISR_DMEIF | feif_msk)) != 0U) {
/* DMA, this could help only if the DMA tries to access an unmapped
address space or violates alignment rules.*/
- const dmaerrormask_t err =
+ dmaerrormask_t err =
( (flags & STM32_DMA_ISR_TEIF) ? DMA_ERR_TRANSFER_ERROR : 0UL) |
- ( (flags & STM32_DMA_ISR_DMEIF) ? DMA_ERR_DIRECTMODE_ERROR : 0UL) |
- ( (flags & feif_msk) ? DMA_ERR_FIFO_ERROR : 0UL) |
- ( getFCR_FS(dmap) == (0b100 << DMA_SxFCR_FS_Pos) ? DMA_ERR_FIFO_EMPTY: 0UL) |
- ( getFCR_FS(dmap) == (0b101 << DMA_SxFCR_FS_Pos) ? DMA_ERR_FIFO_FULL: 0UL);
+ ( (flags & STM32_DMA_ISR_DMEIF) ? DMA_ERR_DIRECTMODE_ERROR : 0UL);
+
+ if (dmap->config->fifo != 0U) {
+#if STM32_DMA_ADVANCED
+ dmaerrormask_t fserr =
+ ( (flags & feif_msk) ? DMA_ERR_FIFO_ERROR : 0UL) |
+ ( getFCR_FS(dmap) == (0b100 << DMA_SxFCR_FS_Pos) ? DMA_ERR_FIFO_EMPTY: 0UL) |
+ ( getFCR_FS(dmap) == (0b101 << DMA_SxFCR_FS_Pos) ? DMA_ERR_FIFO_FULL: 0UL) ;
+ err |= fserr;
+#endif
+ }
+
_dma_isr_error_code(dmap, err);
} else {
@@ -789,16 +1003,23 @@ static void dma_lld_serve_interrupt(DMADriver *dmap, uint32_t flags)
DMA error handler, in this case this interrupt is spurious.*/
if (dmap->state == DMA_ACTIVE) {
#if __DCACHE_PRESENT
- if (dmap->config->dcache_memory_in_use)
+ if (dmap->config->activate_dcache_sync)
switch (dmap->config->direction) {
case DMA_DIR_M2P : break;
- case DMA_DIR_P2M :
- cacheBufferInvalidate(dmap->mem0p,
- dmap->size * dmap->config->msize);
+ case DMA_DIR_P2M : if (dmap->mem0p >= (void *) 0x20000000) {
+ const size_t cacheSize =
+ getCrossCacheBoundaryAwareSize(dmap->mem0p, dmap->size *
+ dmap->config->msize);
+ cacheBufferInvalidate(dmap->mem0p, cacheSize);
+ }
break;
- case DMA_DIR_M2M :
- cacheBufferInvalidate(dmap->periphp,
- dmap->size * dmap->config->msize);
+
+ case DMA_DIR_M2M : if (dmap->periphp >= (void *) 0x20000000) {
+ const size_t cacheSize =
+ getCrossCacheBoundaryAwareSize((void *) dmap->periphp,
+ dmap->size * dmap->config->psize);
+ cacheBufferInvalidate(dmap->periphp, cacheSize);
+ }
break;
}
#endif
@@ -823,7 +1044,7 @@ static void dma_lld_serve_interrupt(DMADriver *dmap, uint32_t flags)
void dma_lld_serve_timeout_interrupt(void *arg)
{
DMADriver *dmap = (DMADriver *) arg;
- if (dmap->config->circular) {
+ if (dmap->config->op_mode != DMA_ONESHOT) {
chSysLockFromISR();
chVTSetI(&dmap->vt, dmap->config->timeout,
&dma_lld_serve_timeout_interrupt, (void *) dmap);
@@ -832,3 +1053,31 @@ void dma_lld_serve_timeout_interrupt(void *arg)
async_timout_enabled_call_end_cb(dmap, FROM_TIMOUT_CODE);
}
#endif
+
+#if STM32_DMA_USE_DOUBLE_BUFFER
+/**
+ * @brief Common ISR code, switch memory pointer in double buffer mode
+ * @note This macro is meant to be used in the low level drivers
+ * implementation only. This function must be called as soon as
+ * DMA has switched buffer.
+ *
+ * @param[in] dmap pointer to the @p DMADriver object
+ * @param[in] nextBuffer pointer to a buffer that is set in MEM0xP or MEM1xP
+ * the one which is not occupied beeing filled
+ * by DMA.
+ * @notapi
+ */
+void* dma_lld_set_next_double_buffer(DMADriver *dmap, void *nextBuffer)
+{
+ void *lastBuffer;
+
+ if (dmaStreamGetCurrentTarget(dmap->dmastream)) {
+ lastBuffer = (void *) dmap->dmastream->stream->M0AR;
+ dmap->dmastream->stream->M0AR = (uint32_t) nextBuffer;
+ } else {
+ lastBuffer = (void *) dmap->dmastream->stream->M1AR;
+ dmap->dmastream->stream->M1AR = (uint32_t) nextBuffer;
+ }
+ return lastBuffer;
+}
+#endif
diff --git a/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.h b/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.h
index b18075ed327..708e287bea5 100644
--- a/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.h
+++ b/sw/airborne/arch/chibios/mcu_periph/hal_stm32_dma.h
@@ -55,6 +55,17 @@ extern "C" {
#define STM32_DMA_SUPPORTS_CSELR FALSE
#endif
+#if !defined(STM32_DMA_SUPPORTS_DMAMUX) || defined(__DOXYGEN__)
+#define STM32_DMA_SUPPORTS_DMAMUX FALSE
+#endif
+
+#if !defined(STM32_DMA_DUMMY_MEMORY_AREA_ADDRESS) || defined(__DOXYGEN__)
+#define STM32_DMA_DUMMY_MEMORY_AREA_ADDRESS 0x80000000
+#endif
+
+#if !defined(__DCACHE_PRESENT)
+#define __DCACHE_PRESENT FALSE
+#endif
/**
* @brief Driver state machine possible states.
@@ -90,6 +101,17 @@ typedef enum {
DMA_DIR_M2M = 3, /**< MEMORY to MEMORY */
} dmadirection_t;
+ /**
+ * @brief DMA transfert memory mode
+ */
+typedef enum {
+ DMA_ONESHOT = 1, /**< One transert then stop */
+ DMA_CONTINUOUS_HALF_BUFFER, /**< Continuous mode to/from the same buffer */
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ DMA_CONTINUOUS_DOUBLE_BUFFER /**< Continuous mode to/from differents buffers */
+#endif
+} dmaopmode_t;
+
/**
* @brief Type of a structure representing an DMA driver.
*/
@@ -100,11 +122,21 @@ typedef struct DMADriver DMADriver;
*
* @param[in] dmap pointer to the @p DMADriver object triggering the
* callback
- * @param[in] buffer pointer to the most recent samples data
+ * @param[in] buffer pointer to the most recent dma data
* @param[in] n number of buffer rows available starting from @p buffer
*/
typedef void (*dmacallback_t)(DMADriver *dmap, void *buffer, const size_t n);
+/**
+ * @brief DMA next buffer query callback type.
+ *
+ * @param[in] dmap pointer to the @p DMADriver object triggering the
+ * callback
+ * @param[in] n number of buffer rows needed in the returned buffer pointer
+ * @return pointer to the next to be used dma buffer
+ */
+typedef void * (*dmanextcallback_t)(DMADriver *dmap, const size_t n);
+
/**
* @brief DMA error callback type.
@@ -246,7 +278,7 @@ typedef struct {
uint32_t stream;
#if STM32_DMA_SUPPORTS_DMAMUX
- uint8_t dmamux;
+ uint32_t dmamux; // 4 bytes wide for mdma use
#else
#if STM32_DMA_SUPPORTS_CSELR
/**
@@ -275,9 +307,9 @@ typedef struct {
/**
- * @brief Enables the circular buffer mode for the stream.
+ * @brief one shot, or circular half buffer, or circular double buffers
*/
- bool circular;
+ dmaopmode_t op_mode;
/**
@@ -285,6 +317,14 @@ typedef struct {
*/
dmacallback_t end_cb;
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ /**
+ * @brief Next data buffer callback function associated to the stream or @p NULL.
+ * @note Mandatory in the DMA_CONTINUOUS_DOUBLE_BUFFER mode
+ */
+ dmanextcallback_t next_cb;
+#endif
+
/**
* @brief Error callback or @p NULL.
*/
@@ -327,7 +367,10 @@ typedef struct {
/**
* @brief DMA memory is in a cached section and beed to be flushed
*/
-bool dcache_memory_in_use;
+ union {
+ bool dcache_memory_in_use; // this name was hardly meaningfull
+ bool activate_dcache_sync;
+ };
#endif
#if STM32_DMA_ADVANCED
#define STM32_DMA_FIFO_SIZE 16 // hardware specification for dma V2
@@ -357,7 +400,7 @@ bool dcache_memory_in_use;
*/
bool transfert_end_ctrl_by_periph; // PFCTRL bit
#endif
-#ifdef STM32_DMA_DRIVER_USER_DATA_FIELD
+#if STM32_DMA_DRIVER_USER_DATA_FIELD
void *user_data;
#endif
} DMAConfig;
@@ -431,6 +474,10 @@ struct DMADriver {
*/
size_t size;
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ volatile uint32_t next_cb_errors;
+#endif
+
#if CH_DBG_SYSTEM_STATE_CHECK
volatile size_t nbTransferError;
volatile size_t nbDirectModeError;
@@ -457,6 +504,7 @@ struct DMADriver {
void dmaObjectInit(DMADriver *dmap);
bool dmaStart(DMADriver *dmap, const DMAConfig *cfg);
+bool dmaReloadConf(DMADriver *dmap, const DMAConfig *cfg);
void dmaStop(DMADriver *dmap);
#if STM32_DMA_USE_WAIT == TRUE
@@ -477,12 +525,38 @@ void dmaStopTransfert(DMADriver *dmap);
bool dmaStartTransfertI(DMADriver *dmap, volatile void *periphp, void *mem0p, const size_t size);
void dmaStopTransfertI(DMADriver *dmap);
-
+uint8_t dmaGetStreamIndex(DMADriver *dmap);
+#ifndef DMA_request_TypeDef
+void dmaGetRegisters(DMADriver *dmap, volatile void *periphp, void *mem0p,
+ const size_t size,
+ DMA_Stream_TypeDef *registers);
+#endif
static inline dmastate_t dmaGetState(DMADriver *dmap) {return dmap->state;}
+static inline size_t dmaGetTransactionCounter(DMADriver *dmap) {return dmaStreamGetTransactionSize(dmap->dmastream);}
+
+#if STM32_DMA_USE_DOUBLE_BUFFER
+/**
+ * @brief get double buffer allocation errors counter
+ *
+ * @param[in] dmap pointer to the @p DMADriver object triggering the
+ * callback
+ * @return the number of allocation error since the last call to
+ * dmaClearNextErrors
+ * @note there is allocation error when nect_cb callback return NULL pointer
+ */
+static inline dmastate_t dmaGetNextErrors(DMADriver *dmap) {return dmap->next_cb_errors;}
+/**
+ * @brief clear double buffer allocation errors counter
+ *
+ * @param[in] dmap pointer to the @p DMADriver object triggering the
+ * callback
+ */
+static inline void dmaClearNextErrors(DMADriver *dmap) {dmap->next_cb_errors = 0U;}
+#endif
// low level driver
-bool dma_lld_start(DMADriver *dmap);
+bool dma_lld_start(DMADriver *dmap, bool allocate_stream);
void dma_lld_stop(DMADriver *dmap);
@@ -495,6 +569,8 @@ void dma_lld_stop_transfert(DMADriver *dmap);
void dma_lld_serve_timeout_interrupt(void *arg);
#endif
+void* dma_lld_set_next_double_buffer(DMADriver *dmap, void *nextBuffer);
+
#if STM32_DMA_USE_ASYNC_TIMOUT
typedef enum {FROM_TIMOUT_CODE, FROM_HALF_CODE, FROM_FULL_CODE, FROM_NON_CIRCULAR_CODE} CbCallContext;
static inline void async_timout_enabled_call_end_cb(DMADriver *dmap, const CbCallContext context)
@@ -562,7 +638,7 @@ static inline void _dma_isr_half_code(DMADriver *dmap)
static inline void _dma_isr_full_code(DMADriver *dmap)
{
- if (dmap->config->circular) {
+ if (dmap->config->op_mode == DMA_CONTINUOUS_HALF_BUFFER) {
#if STM32_DMA_USE_ASYNC_TIMOUT
if (dmap->config->timeout != TIME_INFINITE) {
chSysLockFromISR();
@@ -586,7 +662,8 @@ static inline void _dma_isr_full_code(DMADriver *dmap)
}
}
#endif
- } else { // not circular
+ }
+ else if (dmap->config->op_mode == DMA_ONESHOT) { // not circular
/* End transfert.*/
#if STM32_DMA_USE_ASYNC_TIMOUT
if (dmap->config->timeout != TIME_INFINITE) {
@@ -612,6 +689,21 @@ static inline void _dma_isr_full_code(DMADriver *dmap)
}
_dma_wakeup_isr(dmap);
}
+#if STM32_DMA_USE_DOUBLE_BUFFER
+ else { // CONTINUOUS_DOUBLE_BUFFER
+ /* Next buffer handling */
+ void* const rawNextBuff = dmap->config->next_cb(dmap, dmap->size);
+ if (rawNextBuff == NULL)
+ dmap->next_cb_errors++;
+ void* const nextBuff = rawNextBuff ? rawNextBuff : (void *) STM32_DMA_DUMMY_MEMORY_AREA_ADDRESS;
+ void* const memXp = dma_lld_set_next_double_buffer(dmap, nextBuff);
+ /* Callback handling.*/
+ if ((dmap->config->end_cb != NULL) &&
+ (memXp != (void *) STM32_DMA_DUMMY_MEMORY_AREA_ADDRESS)){
+ dmap->config->end_cb(dmap, memXp, dmap->size);
+ }
+ }
+#endif
}
static inline void _dma_isr_error_code(DMADriver *dmap, dmaerrormask_t err) {
@@ -644,8 +736,11 @@ static inline void _dma_isr_error_code(DMADriver *dmap, dmaerrormask_t err) {
}
_dma_timeout_isr(dmap);
}
-
-
+#ifndef DMA_request_TypeDef
+void dma_lld_get_registers(DMADriver *dmap, volatile void *periphp,
+ void *mem0p, const size_t size,
+ DMA_Stream_TypeDef *registers);
+#endif
#ifdef __cplusplus
}
#endif
diff --git a/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.c b/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.c
new file mode 100644
index 00000000000..600b9b70a50
--- /dev/null
+++ b/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.c
@@ -0,0 +1,817 @@
+#include "mcu_periph/input_capture_arch.h"
+
+
+enum TimICChannel {TIMIC_CH1=1<<0, TIMIC_CH2=1<<1, TIMIC_CH3=1<<2, TIMIC_CH4=1<<3};
+
+static const TimICDriver* driverByTimerIndex[6] = {NULL};
+
+
+static void input_capture_lld_serve_interrupt(const TimICDriver * const timicp) __attribute__((unused));
+static void _input_capture_isr_invoke_capture_cb(const TimICDriver * const timicp, uint32_t channel);
+static void _input_capture_isr_invoke_overflow_cb(const TimICDriver * const timicp);
+
+void timIcObjectInit(TimICDriver *timicp)
+{
+ timicp->config = NULL;
+ timicp->state = TIMIC_STOP;
+ timicp->dier = 0;
+}
+
+void timIcStart(TimICDriver *timicp, const TimICConfig *configp)
+{
+ osalDbgCheck((configp != NULL) && (timicp != NULL));
+ osalDbgAssert((configp->prescaler >= 1) &&
+ (configp->prescaler <= 65536),
+ "prescaler must be 1 .. 65536");
+ osalDbgAssert(timicp->state == TIMIC_STOP, "state error");
+ timicp->config = configp;
+ stm32_tim_t * const timer = timicp->config->timer;
+ chMtxObjectInit(&timicp->mut);
+ timIcRccEnable(timicp);
+ timicp->channel = 0;
+ if (timicp->config->active & (CH1_RISING_EDGE | CH1_FALLING_EDGE | CH1_BOTH_EDGES))
+ timicp->channel |= TIMIC_CH1;
+ if (timicp->config->active & (CH2_RISING_EDGE | CH2_FALLING_EDGE | CH2_BOTH_EDGES))
+ timicp->channel |= TIMIC_CH2;
+ if (timicp->config->active & (CH3_RISING_EDGE | CH3_FALLING_EDGE | CH3_BOTH_EDGES))
+ timicp->channel |= TIMIC_CH3;
+ if (timicp->config->active & (CH4_RISING_EDGE | CH4_FALLING_EDGE | CH4_BOTH_EDGES))
+ timicp->channel |= TIMIC_CH4;
+
+ timer->CR1 = 0; // disable timer
+
+ // hack in case of timer with more fields
+#if defined (STM32G0XX) || defined (STM32G4XX)|| defined (STM32H7XX)
+
+ TIM_TypeDef *cmsisTimer = (TIM_TypeDef *) timer;
+ cmsisTimer->CCMR3 = cmsisTimer->AF1 = cmsisTimer->AF2 =
+ cmsisTimer->TISEL = 0;
+#endif
+
+ timer->PSC = configp->prescaler - 1U; // prescaler
+ timer->ARR = configp->arr ? configp->arr : 0xffffffff;
+ timer->DCR = configp->dcr;
+ if (timicp->config->mode == TIMIC_PWM_IN) {
+ chDbgAssert(__builtin_popcount(timicp->channel) == 1, "In pwm mode, only one channel must be set");
+ chDbgAssert((timicp->config->capture_cb == NULL) && (timicp->config->overflow_cb == NULL),
+ "In pwm mode, callback are not implemented, use PWMDriver instead");
+ switch (timicp->channel) {
+ case TIMIC_CH1:
+ timer->CCMR1 = (0b01 << TIM_CCMR1_CC1S_Pos) | (0b10 << TIM_CCMR1_CC2S_Pos);
+ timer->CCMR2 = 0U;
+ timer->CCER = TIM_CCER_CC2P; /* CC1P et CC1NP = 0 */
+ timer->SMCR = (0b101 << TIM_SMCR_TS_Pos) | (0b100 << TIM_SMCR_SMS_Pos);
+ timer->CCER |= (TIM_CCER_CC1E | TIM_CCER_CC2E);
+ break;
+ case TIMIC_CH2:
+ timer->CCMR1 = (0b10 << TIM_CCMR1_CC1S_Pos) | (0b01 << TIM_CCMR1_CC2S_Pos);
+ timer->CCMR2 = 0U;
+ timer->CCER = TIM_CCER_CC1P; /* CC2P et CC2NP = 0 */
+ timer->SMCR = (0b110 << TIM_SMCR_TS_Pos) | (0b100 << TIM_SMCR_SMS_Pos);
+ timer->CCER |= (TIM_CCER_CC1E | TIM_CCER_CC2E);
+ break;
+ case TIMIC_CH3:
+ timer->CCMR2 = (0b01 << TIM_CCMR2_CC3S_Pos) | (0b10 << TIM_CCMR2_CC4S_Pos);
+ timer->CCMR1 = 0U;
+ timer->CCER = TIM_CCER_CC4P; /* CC3P et CC3NP = 0 */
+ timer->SMCR = (0b101 << TIM_SMCR_TS_Pos) | (0b100 << TIM_SMCR_SMS_Pos);
+ timer->CCER |= (TIM_CCER_CC3E | TIM_CCER_CC4E);
+ break;
+ case TIMIC_CH4:
+ timer->CCMR2 = (0b10 << TIM_CCMR2_CC3S_Pos) | (0b01 << TIM_CCMR2_CC4S_Pos);
+ timer->CCMR1 = 0U;
+ timer->CCER = TIM_CCER_CC3P; /* CC4P et CC4NP = 0 */
+ timer->SMCR = (0b110 << TIM_SMCR_TS_Pos) | (0b100 << TIM_SMCR_SMS_Pos);
+ timer->CCER |= (TIM_CCER_CC3E | TIM_CCER_CC4E);
+ break;
+ default:
+ chSysHalt("channel must be TIMIC_CH1 .. TIMIC_CH4");
+ }
+ } else if (timicp->config->mode == TIMIC_INPUT_CAPTURE) {
+ /*
+ Select the active input: TIMx_CCR1 must be linked to the TI1 input, so write the CC1S
+ bits to 01 in the TIMx_CCMR1 register. As soon as CC1S becomes different from 00,
+ the channel is configured in input and the TIMx_CCR1 register becomes read-only.
+ •
+ Select the edge of the active transition on the TI1 channel by writing CC1P and CC1NP
+ bits to 0 in the TIMx_CCER register (rising edge in this case).
+ •
+ •
+ Enable capture from the counter into the capture register by setting the CC1E bit in the
+ TIMx_CCER register.
+ •
+ If needed, enable the related interrupt request by setting the CC1IE bit in the
+ TIMx_DIER register, and/or the DMA request by setting the CC1DE bit in the
+ TIMx_DIER register.
+ */
+
+
+ timer->CCMR1 = 0;
+ timer->CCMR2 = 0;
+ timer->CCER = 0;
+ timer->SMCR = 0;
+ timer->CCER = 0;
+
+ if (timicp->channel & TIMIC_CH1) {
+ switch (timicp->config->active & (CH1_RISING_EDGE | CH1_FALLING_EDGE | CH1_BOTH_EDGES)) {
+ case CH1_RISING_EDGE:
+ timer->CCER |= 0;
+ break;
+ case CH1_FALLING_EDGE:
+ timer->CCER |= TIM_CCER_CC1P;
+ break;
+ case CH1_BOTH_EDGES:
+ timer->CCER |= (TIM_CCER_CC1P | TIM_CCER_CC1NP);
+ break;
+ default:
+ chSysHalt("No configuration given for CH1");
+ }
+ timer->CCMR1 |= (0b01 << TIM_CCMR1_CC1S_Pos);
+ timer->CCER |= TIM_CCER_CC1E;
+ timicp->dier |= STM32_TIM_DIER_CC1IE;
+ }
+ if (timicp->channel & TIMIC_CH2) {
+ switch (timicp->config->active & (CH2_RISING_EDGE | CH2_FALLING_EDGE | CH2_BOTH_EDGES)) {
+ case CH2_RISING_EDGE:
+ timer->CCER |= 0;
+ break;
+ case CH2_FALLING_EDGE:
+ timer->CCER |= TIM_CCER_CC2P;
+ break;
+ case CH2_BOTH_EDGES:
+ timer->CCER |= (TIM_CCER_CC2P | TIM_CCER_CC2NP);
+ break;
+ default:
+ chSysHalt("No configuration given for CH2");
+ }
+ timer->CCMR1 |= (0b01 << TIM_CCMR1_CC2S_Pos);
+ timer->CCER |= TIM_CCER_CC2E;
+ timicp->dier |= STM32_TIM_DIER_CC2IE;
+ }
+ if (timicp->channel & TIMIC_CH3) {
+ switch (timicp->config->active & (CH3_RISING_EDGE | CH3_FALLING_EDGE | CH3_BOTH_EDGES)) {
+ case CH3_RISING_EDGE:
+ timer->CCER |= 0;
+ break;
+ case CH3_FALLING_EDGE:
+ timer->CCER |= TIM_CCER_CC3P;
+ break;
+ case CH3_BOTH_EDGES:
+ timer->CCER |= (TIM_CCER_CC3P | TIM_CCER_CC3NP);
+ break;
+ default:
+ chSysHalt("No configuration given for CH3");
+ }
+ timer->CCMR2 |= (0b01 << TIM_CCMR2_CC3S_Pos);
+ timer->CCER |= TIM_CCER_CC3E;
+ timicp->dier |= STM32_TIM_DIER_CC3IE;
+ }
+ if (timicp->channel & TIMIC_CH4) {
+ switch (timicp->config->active & (CH4_RISING_EDGE | CH4_FALLING_EDGE | CH4_BOTH_EDGES)) {
+ case CH4_RISING_EDGE:
+ timer->CCER |= 0;
+ break;
+ case CH4_FALLING_EDGE:
+ timer->CCER |= TIM_CCER_CC4P;
+ break;
+ case CH4_BOTH_EDGES:
+ timer->CCER |= (TIM_CCER_CC4P | TIM_CCER_CC4NP);
+ break;
+ default:
+ chSysHalt("No configuration given for CH4");
+ }
+ timer->CCMR2 |= (0b01 << TIM_CCMR2_CC4S_Pos);
+ timer->CCER |= TIM_CCER_CC4E;
+ timicp->dier |= STM32_TIM_DIER_CC4IE;
+ }
+ } else {
+ chSysHalt("invalid mode");
+ }
+ // keep only DMA bits, not ISR bits that are handle by driver
+ if (timicp->config->capture_cb == NULL)
+ timicp->dier = 0;
+ if (timicp->config->overflow_cb)
+ timicp->dier |= STM32_TIM_DIER_UIE;
+
+ timicp->dier = timicp->dier | (timicp->config->dier & (~ STM32_TIM_DIER_IRQ_MASK));
+ timicp->state = TIMIC_READY;
+}
+
+void timIcStartCapture(TimICDriver *timicp)
+{
+ osalDbgCheck(timicp != NULL);
+ osalDbgAssert(timicp->state == TIMIC_READY, "state error");
+ stm32_tim_t * const timer = timicp->config->timer;
+ osalDbgCheck(timer != NULL);
+ timer->CR1 = STM32_TIM_CR1_URS;
+ timer->EGR |= STM32_TIM_EGR_UG;
+ timer->SR = 0;
+ timer->DIER = timicp->dier;
+ timer->CR1 = STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN;
+ timicp->state = TIMIC_ACTIVE;
+}
+
+void timIcStopCapture(TimICDriver *timicp)
+{
+ osalDbgCheck(timicp != NULL);
+ osalDbgAssert(timicp->state != TIMIC_STOP, "state error");
+ stm32_tim_t * const timer = timicp->config->timer;
+ osalDbgCheck(timer != NULL);
+ timer->CR1 &= ~TIM_CR1_CEN;
+ timer->DIER = 0;
+ timicp->state = TIMIC_READY;
+}
+
+void timIcStop(TimICDriver *timicp)
+{
+ osalDbgAssert(timicp->state != TIMIC_STOP, "state error");
+ chMtxLock(&timicp->mut);
+ timIcRccDisable(timicp);
+ timIcObjectInit(timicp);
+ chMtxUnlock(&timicp->mut);
+ timicp->state = TIMIC_STOP;
+ timicp->dier = 0;
+}
+
+
+
+
+void timIcRccEnable(const TimICDriver * const timicp)
+{
+ const stm32_tim_t * const timer = timicp->config->timer;
+ const bool use_isr = timicp->config->capture_cb || timicp->config->overflow_cb;
+#ifdef TIM1
+ if (timer == STM32_TIM1) {
+ driverByTimerIndex[0] = timicp;
+ rccEnableTIM1(true);
+ rccResetTIM1();
+ if (use_isr) {
+#ifdef STM32_TIM1_UP_TIM10_NUMBER
+ nvicEnableVector(STM32_TIM1_UP_TIM10_NUMBER, STM32_IRQ_TIM1_UP_TIM10_PRIORITY);
+#endif
+#ifdef STM32_TIM1_CC_NUMBER
+ nvicEnableVector(STM32_TIM1_CC_NUMBER, STM32_IRQ_TIM1_CC_PRIORITY);
+#endif
+ }
+ }
+#endif
+#ifdef TIM2
+ else if (timer == STM32_TIM2) {
+ driverByTimerIndex[1] = timicp;
+ rccEnableTIM2(true);
+ rccResetTIM2();
+ if (use_isr) {
+ nvicEnableVector(STM32_TIM2_NUMBER, STM32_IRQ_TIM2_PRIORITY);
+ }
+ }
+#endif
+#ifdef TIM3
+ else if (timer == STM32_TIM3) {
+ driverByTimerIndex[2] = timicp;
+ rccEnableTIM3(true);
+ rccResetTIM3();
+ if (use_isr) {
+ nvicEnableVector(STM32_TIM3_NUMBER, STM32_IRQ_TIM3_PRIORITY);
+ }
+ }
+#endif
+#ifdef TIM4
+ else if (timer == STM32_TIM4) {
+ driverByTimerIndex[3] = timicp;
+ rccEnableTIM4(true);
+ rccResetTIM4();
+ if (use_isr) {
+ nvicEnableVector(STM32_TIM4_NUMBER, STM32_IRQ_TIM4_PRIORITY);
+ }
+ }
+#endif
+#ifdef TIM5
+ else if (timer == STM32_TIM5) {
+ driverByTimerIndex[4] = timicp;
+ rccEnableTIM5(true);
+ rccResetTIM5();
+ if (use_isr) {
+ nvicEnableVector(STM32_TIM5_NUMBER, STM32_IRQ_TIM5_PRIORITY);
+ }
+ }
+#endif
+#ifdef TIM8
+ else if (timer == STM32_TIM8) {
+ driverByTimerIndex[5] = timicp;
+ rccEnableTIM8(true);
+ rccResetTIM8();
+ if (use_isr) {
+#ifdef STM32_TIM8_UP_TIM13_NUMBER
+ nvicEnableVector(STM32_TIM8_UP_TIM13_NUMBER, STM32_IRQ_TIM8_UP_TIM13_PRIORITY);
+#endif
+#ifdef STM32_TIM8_CC_NUMBER
+ nvicEnableVector(STM32_TIM8_CC_NUMBER, STM32_IRQ_TIM8_CC_PRIORITY);
+#endif
+ }
+ }
+#endif
+#ifdef TIM9
+ else if (timer == STM32_TIM9) {
+ rccEnableTIM9(true);
+ rccResetTIM9();
+ }
+#endif
+#ifdef TIM10
+ else if (timer == STM32_TIM10) {
+ rccEnableTIM10(true);
+ rccResetTIM10();
+ }
+#endif
+#ifdef TIM11
+ else if (timer == STM32_TIM11) {
+ rccEnableTIM11(true);
+ rccResetTIM11();
+ }
+#endif
+#ifdef TIM12
+ else if (timer == STM32_TIM12) {
+ rccEnableTIM12(true);
+ rccResetTIM12();
+ }
+#endif
+#ifdef TIM13
+ else if (timer == STM32_TIM13) {
+ rccEnableTIM13(true);
+ rccResetTIM13();
+ }
+#endif
+#ifdef TIM14
+ else if (timer == STM32_TIM14) {
+ rccEnableTIM14(true);
+ rccResetTIM14();
+ }
+#endif
+#ifdef TIM15
+ else if (timer == STM32_TIM15) {
+ rccEnableTIM15(true);
+ rccResetTIM15();
+ }
+#endif
+#ifdef TIM16
+ else if (timer == STM32_TIM16) {
+ rccEnableTIM16(true);
+ rccResetTIM16();
+ }
+#endif
+#ifdef TIM17
+ else if (timer == STM32_TIM17) {
+ rccEnableTIM17(true);
+ rccResetTIM17();
+ }
+#endif
+#ifdef TIM18
+ else if (timer == STM32_TIM18) {
+ rccEnableTIM18(true);
+ rccResetTIM18();
+ }
+#endif
+#ifdef TIM19
+ else if (timer == STM32_TIM19) {
+ rccEnableTIM19(true);
+ rccResetTIM19();
+ }
+#endif
+ else {
+ chSysHalt("not a valid timer");
+ }
+};
+
+void timIcRccDisable(const TimICDriver * const timicp)
+{
+ const stm32_tim_t * const timer = timicp->config->timer;
+#ifdef TIM1
+ if (timer == STM32_TIM1) {
+ rccResetTIM1();
+ rccDisableTIM1();
+ }
+#endif
+#ifdef TIM2
+ else if (timer == STM32_TIM2) {
+ rccResetTIM2();
+ rccDisableTIM2();
+ }
+#endif
+#ifdef TIM3
+ else if (timer == STM32_TIM3) {
+ rccResetTIM3();
+ rccDisableTIM3();
+ }
+#endif
+#ifdef TIM4
+ else if (timer == STM32_TIM4) {
+ rccResetTIM4();
+ rccDisableTIM4();
+ }
+#endif
+#ifdef TIM5
+ else if (timer == STM32_TIM5) {
+ rccResetTIM5();
+ rccDisableTIM5();
+ }
+#endif
+#ifdef TIM8
+ else if (timer == STM32_TIM8) {
+ rccResetTIM8();
+ rccDisableTIM8();
+ }
+#endif
+#ifdef TIM9
+ else if (timer == STM32_TIM9) {
+ rccResetTIM9();
+ rccDisableTIM9();
+ }
+#endif
+#ifdef TIM10
+ else if (timer == STM32_TIM10) {
+ rccResetTIM10();
+ rccDisableTIM10();
+ }
+#endif
+#ifdef TIM11
+ else if (timer == STM32_TIM11) {
+ rccResetTIM11();
+ rccDisableTIM11();
+ }
+#endif
+#ifdef TIM12
+ else if (timer == STM32_TIM12) {
+ rccResetTIM12();
+ rccDisableTIM12();
+ }
+#endif
+#ifdef TIM13
+ else if (timer == STM32_TIM13) {
+ rccResetTIM13();
+ rccDisableTIM13();
+ }
+#endif
+#ifdef TIM14
+ else if (timer == STM32_TIM14) {
+ rccResetTIM14();
+ rccDisableTIM14();
+ }
+#endif
+#ifdef TIM15
+ else if (timer == STM32_TIM15) {
+ rccResetTIM15();
+ rccDisableTIM15();
+ }
+#endif
+#ifdef TIM16
+ else if (timer == STM32_TIM16) {
+ rccResetTIM16();
+ rccDisableTIM16();
+ }
+#endif
+#ifdef TIM17
+ else if (timer == STM32_TIM17) {
+ rccResetTIM17();
+ rccDisableTIM17();
+ }
+#endif
+#ifdef TIM18
+ else if (timer == STM32_TIM18) {
+ rccResetTIM18();
+ rccDisableTIM18();
+ }
+#endif
+#ifdef TIM19
+ else if (timer == STM32_TIM19) {
+ rccResetTIM19();
+ rccDisableTIM19();
+ }
+#endif
+ else {
+ chSysHalt("not a valid timer");
+ }
+};
+
+
+/*===========================================================================*/
+/* Driver interrupt handlers. */
+/*===========================================================================*/
+#ifndef STM32_INPUT_CAPTURE_USE_TIM1
+#define STM32_INPUT_CAPTURE_USE_TIM1 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_USE_TIM2
+#define STM32_INPUT_CAPTURE_USE_TIM2 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_USE_TIM3
+#define STM32_INPUT_CAPTURE_USE_TIM3 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_USE_TIM4
+#define STM32_INPUT_CAPTURE_USE_TIM4 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_USE_TIM5
+#define STM32_INPUT_CAPTURE_USE_TIM5 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_USE_TIM8
+#define STM32_INPUT_CAPTURE_USE_TIM8 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM1
+#define STM32_INPUT_CAPTURE_SHARE_TIM1 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM2
+#define STM32_INPUT_CAPTURE_SHARE_TIM2 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM3
+#define STM32_INPUT_CAPTURE_SHARE_TIM3 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM4
+#define STM32_INPUT_CAPTURE_SHARE_TIM4 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM5
+#define STM32_INPUT_CAPTURE_SHARE_TIM5 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_SHARE_TIM8
+#define STM32_INPUT_CAPTURE_SHARE_TIM8 false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM1_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM1_ISR false
+#endif
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM2_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM2_ISR false
+#endif
+
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM3_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM3_ISR false
+#endif
+
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM4_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM4_ISR false
+#endif
+
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM5_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM5_ISR false
+#endif
+
+
+#ifndef STM32_INPUT_CAPTURE_ENABLE_TIM8_ISR
+#define STM32_INPUT_CAPTURE_ENABLE_TIM8_ISR false
+#endif
+
+
+
+#if STM32_INPUT_CAPTURE_USE_TIM1 && (!STM32_INPUT_CAPTURE_SHARE_TIM1) && \
+(STM32_GPT_USE_TIM1 || STM32_ICU_USE_TIM1 || STM32_PWM_USE_TIM1)
+#error "STM32 INPUT_CAPTURE USE TIM1 but already used by GPT or ICU or PWM"
+#endif
+
+#if STM32_INPUT_CAPTURE_USE_TIM2 && (!STM32_INPUT_CAPTURE_SHARE_TIM2) && \
+(STM32_GPT_USE_TIM2 || STM32_ICU_USE_TIM2 || STM32_PWM_USE_TIM2)
+#error "STM32 INPUT_CAPTURE USE TIM2 but already used by GPT or ICU or PWM"
+#endif
+
+#if STM32_INPUT_CAPTURE_USE_TIM3 && (!STM32_INPUT_CAPTURE_SHARE_TIM3) && \
+(STM32_GPT_USE_TIM3 || STM32_ICU_USE_TIM3 || STM32_PWM_USE_TIM3)
+#error "STM32 INPUT_CAPTURE USE TIM3 but already used by GPT or ICU or PWM"
+#endif
+
+#if STM32_INPUT_CAPTURE_USE_TIM4 && (!STM32_INPUT_CAPTURE_SHARE_TIM4) && \
+(STM32_GPT_USE_TIM4 || STM32_ICU_USE_TIM4 || STM32_PWM_USE_TIM4)
+#error "STM32 INPUT_CAPTURE USE TIM4 but already used by GPT or ICU or PWM"
+#endif
+
+#if STM32_INPUT_CAPTURE_USE_TIM5 && (!STM32_INPUT_CAPTURE_SHARE_TIM5) && \
+(STM32_GPT_USE_TIM5 || STM32_ICU_USE_TIM5 || STM32_PWM_USE_TIM5)
+#error "STM32 INPUT_CAPTURE USE TIM5 but already used by GPT or ICU or PWM"
+#endif
+
+#if STM32_INPUT_CAPTURE_USE_TIM8 && (!STM32_INPUT_CAPTURE_SHARE_TIM8) && \
+(STM32_GPT_USE_TIM8 || STM32_ICU_USE_TIM8 || STM32_PWM_USE_TIM8)
+#error "STM32 INPUT_CAPTURE USE TIM8 but already used by GPT or ICU or PWM"
+#endif
+
+
+#if STM32_INPUT_CAPTURE_USE_TIM1 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM1_ISR
+#if defined(STM32_TIM1_UP_TIM10_HANDLER)
+/**
+ * @brief TIM1 compare interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM1_UP_TIM10_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[0]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#elif defined(STM32_TIM1_UP_HANDLER)
+OSAL_IRQ_HANDLER(STM32_TIM1_UP_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[0]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#else
+#error "no handler defined for TIM1"
+#endif
+
+#if !defined(STM32_TIM1_CC_HANDLER)
+#error "STM32_TIM1_CC_HANDLER not defined"
+#endif
+/**
+ * @brief TIM1 compare interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM1_CC_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[0]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM1_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM1 */
+
+#if STM32_INPUT_CAPTURE_USE_TIM2 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM2_ISR
+#if !defined(STM32_TIM2_HANDLER)
+#error "STM32_TIM2_HANDLER not defined"
+#endif
+/**
+ * @brief TIM2 interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM2_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[1]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM2_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM2 */
+
+#if STM32_INPUT_CAPTURE_USE_TIM3 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM3_ISR
+#if !defined(STM32_TIM3_HANDLER)
+#error "STM32_TIM3_HANDLER not defined"
+#endif
+/**
+ * @brief TIM3 interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM3_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[2]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM3_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM3 */
+
+#if STM32_INPUT_CAPTURE_USE_TIM4 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM4_ISR
+#if !defined(STM32_TIM4_HANDLER)
+#error "STM32_TIM4_HANDLER not defined"
+#endif
+/**
+ * @brief TIM4 interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM4_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[3]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM4_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM4 */
+
+#if STM32_INPUT_CAPTURE_USE_TIM5 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM5_ISR
+#if !defined(STM32_TIM5_HANDLER)
+#error "STM32_TIM5_HANDLER not defined"
+#endif
+/**
+ * @brief TIM5 interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM5_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[4]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM5_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM5 */
+
+#if STM32_INPUT_CAPTURE_USE_TIM8 || defined(__DOXYGEN__)
+#if STM32_INPUT_CAPTURE_ENABLE_TIM8_ISR
+#if defined(STM32_TIM8_UP_TIM13_HANDLER)
+/**
+ * @brief TIM8 compare interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM8_UP_TIM13_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[5]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif
+
+#if !defined(STM32_TIM8_CC_HANDLER)
+#error "STM32_TIM8_CC_HANDLER not defined"
+#endif
+/**
+ * @brief TIM8 compare interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(STM32_TIM8_CC_HANDLER) {
+
+ OSAL_IRQ_PROLOGUE();
+
+ input_capture_lld_serve_interrupt(driverByTimerIndex[5]);
+
+ OSAL_IRQ_EPILOGUE();
+}
+#endif /* STM32_INPUT_CAPTURE_ENABLE_TIM8_ISR */
+#endif /* STM32_INPUT_CAPTURE_USE_TIM8 */
+
+static void input_capture_lld_serve_interrupt(const TimICDriver * const timicp)
+{
+ uint32_t sr;
+ stm32_tim_t * const timer = timicp->config->timer;
+
+ sr = timer->SR;
+ sr &= (timer->DIER & STM32_TIM_DIER_IRQ_MASK);
+ timer->SR = ~sr;
+
+ if (timicp->channel & TIMIC_CH1) {
+ if ((sr & STM32_TIM_SR_CC1IF) != 0)
+ _input_capture_isr_invoke_capture_cb(timicp, 0);
+ }
+ if (timicp->channel & TIMIC_CH2) {
+ if ((sr & STM32_TIM_SR_CC2IF) != 0)
+ _input_capture_isr_invoke_capture_cb(timicp, 1);
+ }
+ if (timicp->channel & TIMIC_CH3) {
+ if ((sr & STM32_TIM_SR_CC3IF) != 0)
+ _input_capture_isr_invoke_capture_cb(timicp, 2);
+ }
+ if (timicp->channel & TIMIC_CH4) {
+ if ((sr & STM32_TIM_SR_CC4IF) != 0)
+ _input_capture_isr_invoke_capture_cb(timicp, 3);
+ }
+
+ if ((sr & STM32_TIM_SR_UIF) != 0)
+ _input_capture_isr_invoke_overflow_cb(timicp);
+}
+
+static void _input_capture_isr_invoke_capture_cb(const TimICDriver * const timicp, uint32_t channel)
+{
+ if (timicp->config->capture_cb) {
+ timicp->config->capture_cb(timicp, channel, timicp->config->timer->CCR[channel]);
+ }
+}
+
+static void _input_capture_isr_invoke_overflow_cb(const TimICDriver * const timicp)
+{
+ if (timicp->config->overflow_cb)
+ timicp->config->overflow_cb(timicp);
+}
diff --git a/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.h b/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.h
new file mode 100644
index 00000000000..5f45b808984
--- /dev/null
+++ b/sw/airborne/arch/chibios/mcu_periph/input_capture_arch.h
@@ -0,0 +1,108 @@
+#pragma once
+
+#include "ch.h"
+#include "hal.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ enum TimICMode {TIMIC_INPUT_CAPTURE, TIMIC_PWM_IN} ;
+ enum TimICActive {CH1_RISING_EDGE=1<<0, CH1_FALLING_EDGE=1<<1, CH1_BOTH_EDGES=1<<2,
+ CH2_RISING_EDGE=1<<3, CH2_FALLING_EDGE=1<<4, CH2_BOTH_EDGES=1<<5,
+ CH3_RISING_EDGE=1<<6, CH3_FALLING_EDGE=1<<7, CH3_BOTH_EDGES=1<<8,
+ CH4_RISING_EDGE=1<<9, CH4_FALLING_EDGE=1<<10, CH4_BOTH_EDGES=1<<11
+ } ;
+ enum TimICState {TIMIC_STOP, TIMIC_READY, TIMIC_ACTIVE};
+ typedef struct TimICDriver TimICDriver;
+ typedef void (*TimICCallbackCapture_t)(const TimICDriver *timicp, uint32_t channel, uint32_t capture);
+ typedef void (*TimICCallbackOverflow_t)(const TimICDriver *timicp);
+
+ /**
+ * @brief TimIC Driver configuration structure.
+ */
+ typedef struct {
+ /**
+ * @brief hardware timer pointer (example : &STM32_TIM1)
+ */
+ stm32_tim_t *timer;
+ TimICCallbackCapture_t capture_cb;
+ TimICCallbackOverflow_t overflow_cb;
+ enum TimICMode mode;
+ uint32_t active;
+ uint32_t dier;
+ uint32_t dcr;
+ uint32_t prescaler:17;
+ uint32_t arr;
+ } TimICConfig;
+
+
+ /**
+ * @brief Structure representing a TimIC driver.
+ */
+ struct TimICDriver {
+ /**
+ * @brief Current configuration data.
+ */
+ const TimICConfig *config;
+ /**
+ * @brief mutex to protect data read/write in concurrent context
+ */
+ uint32_t channel;
+ uint32_t dier;
+ mutex_t mut;
+ enum TimICState state;
+ };
+
+
+ /**
+ * @brief Initializes an input capture driver
+ *
+ * @param[out] inputCapturep pointer to a @p TimICDriver structure
+ * @init
+ */
+ void timIcObjectInit(TimICDriver *timicp);
+
+ /**
+ * @brief start an input capture driver
+ *
+ * @param[in] timicp pointer to a @p TimICDriver structure
+ * @param[in] configp pointer to a @p TimICConfig structure
+ * @brief configure the timer to get input capture data from timer
+ */
+ void timIcStart(TimICDriver *timicp, const TimICConfig *configp);
+
+ /**
+ * @brief start to capture
+ *
+ * @param[in] timicp pointer to a @p TimICDriver structure
+ * @brief start the input capture data from timer
+ */
+ void timIcStartCapture(TimICDriver *timicp);
+
+ /**
+ * @brief stop to capture
+ *
+ * @param[in] timicp pointer to a @p TimICDriver structure
+ * @brief stop the input capture data from timer
+ */
+ void timIcStopCapture(TimICDriver *timicp);
+
+ /**
+ * @brief stop a quadrature encoder driver
+ *
+ * @param[in] timicp pointer to a @p TimICDriver structure
+ * @brief stop and release the timer. After stop, any operation on timicp
+ * will result in undefined behavior and probably hardware fault
+ */
+ void timIcStop(TimICDriver *timicp);
+
+ void timIcRccEnable(const TimICDriver * const timicp);
+ void timIcRccDisable(const TimICDriver * const timicp);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.c b/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.c
new file mode 100644
index 00000000000..3e2dc85841d
--- /dev/null
+++ b/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.c
@@ -0,0 +1,132 @@
+#include "timerDmaCache.h"
+
+
+
+static void rccEnableAndReset(const stm32_tim_t * const timer);
+
+
+
+void timerDmaCache_cache(TimerDmaCache *tdcp, const DMADriver *fromDma, const stm32_tim_t *fromTim)
+{
+ memcpy(&tdcp->DMA_regs, fromDma->dmastream->stream, sizeof(tdcp->DMA_regs));
+ memcpy(&tdcp->TIM_regs, fromTim, sizeof(tdcp->TIM_regs));
+ tdcp->DMA_regs.CR &= ~STM32_DMA_CR_EN;
+ tdcp->TIM_regs.CR1 &= ~STM32_TIM_CR1_CEN;
+}
+
+void timerDmaCache_restore(const TimerDmaCache *tdcp, DMADriver *toDma, stm32_tim_t *toTim)
+{
+ rccEnableAndReset(toTim);
+ toTim->CR1 = 0;
+ memcpy((void *) &toTim->CR2, (void *) &tdcp->TIM_regs.CR2, sizeof(tdcp->TIM_regs) - 4U);
+ memcpy(toDma->dmastream->stream, &tdcp->DMA_regs, sizeof(tdcp->DMA_regs));
+}
+
+static void rccEnableAndReset(const stm32_tim_t * const timer)
+{
+#ifdef TIM1
+ if (timer == STM32_TIM1) {
+ rccEnableTIM1(true);
+ rccResetTIM1();
+ }
+#endif
+#ifdef TIM2
+ else if (timer == STM32_TIM2) {
+ rccEnableTIM2(true);
+ rccResetTIM2();
+ }
+#endif
+#ifdef TIM3
+ else if (timer == STM32_TIM3) {
+ rccEnableTIM3(true);
+ rccResetTIM3();
+ }
+#endif
+#ifdef TIM4
+ else if (timer == STM32_TIM4) {
+ rccEnableTIM4(true);
+ rccResetTIM4();
+ }
+#endif
+#ifdef TIM5
+ else if (timer == STM32_TIM5) {
+ rccEnableTIM5(true);
+ rccResetTIM5();
+ }
+#endif
+#ifdef TIM8
+ else if (timer == STM32_TIM8) {
+ rccEnableTIM8(true);
+ rccResetTIM8();
+ }
+#endif
+#ifdef TIM9
+ else if (timer == STM32_TIM9) {
+ rccEnableTIM9(true);
+ rccResetTIM9();
+ }
+#endif
+#ifdef TIM10
+ else if (timer == STM32_TIM10) {
+ rccEnableTIM10(true);
+ rccResetTIM10();
+ }
+#endif
+#ifdef TIM11
+ else if (timer == STM32_TIM11) {
+ rccEnableTIM11(true);
+ rccResetTIM11();
+ }
+#endif
+#ifdef TIM12
+ else if (timer == STM32_TIM12) {
+ rccEnableTIM12(true);
+ rccResetTIM12();
+ }
+#endif
+#ifdef TIM13
+ else if (timer == STM32_TIM13) {
+ rccEnableTIM13(true);
+ rccResetTIM13();
+ }
+#endif
+#ifdef TIM14
+ else if (timer == STM32_TIM14) {
+ rccEnableTIM14(true);
+ rccResetTIM14();
+ }
+#endif
+#ifdef TIM15
+ else if (timer == STM32_TIM15) {
+ rccEnableTIM15(true);
+ rccResetTIM15();
+ }
+#endif
+#ifdef TIM16
+ else if (timer == STM32_TIM16) {
+ rccEnableTIM16(true);
+ rccResetTIM16();
+ }
+#endif
+#ifdef TIM17
+ else if (timer == STM32_TIM17) {
+ rccEnableTIM17(true);
+ rccResetTIM17();
+ }
+#endif
+#ifdef TIM18
+ else if (timer == STM32_TIM18) {
+ rccEnableTIM18(true);
+ rccResetTIM18();
+ }
+#endif
+#ifdef TIM19
+ else if (timer == STM32_TIM19) {
+ rccEnableTIM19(true);
+ rccResetTIM19();
+ }
+#endif
+ else {
+ chSysHalt("not a valid timer");
+ }
+}
diff --git a/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.h b/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.h
new file mode 100644
index 00000000000..75a329c1604
--- /dev/null
+++ b/sw/airborne/arch/chibios/mcu_periph/timerDmaCache.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include
+#include
+#include
+#include "mcu_periph/hal_stm32_dma.h"
+
+
+
+typedef struct {
+ DMA_Stream_TypeDef DMA_regs;
+ stm32_tim_t TIM_regs;
+} TimerDmaCache;
+
+void timerDmaCache_cache(TimerDmaCache *tdcp, const DMADriver *fromDma, const stm32_tim_t *fromTim);
+void timerDmaCache_restore(const TimerDmaCache *tdcp, DMADriver *toDma, stm32_tim_t *toTim);
+
+
+
diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
index 42b05336ec3..86572ce20ac 100644
--- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
+++ b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
@@ -48,36 +48,57 @@ struct dshot_private actuators_dshot_private[ACTUATORS_DSHOT_NB];
#if DSHOT_CONF_TIM1
static DSHOTDriver DSHOTD1;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot1DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot1DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg1 = DSHOT_CONF1_DEF;
#endif
#if DSHOT_CONF_TIM2
static DSHOTDriver DSHOTD2;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot2DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot2DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg2 = DSHOT_CONF2_DEF;
#endif
#if DSHOT_CONF_TIM3
static DSHOTDriver DSHOTD3;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot3DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot3DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg3 = DSHOT_CONF3_DEF;
#endif
#if DSHOT_CONF_TIM4
static DSHOTDriver DSHOTD4;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot4DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot4DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg4 = DSHOT_CONF4_DEF;
#endif
#if DSHOT_CONF_TIM5
static DSHOTDriver DSHOTD5;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot5DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot5DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg5 = DSHOT_CONF5_DEF;
#endif
#if DSHOT_CONF_TIM8
static DSHOTDriver DSHOTD8;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot8DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot8DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg8 = DSHOT_CONF8_DEF;
#endif
#if DSHOT_CONF_TIM9
static DSHOTDriver DSHOTD9;
static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot9DmaBuffer);
+#if DSHOT_BIDIR
+static IN_DMA_SECTION_NOINIT(DshotRpmCaptureDmaBuffer dshot9DmaCaptureBuffer);
+#endif
static DSHOTConfig dshotcfg9 = DSHOT_CONF9_DEF;
#endif
@@ -85,15 +106,23 @@ static DSHOTConfig dshotcfg9 = DSHOT_CONF9_DEF;
static void esc_msg_send(struct transport_tx *trans, struct link_device *dev) {
for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) {
if (actuators_dshot_values[i].activated) {
- const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
+ DshotTelemetry dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
+
+ actuators_dshot_values[i].current = (float)dtelem.frame.current * 0.01f;
+ actuators_dshot_values[i].voltage = (float)dtelem.frame.voltage * 0.01f;
+ actuators_dshot_values[i].rpm = (float)dtelem.frame.rpm;
+
+#if DSHOT_BIDIR
+ const uint32_t erpm = dshotGetRpm(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
+ if(erpm != DSHOT_BIDIR_ERR_CRC && erpm != DSHOT_BIDIR_TLM_EDT) {
+ actuators_dshot_values[i].rpm = (float) erpm;
+ }
+#endif
- actuators_dshot_values[i].current = (float)dtelem->current * 0.01f;
- actuators_dshot_values[i].voltage = (float)dtelem->voltage * 0.01f;
- actuators_dshot_values[i].rpm = (float)dtelem->rpm;
float bat_voltage = electrical.vsupply;
float power = actuators_dshot_values[i].current * bat_voltage;
- float energy = (float)dtelem->consumption;
- float temp = dtelem->temp;
+ float energy = (float)dtelem.frame.consumption;
+ float temp = dtelem.frame.temp;
float temp_dev = 0;
pprz_msg_send_ESC(trans, dev, AC_ID,
&actuators_dshot_values[i].current,
@@ -189,51 +218,51 @@ void actuators_dshot_arch_init(void)
* Configure GPIO
*----------------*/
#ifdef DSHOT_SERVO_0
- gpio_setup_pin_af(DSHOT_SERVO_0_GPIO, DSHOT_SERVO_0_PIN, DSHOT_SERVO_0_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_0_GPIO, DSHOT_SERVO_0_PIN, DSHOT_SERVO_0_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_0], &actuators_dshot_private[DSHOT_SERVO_0], &DSHOT_SERVO_0_DRIVER, DSHOT_SERVO_0_CHANNEL);
#endif
#ifdef DSHOT_SERVO_1
- gpio_setup_pin_af(DSHOT_SERVO_1_GPIO, DSHOT_SERVO_1_PIN, DSHOT_SERVO_1_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_1_GPIO, DSHOT_SERVO_1_PIN, DSHOT_SERVO_1_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_1], &actuators_dshot_private[DSHOT_SERVO_1], &DSHOT_SERVO_1_DRIVER, DSHOT_SERVO_1_CHANNEL);
#endif
#ifdef DSHOT_SERVO_2
- gpio_setup_pin_af(DSHOT_SERVO_2_GPIO, DSHOT_SERVO_2_PIN, DSHOT_SERVO_2_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_2_GPIO, DSHOT_SERVO_2_PIN, DSHOT_SERVO_2_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_2], &actuators_dshot_private[DSHOT_SERVO_2], &DSHOT_SERVO_2_DRIVER, DSHOT_SERVO_2_CHANNEL);
#endif
#ifdef DSHOT_SERVO_3
- gpio_setup_pin_af(DSHOT_SERVO_3_GPIO, DSHOT_SERVO_3_PIN, DSHOT_SERVO_3_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_3_GPIO, DSHOT_SERVO_3_PIN, DSHOT_SERVO_3_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_3], &actuators_dshot_private[DSHOT_SERVO_3], &DSHOT_SERVO_3_DRIVER, DSHOT_SERVO_3_CHANNEL);
#endif
#ifdef DSHOT_SERVO_4
- gpio_setup_pin_af(DSHOT_SERVO_4_GPIO, DSHOT_SERVO_4_PIN, DSHOT_SERVO_4_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_4_GPIO, DSHOT_SERVO_4_PIN, DSHOT_SERVO_4_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_4], &actuators_dshot_private[DSHOT_SERVO_4], &DSHOT_SERVO_4_DRIVER, DSHOT_SERVO_4_CHANNEL);
#endif
#ifdef DSHOT_SERVO_5
- gpio_setup_pin_af(DSHOT_SERVO_5_GPIO, DSHOT_SERVO_5_PIN, DSHOT_SERVO_5_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_5_GPIO, DSHOT_SERVO_5_PIN, DSHOT_SERVO_5_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_5], &actuators_dshot_private[DSHOT_SERVO_5], &DSHOT_SERVO_5_DRIVER, DSHOT_SERVO_5_CHANNEL);
#endif
#ifdef DSHOT_SERVO_6
- gpio_setup_pin_af(DSHOT_SERVO_6_GPIO, DSHOT_SERVO_6_PIN, DSHOT_SERVO_6_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_6_GPIO, DSHOT_SERVO_6_PIN, DSHOT_SERVO_6_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_6], &actuators_dshot_private[DSHOT_SERVO_6], &DSHOT_SERVO_6_DRIVER, DSHOT_SERVO_6_CHANNEL);
#endif
#ifdef DSHOT_SERVO_7
- gpio_setup_pin_af(DSHOT_SERVO_7_GPIO, DSHOT_SERVO_7_PIN, DSHOT_SERVO_7_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_7_GPIO, DSHOT_SERVO_7_PIN, DSHOT_SERVO_7_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_7], &actuators_dshot_private[DSHOT_SERVO_7], &DSHOT_SERVO_7_DRIVER, DSHOT_SERVO_7_CHANNEL);
#endif
#ifdef DSHOT_SERVO_8
- gpio_setup_pin_af(DSHOT_SERVO_8_GPIO, DSHOT_SERVO_8_PIN, DSHOT_SERVO_8_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_8_GPIO, DSHOT_SERVO_8_PIN, DSHOT_SERVO_8_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_8], &actuators_dshot_private[DSHOT_SERVO_8], &DSHOT_SERVO_8_DRIVER, DSHOT_SERVO_8_CHANNEL);
#endif
#ifdef DSHOT_SERVO_9
- gpio_setup_pin_af(DSHOT_SERVO_9_GPIO, DSHOT_SERVO_9_PIN, DSHOT_SERVO_9_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_9_GPIO, DSHOT_SERVO_9_PIN, DSHOT_SERVO_9_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_9], &actuators_dshot_private[DSHOT_SERVO_9], &DSHOT_SERVO_9_DRIVER, DSHOT_SERVO_9_CHANNEL);
#endif
#ifdef DSHOT_SERVO_10
- gpio_setup_pin_af(DSHOT_SERVO_10_GPIO, DSHOT_SERVO_10_PIN, DSHOT_SERVO_10_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_10_GPIO, DSHOT_SERVO_10_PIN, DSHOT_SERVO_10_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_10], &actuators_dshot_private[DSHOT_SERVO_10], &DSHOT_SERVO_10_DRIVER, DSHOT_SERVO_10_CHANNEL);
#endif
#ifdef DSHOT_SERVO_11
- gpio_setup_pin_af(DSHOT_SERVO_11_GPIO, DSHOT_SERVO_11_PIN, DSHOT_SERVO_11_AF, true);
+ gpio_setup_pin_af_pullup(DSHOT_SERVO_11_GPIO, DSHOT_SERVO_11_PIN, DSHOT_SERVO_11_AF);
dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_11], &actuators_dshot_private[DSHOT_SERVO_11], &DSHOT_SERVO_11_DRIVER, DSHOT_SERVO_11_CHANNEL);
#endif
@@ -332,11 +361,22 @@ void actuators_dshot_arch_commit(void)
struct act_feedback_t feedback[ACTUATORS_DSHOT_NB] = { 0 };
for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) {
- feedback[i].idx = ACTUATORS_DSHOT_OFFSET + i;
+ feedback[i].idx = get_servo_idx_DSHOT(i);
if (actuators_dshot_values[i].activated) {
- const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
- feedback[i].rpm = dtelem->rpm;
+ DshotTelemetry dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
+ feedback[i].rpm = dtelem.frame.rpm;
feedback[i].set.rpm = true;
+#if DSHOT_BIDIR
+ const uint32_t erpm = dshotGetRpm(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
+ if(erpm != DSHOT_BIDIR_ERR_CRC) {
+ if(erpm != DSHOT_BIDIR_TLM_EDT) {
+ feedback[i].rpm = (float) erpm;
+ feedback[i].set.rpm = true;
+ }
+ } else {
+ feedback[i].set.rpm = false;
+ }
+#endif
}
}
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_DSHOT_ID, feedback, ACTUATORS_DSHOT_NB);
diff --git a/sw/airborne/arch/chibios/modules/actuators/dshot_erps.c b/sw/airborne/arch/chibios/modules/actuators/dshot_erps.c
new file mode 100644
index 00000000000..b5a918fe468
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/dshot_erps.c
@@ -0,0 +1,270 @@
+#include "modules/actuators/dshot_erps.h"
+//#include "stdutil.h"
+
+
+/**
+ * @brief IBM GCR encoding lookup table
+ */
+static const uint8_t gcrNibble[16] = {
+ 0x19, 0x1B, 0x12, 0x13, 0x1D, 0x15, 0x16, 0x17,
+ 0x1A, 0x09, 0x0A, 0x0B, 0x1E, 0x0D, 0x0E, 0x0F};
+
+/**
+ * @brief IBM GCR decoding lookup table
+ */
+static const uint8_t gcrNibbleInv[32] = {
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0x9, 0xa, 0xb, 0xff, 0xd, 0xe, 0xf,
+ 0xff, 0xff, 0x2, 0x3, 0xff, 0x5, 0x6, 0x7,
+ 0xff, 0x0, 0x8, 0x1, 0xff, 0x4, 0xc, 0xff};
+
+static uint8_t crc4(uint16_t val);
+static DshotEPeriodPacket eperiodToPacked(const uint32_t eperiod);
+static uint32_t greyEncode(uint32_t num);
+static uint32_t greyDecode(const uint32_t num);
+static uint32_t gcrEncode(uint32_t from);
+static uint32_t gcrDecode(uint32_t from);
+static uint32_t eperiodEncode(const uint16_t eperiod);
+static uint32_t eperiodDecode(const uint32_t frame);
+
+
+
+static void setFromEperiod(DshotErps *derpsp, uint32_t eperiod);
+static void frameToPacket(DshotErps *derpsp);
+static void packetToFrame(DshotErps *derpsp);
+
+
+
+/**
+ * @brief initialise from GCR encoded frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+const DshotErps* DshotErpsSetFromFrame(DshotErps *derpsp, uint32_t frame)
+{
+ derpsp->ef = frame;
+ frameToPacket(derpsp);
+ return derpsp;
+}
+
+/**
+ * @brief initialise from rpm value
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+const DshotErps* DshotErpsSetFromRpm(DshotErps *derpsp, uint32_t rpm)
+{
+ uint32_t eperiod = ((uint32_t) 60e6f) / rpm;
+ setFromEperiod(derpsp, eperiod);
+ return derpsp;
+}
+
+/**
+ * @brief return eperiod from mantisse and exponent
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @return eperiod in microseconds
+ * @note getEperiod avoid a division and is more cpu friendly than getRpm
+ * @private
+ */
+uint32_t DshotErpsGetEperiod(const DshotErps *derpsp)
+{
+ return derpsp->ep.mantisse << derpsp->ep.exponent;
+}
+
+
+/**
+ * @brief calculate and return rpm
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @return rotational speed in RPM
+ * @note involve a division, which is a cpu cycle hog, If you can
+ use getEperiod instead, it will be less calculus intensive
+ * @api
+ */
+uint32_t DshotErpsGetRpm(const DshotErps *derpsp)
+{
+ return ((uint32_t) 60e6f) / DshotErpsGetEperiod(derpsp);
+}
+
+/**
+ * @brief check packed validity
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @return true if CRC is OK
+ * @api
+ */
+bool DshotErpsCheckCrc4(const DshotErps *derpsp)
+{
+ return (crc4(derpsp->ep.rawFrame) == derpsp->ep.crc);
+}
+
+/**
+ * @brief calculate crc4
+ *
+ * @param[in] val
+ * @return true if CRC is OK
+ * @private
+ */
+static uint8_t crc4(uint16_t val)
+{
+ val >>= 4;
+ return (~(val ^ (val >> 4) ^ (val >> 8))) & 0x0F;
+}
+
+/**
+ * @brief encode packet from eperiod
+ *
+ * @param[in] eperiod in microseconds
+ * @return encoded DshotEPeriodPacket
+ * @private
+ */
+static DshotEPeriodPacket eperiodToPacked(const uint32_t eperiod)
+{
+ DshotEPeriodPacket p;
+ const uint32_t exponent = eperiod >> 9;
+
+ if (exponent > 128) {
+ p = (DshotEPeriodPacket) {.crc = 0, .mantisse = 511, .exponent = 7};
+ } else {
+ p.exponent = 32U - __builtin_clz(exponent);
+ p.mantisse = eperiod >> p.exponent;
+ }
+ p.crc = crc4(p.rawFrame);
+ return p;
+}
+
+/**
+ * @brief get grey binary value from natural binary
+ *
+ * @param[in] num natural binary value
+ * @return grey encoded value
+ * @private
+ */
+static uint32_t greyEncode(uint32_t num)
+{
+ num ^= (num >> 16);
+ num ^= (num >> 8);
+ num ^= (num >> 4);
+ num ^= (num >> 2);
+ num ^= (num >> 1);
+ return num;
+}
+
+/**
+ * @brief get natural binary value from grey binary
+ *
+ * @param[in] grey encoded value
+ * @return num natural binary value
+ * @private
+ */
+static uint32_t greyDecode(const uint32_t num)
+{
+ return num ^ (num >> 1);
+}
+
+/**
+ * @brief encode 16 bit value to 20 bits GCR
+ *
+ * @param[in] binary value
+ * @return GCR(0,2) encoded value
+ * @private
+ */
+static uint32_t gcrEncode(uint32_t from)
+{
+ uint32_t ret = 0;
+ for (size_t i = 0U; i < 4U; i++) {
+ // printf("nibble %u from = 0x%x to = 0x%x\n",
+ // i, from & 0xf, gcrNibble[from & 0xf]);
+ ret |= (gcrNibble[from & 0xf] << (i*5));
+ from >>= 4U;
+ }
+ return ret;
+}
+
+/**
+ * @brief decode 20 bits GCR value to 16 bits natural
+ *
+ * @param[in] GCR(0,2) encoded value
+ * @return binary value
+ * @private
+ */
+static uint32_t gcrDecode(uint32_t from)
+{
+ uint32_t ret = 0;
+ for (size_t i = 0; i < 4U; i++) {
+ const uint32_t nibble = gcrNibbleInv[from & 0x1f];
+ if (nibble == 0xff) {
+ ret = 0x0;
+ break;
+ }
+ // printf("nibble %u from = 0x%x to = 0x%x\n",
+ // i, from & 0xf, gcrNibble[from & 0xf]);
+ ret |= (nibble << (i << 2U));
+ from >>= 5U;
+ }
+ return ret;
+}
+
+/**
+ * @brief encode eperiod to 20 bits GCR of grey value
+ *
+ * @param[in] eperiod in microseconds
+ * @return GCR(0,2) encoded value ready to be transmitted
+ * @private
+ */
+static uint32_t eperiodEncode(const uint16_t eperiod)
+{
+ return greyEncode(gcrEncode(eperiod));
+}
+
+/**
+ * @brief decode 20 bits GCR of grey value to eperiod
+ *
+ * @param[in] GCR(0,2) encoded value received from ESC
+ * @return eperiod in microseconds
+ * @private
+ */
+static uint32_t eperiodDecode(const uint32_t frame)
+{
+ return gcrDecode(greyDecode(frame));
+}
+
+/**
+ * @brief initialize from eperiod in microseconds
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @param[in] eperiod in microseconds
+ * @private
+ */
+static void setFromEperiod(DshotErps *derpsp, uint32_t eperiod)
+{
+ derpsp->ep = eperiodToPacked(eperiod);
+ packetToFrame(derpsp);
+}
+
+/**
+ * @brief decode eperiod
+ *
+ * @param[in out] derpsp pointer to the @p DshotErps object
+ * @private
+ */
+static void frameToPacket(DshotErps *derpsp)
+{
+ derpsp->ep.rawFrame = eperiodDecode(derpsp->ef);
+ // DebugTrace("DBG> 0x%lx => 0x%x", derpsp->ef, derpsp->ep.rawFrame);
+}
+
+/**
+ * @brief encode eperiod
+ *
+ * @param[in out] derpsp pointer to the @p DshotErps object
+ * @private
+ */
+static void packetToFrame(DshotErps *derpsp)
+{
+ derpsp->ef = eperiodEncode(derpsp->ep.rawFrame);
+}
+
diff --git a/sw/airborne/arch/chibios/modules/actuators/dshot_erps.h b/sw/airborne/arch/chibios/modules/actuators/dshot_erps.h
new file mode 100644
index 00000000000..a75c76246fb
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/dshot_erps.h
@@ -0,0 +1,150 @@
+#pragma once
+
+#include
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief ERPS telemetry type
+ * @note last (as of mid 2022) addition to the dshot bidir protocol
+ */
+ typedef enum {
+ EDT_NOTEDT=0x0,
+ EDT_TEMP=0x2, EDT_VOLT=0x4, EDT_CURRENT=0x6,
+ EDT_DBG1=0x8, EDT_DBG2=0xA, EDT_STRESS=0xC,
+ EDT_STATUS=0xE
+ } EdtType;
+
+typedef enum {
+ EDT_STATUS_ALERT = 1<<7, EDT_STATUS_WARNING = 1<<6, EDT_STATUS_ERROR = 1<<5,
+ EDT_STATUS_MAX_STRESS_MASK=0b111<1
+ } EdtStatus;
+
+
+/**
+ * @brief ERPS classic rpm frame
+ */
+typedef union {
+ struct {
+ uint16_t crc:4;
+ uint16_t mantisse:9;
+ uint16_t exponent:3;
+ };
+ uint16_t rawFrame;
+} DshotEPeriodPacket;
+
+/**
+ * @brief ERPS telemetry frame
+ */
+typedef union {
+ struct {
+ uint16_t crc:4;
+ uint16_t edt_value:8;
+ EdtType edt_type:4;
+ } ;
+ uint16_t rawFrame;
+} DshotEPeriodTelemetry;
+
+_Static_assert(sizeof(DshotEPeriodPacket) == sizeof(uint16_t), "DshotEPeriodPacket size error");
+_Static_assert(sizeof(DshotEPeriodTelemetry) == sizeof(uint16_t), "DshotEPeriodTelemetry size error");
+
+
+/**
+ * @brief ERPS complete frame, raw and decoded
+ */
+ typedef struct {
+ DshotEPeriodPacket ep; // 16 bits packet
+ uint32_t ef; // 21 bits frame
+ } DshotErps;
+
+
+
+ const DshotErps* DshotErpsSetFromFrame(DshotErps *derpsp, uint32_t frame);
+ const DshotErps* DshotErpsSetFromRpm(DshotErps *derpsp, uint32_t rpm);
+ uint32_t DshotErpsGetEperiod(const DshotErps *derpsp);
+ uint32_t DshotErpsGetRpm(const DshotErps *derpsp);
+ bool DshotErpsCheckCrc4(const DshotErps *derpsp);
+
+/**
+ * @brief return encoded frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+ static inline uint32_t DshotErpsGetFrame(const DshotErps *derpsp) {return derpsp->ef;}
+/**
+ * @brief return true if current frame is a telemetry frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+ static inline bool DshotErpsIsEdt(const DshotErps *derpsp) {
+ const DshotEPeriodTelemetry tm = {.rawFrame = derpsp->ep.rawFrame};
+ return
+ ((tm.edt_type & 0b0001) == 0) &&
+ ((tm.edt_type & 0b1110) != 0);
+ }
+
+/**
+ * @brief return type of a telemetry frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+ static inline EdtType DshotErpsEdtType(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_type;
+}
+/**
+ * @brief return temperature for a temperature telemetry frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+static inline uint8_t DshotErpsEdtTempCentigrade(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_value;
+}
+/**
+ * @brief return voltage for a voltage telemetry frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+static inline uint16_t DshotErpsEdtCentiVolts(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_value * 100U / 4U;
+}
+/**
+ * @brief return current intensity for a current telemetry frame
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+static inline uint16_t DshotErpsEdtCurrentAmp(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_value;
+}
+
+/**
+ * @brief return stress value
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+static inline uint16_t DshotErpsEdtStress(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_value;
+}
+
+/**
+ * @brief return status value
+ *
+ * @param[in] derpsp pointer to the @p DshotErps object
+ * @api
+ */
+static inline uint16_t DshotErpsEdtStatus(const DshotErps *derpsp) {
+ return (DshotEPeriodTelemetry) {.rawFrame = derpsp->ep.rawFrame}.edt_value;
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.c b/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.c
new file mode 100644
index 00000000000..edfb4936f79
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.c
@@ -0,0 +1,420 @@
+#include "modules/actuators/dshot_rpmCapture.h"
+#include
+#if DSHOT_STATISTICS
+#include
+#endif
+
+#if DSHOT_SPEED == 0
+#error dynamic dshot speed is not yet implemented in DSHOT BIDIR
+#endif
+
+#ifdef STM32H7XX
+static const float TIM_FREQ_MHZ = (STM32_TIMCLK1 / 1e6d);
+#else
+// MUST FIXE : on F4 and F7, dshot bidir is not compatible with 84MHz timers
+static const float TIM_FREQ_MHZ = (STM32_SYSCLK / 1e6d);
+#endif
+static const float bit1t_us = TIM_FREQ_MHZ * 6.67d * 4 / 5;
+static const float speed_factor = DSHOT_SPEED / 150;
+
+static const uint32_t ERPS_BIT1_DUTY = bit1t_us / speed_factor;
+static const uint32_t TIM_PRESCALER = 1U;
+
+/* gpt based timout is dependant of CPU speed, because
+ the more time you spend in the ISR and context switches,
+ the less you have to wait for telemetry frame completion */
+#if defined STM32H7XX
+#define SWTICH_TO_CAPTURE_BASE_TIMOUT 38U
+#elif defined STM32F7XX
+#define SWTICH_TO_CAPTURE_BASE_TIMOUT 32U
+#else
+#define SWTICH_TO_CAPTURE_BASE_TIMOUT 28U
+#endif
+
+
+static void startCapture(DshotRpmCapture *drcp);
+static void stopCapture(DshotRpmCapture *drcp);
+static void initCache(DshotRpmCapture *drcp);
+static uint32_t processErpsDmaBuffer(const uint16_t *capture, size_t dmaLen);
+static void buildDmaConfig(DshotRpmCapture *drcp);
+static void dmaErrCb(DMADriver *dmad, dmaerrormask_t em);
+static void gptCb(GPTDriver *gptd);
+
+#if DSHOT_STATISTICS
+static volatile dmaerrormask_t lastErr;
+static volatile uint32_t dmaErrs = 0;
+#endif
+
+#if !defined __GNUC__ || __GNUC__ < 13
+#define nullptr NULL
+#endif
+
+
+
+static const struct {
+ uint32_t active;
+ uint32_t dier;
+} activeDier[4] = {
+ {CH1_BOTH_EDGES,
+ TIM_DIER_CC1DE | TIM_DIER_TDE},
+ {CH1_BOTH_EDGES | CH2_BOTH_EDGES,
+ TIM_DIER_CC1DE | TIM_DIER_CC2DE | TIM_DIER_TDE},
+ {CH1_BOTH_EDGES | CH2_BOTH_EDGES | CH3_BOTH_EDGES,
+ TIM_DIER_CC1DE | TIM_DIER_CC2DE | TIM_DIER_CC3DE | TIM_DIER_TDE},
+ {CH1_BOTH_EDGES | CH2_BOTH_EDGES | CH3_BOTH_EDGES | CH4_BOTH_EDGES,
+ TIM_DIER_CC1DE | TIM_DIER_CC2DE | TIM_DIER_CC3DE | TIM_DIER_CC4DE | TIM_DIER_TDE}
+};
+
+static const TimICConfig timicCfgSkel = {
+ .timer = NULL,
+ .capture_cb = NULL,
+ .overflow_cb = NULL,
+ .mode = TIMIC_INPUT_CAPTURE,
+ .active = activeDier[DSHOT_CHANNELS-1].active,
+ .dier = activeDier[DSHOT_CHANNELS-1].dier,
+ .dcr = 0, // direct access, not using DMAR
+ .prescaler = TIM_PRESCALER,
+ .arr = 0xffffffff
+};
+
+/*
+this driver need additional field in gptDriver structure (in halconf.h):
+one should add following line in the GPT section of halconf.h :
+#define GPT_DRIVER_EXT_FIELDS void *user_data;
+*/
+#ifndef GPT_DRIVER_EXT_FIELDS
+#error dshot rpmcapture driver involve defining #define GPT_DRIVER_EXT_FIELDS void *user_data; in halconf.h
+#else
+// redefine at the needed value to trigger error if defined with another value
+#define GPT_DRIVER_EXT_FIELDS void *user_data;
+#endif
+
+static const GPTConfig gptCfg = {
+ .frequency = 1000U * 1000U, // 1MHz
+ .callback = &gptCb,
+ .cr2 = 0,
+ .dier = 0
+};
+
+
+/**
+ * @brief Configures and activates the DSHOT ERPS CAPTURE driver.
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @param[in] cfg pointer to the @p DshotRpmCaptureConfig object.
+ * @param[in] timer pointer to the underlying timer (same one used to output control frame)
+ * @api
+ */
+void dshotRpmCaptureStart(DshotRpmCapture *drcp, const DshotRpmCaptureConfig *cfg,
+ stm32_tim_t *timer)
+{
+ memset(drcp, 0, sizeof(*drcp));
+ drcp->config = cfg;
+ drcp->icCfg = timicCfgSkel;
+ drcp->icCfg.timer = timer;
+ initCache(drcp);
+}
+
+/**
+ * @brief stop the the DSHOT ERPS CAPTURE driver.
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @api
+ */
+void dshotRpmCaptureStop(DshotRpmCapture *drcp)
+{
+ stopCapture(drcp);
+}
+
+#if DSHOT_STATISTICS
+/**
+ * @brief return dma error counter
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @api
+ */
+uint32_t dshotRpmGetDmaErr(void) {
+return dmaErrs;
+}
+#endif
+
+
+/**
+ * @brief capture the DSHOT ERPS frame(s) : one frame for each DSHOT_CHANNELS
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @note synchronous API, when it returns, the erpm data is available
+ * @api
+ */
+void dshotRpmCatchErps(DshotRpmCapture *drcp)
+{
+ startCapture(drcp);
+ static systime_t ts = 0;
+ if (ts == 0)
+ ts = chVTGetSystemTimeX();
+
+ memset(drcp->config->dma_capture->dma_buf, 0, sizeof(drcp->config->dma_capture->dma_buf));
+
+ for (size_t i = 0; i < DSHOT_CHANNELS; i++) {
+ dmaStartTransfert(&drcp->dmads[i], &drcp->icd.config->timer->CCR[i],
+ drcp->config->dma_capture->dma_buf[i].buf,
+ DSHOT_DMA_DATA_LEN + DSHOT_DMA_EXTRADATA_LEN);
+ }
+
+ osalSysLock();
+ // dma end callback will resume the thread upon completion of ALL dma transaction
+ // else, the timeout will take care of thread resume
+ static const sysinterval_t timeoutUs = SWTICH_TO_CAPTURE_BASE_TIMOUT +
+ (120U * 150U / DSHOT_SPEED);
+ //palSetLine(LINE_LA_DBG_1);
+ gptStartOneShotI(drcp->config->gptd, timeoutUs);
+ // palClearLine(LINE_LA_DBG_1);
+ chThdSuspendS(&drcp->dmads[0].thread);
+ // palSetLine(LINE_LA_DBG_1);
+ // chSysPolledDelayX(1);
+ // palClearLine(LINE_LA_DBG_1);
+
+ for (size_t i = 0; i < DSHOT_CHANNELS; i++)
+ dmaStopTransfertI(&drcp->dmads[i]);
+
+ osalSysUnlock();
+ stopCapture(drcp);
+
+#if DSHOT_STATISTICS
+ const rtcnt_t tstart = chSysGetRealtimeCounterX();
+#endif
+
+ for (size_t i = 0; i < DSHOT_CHANNELS; i++) {
+#if __DCACHE_PRESENT
+ if (drcp->config->dcache_memory_in_use == true) {
+ cacheBufferInvalidate(drcp->config->dma_capture->dma_buf[i].buf,
+ DSHOT_DMA_DATA_LEN + DSHOT_DMA_EXTRADATA_LEN);
+ }
+#endif
+ drcp->rpms[i] = processErpsDmaBuffer(drcp->config->dma_capture->dma_buf[i].buf,
+ DSHOT_DMA_DATA_LEN + DSHOT_DMA_EXTRADATA_LEN -
+ dmaGetTransactionCounter(&drcp->dmads[i]));
+ }
+#if DSHOT_STATISTICS
+ const rtcnt_t tstop = chSysGetRealtimeCounterX();
+ drcp->nbDecode += DSHOT_CHANNELS;
+ drcp->accumDecodeTime += (tstop - tstart);
+#endif
+
+#if defined(DFREQ) && (DFREQ < 10) && (DFREQ != 0)
+ DebugTrace("dma out on %s", msg == MSG_OK ? "completion" : "timeout");
+#endif
+}
+
+#if DSHOT_STATISTICS
+/**
+ * @brief trace content of DMA buffer for tuning
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @param[in] index index of channel [0 .. 4]
+ * @api
+ */
+void dshotRpmTrace(DshotRpmCapture *drcp, uint8_t index)
+{
+ for (size_t i = 0; i < DSHOT_CHANNELS; i++) {
+ if ((index != 0xff) && (index != i))
+ continue;
+ uint16_t cur = drcp->config->dma_capture->dma_buf[i].buf[0];
+ for (size_t j = 1; j < DSHOT_DMA_DATA_LEN; j++) {
+ const uint16_t dur = drcp->config->dma_capture->dma_buf[i].buf[j] - cur;
+ cur = drcp->config->dma_capture->dma_buf[i].buf[j];
+ chprintf(chp, "[%u] %u, ", i, dur);
+ }
+ DebugTrace("");
+ }
+ DebugTrace("");
+}
+#endif
+
+
+/*
+# _ __ _ _
+# | '_ \ (_) | |
+# | |_) | _ __ _ __ __ __ _ | |_ ___
+# | .__/ | '__| | | \ \ / / / _` | | __| / _ \
+# | | | | | | \ V / | (_| | \ |_ | __/
+# |_| |_| |_| \_/ \__,_| \__| \___|
+*/
+
+/**
+ * @brief build dma configuration structure
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @private
+ */
+static void buildDmaConfig(DshotRpmCapture *drcp)
+{
+ static const DMAConfig skel = (DMAConfig) {
+ .stream = 0,
+#if STM32_DMA_SUPPORTS_DMAMUX
+ .dmamux = 0,
+#else
+ .channel = 0,
+#endif
+ .inc_peripheral_addr = false,
+ .inc_memory_addr = true,
+ .op_mode = DMA_ONESHOT,
+ .end_cb = NULL,
+ .error_cb = &dmaErrCb,
+ //.error_cb = NULL,
+#if STM32_DMA_USE_ASYNC_TIMOUT
+ .timeout = TIME_MS2I(100),
+#endif
+ .direction = DMA_DIR_P2M,
+ .dma_priority = 1,
+ .irq_priority = 7,
+ .psize = 2,
+ .msize = 2,
+ .pburst = 0,
+ .mburst = 0,
+ .fifo = 4,
+ .periph_inc_size_4 = false,
+ .transfert_end_ctrl_by_periph = false
+ };
+
+ for (size_t i = 0; i < DSHOT_CHANNELS; i++) {
+ drcp->dmaCfgs[i] = skel;
+ drcp->dmaCfgs[i].stream = drcp->config->dma_streams[i].stream;
+#if STM32_DMA_SUPPORTS_DMAMUX
+ drcp->dmaCfgs[i].dmamux = drcp->config->dma_streams[i].dmamux;
+#else
+ drcp->dmaCfgs[i].channel = drcp->config->dma_streams[i].channel;
+#endif
+ }
+}
+
+/**
+ * @brief build dma and timer registers cache
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @private
+ */
+static void initCache(DshotRpmCapture *drcp)
+{
+ const DshotRpmCaptureConfig *cfg = drcp->config;
+ timIcObjectInit(&drcp->icd);
+ timIcStart(&drcp->icd, &drcp->icCfg);
+ cfg->gptd->user_data = drcp; // user data in GPT driver
+
+ buildDmaConfig(drcp);
+ for (size_t i=0; i < DSHOT_CHANNELS; i++) {
+ dmaObjectInit(&drcp->dmads[i]);
+ dmaStart(&drcp->dmads[i], &drcp->dmaCfgs[i]);
+ }
+
+ timerDmaCache_cache(&drcp->cache, &drcp->dmads[0], drcp->icCfg.timer);
+ dshotRpmCaptureStop(drcp);
+}
+
+/**
+ * @brief start the DMA capture
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @private
+ */
+static void startCapture(DshotRpmCapture *drcp)
+{
+ gptStart(drcp->config->gptd, &gptCfg);
+ timerDmaCache_restore(&drcp->cache, &drcp->dmads[0], drcp->icCfg.timer);
+ timIcStartCapture(&drcp->icd);
+}
+
+/**
+ * @brief stop the DMA capture
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @private
+ */
+static void stopCapture(DshotRpmCapture *drcp)
+{
+ timIcStopCapture(&drcp->icd);
+ timIcRccDisable(&drcp->icd);
+ gptStop(drcp->config->gptd);
+}
+
+
+/**
+ * @brief convert DMA buffer full of pulse length into raw ERPS frame
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @private
+ */
+static uint32_t processErpsDmaBuffer(const uint16_t *capture, size_t dmaLen)
+{
+ static const size_t frameLen = 20U;
+ uint32_t erpsVal = 0;
+ uint_fast8_t bit = 0x0;
+ uint_fast8_t bitIndex = 0;
+ uint_fast16_t prec = capture[0];
+
+ for (size_t i = 1U; i < dmaLen; i++) {
+ const uint_fast16_t len = capture[i] - prec;
+ prec = capture[i];
+
+ // GRC encoding garanties that there can be no more than 3 consecutives bits at the same level
+ // made some test to replace division by multiplication + shift without any speed gain
+ const uint_fast8_t nbConsecutives = (len + (ERPS_BIT1_DUTY / 2U)) / ERPS_BIT1_DUTY;
+ if (bit) {
+ switch(nbConsecutives) {
+ case 1U: erpsVal |= (0b001 << (frameLen - bitIndex)); break;
+ case 2U: erpsVal |= (0b011 << (frameLen - bitIndex - 1U)); break;
+ default: erpsVal |= (0b111 << (frameLen - bitIndex - 2U)); break;
+ }
+ }
+ bit = ~bit; // flip bit
+ bitIndex += nbConsecutives;
+ }
+ // there can be several high bits hidden in the trailing high level signal
+ for (size_t j=bitIndex; j <= frameLen; j++)
+ erpsVal |= (1U << (frameLen - j));
+
+ // alternative implementation for the trailing high level signal
+ // slower on M4
+ /* switch(frameLen - bitIndex) { */
+ /* case 0U: erpsVal |= 0b001; break; */
+ /* case 1U: erpsVal |= 0b011; break; */
+ /* case 2U: erpsVal |= 0b111; break; */
+ /* default: erpsVal |= 0b1111; break; */
+ /* } */
+
+ // DebugTrace("bit index = %u; erpsVal = 0x%lx", bitIndex, erpsVal);
+ return erpsVal;
+}
+
+/**
+ * @brief dma error ISR :
+ *
+ * @param[in] drcp pointer to the @p DMADriver
+ * @param[in] em error mask
+ * @note does nothing if DSHOT_STATISTICS == FALSE
+ * @private
+ */
+static void dmaErrCb(DMADriver *, dmaerrormask_t em)
+{
+#if DSHOT_STATISTICS
+ lastErr = em;
+ dmaErrs++;
+#else
+ (void) em;
+#endif
+}
+
+/**
+ * @brief dma timeout ISR :
+ *
+ * @param[in] gptd pointer to the @p GPTDriver that trigger the timeout
+ * @note does nothing if DSHOT_STATISTICS == FALSE
+ * @private
+ */
+static void gptCb(GPTDriver *gptd)
+{
+ chSysLockFromISR();
+ DshotRpmCapture *drcd = (DshotRpmCapture *) gptd->user_data;
+ chThdResumeI(&drcd->dmads[0].thread, MSG_TIMEOUT);
+ chSysUnlockFromISR();
+}
diff --git a/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.h b/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.h
new file mode 100644
index 00000000000..a04d0669c95
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/dshot_rpmCapture.h
@@ -0,0 +1,216 @@
+#pragma once
+
+#include
+#include
+#include "mcu_periph/input_capture_arch.h"
+#include "mcu_periph/hal_stm32_dma.h"
+#include "mcu_periph/timerDmaCache.h"
+#include "modules/actuators/esc_dshot_config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define DSHOT_DMA_DATA_LEN 16U
+#define DSHOT_DMA_EXTRADATA_LEN 2U
+
+
+#if STM32_DMA_SUPPORTS_DMAMUX == false
+/**
+ * @brief macro helper to design DMA stream
+ * @note does not apply for DMAMUX MCU
+ */
+#define DSHOT_CONCAT_CAPTURE_NX(pre, tim, stream, channel) pre ## tim ## stream , pre ## tim ## channel
+#define DSHOTS_1STREAM(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH1_DMA_STREAM, _CH1_DMA_CHANNEL)
+#define DSHOTS_2STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH1_DMA_STREAM, _CH1_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH2_DMA_STREAM, _CH2_DMA_CHANNEL)
+#define DSHOTS_3STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH1_DMA_STREAM, _CH1_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH2_DMA_STREAM, _CH2_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH3_DMA_STREAM, _CH3_DMA_CHANNEL)
+#define DSHOTS_4STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH1_DMA_STREAM, _CH1_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH2_DMA_STREAM, _CH2_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH3_DMA_STREAM, _CH3_DMA_CHANNEL), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_TIM, tim, _CH4_DMA_STREAM, _CH4_DMA_CHANNEL)
+
+#else // STM32_DMA_SUPPORTS_DMAMUX == true
+/**
+ * @brief macro helper to design DMA stream
+ * @note specific for DMAMUX MCU
+ make use of STM32_DMA_STREAM_ID_ANY auto placement
+ */
+#define DSHOT_CONCAT_CAPTURE_NX(pre, tim, event) {STM32_DMA_STREAM_ID_ANY , pre ## tim ## event}
+#define DSHOTS_1STREAM(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH1)
+#define DSHOTS_2STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH1), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH2)
+#define DSHOTS_3STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH1), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH2), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH3)
+#define DSHOTS_4STREAMS(tim) DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH1), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH2), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH3), \
+ DSHOT_CONCAT_CAPTURE_NX(STM32_DMAMUX1_TIM, tim, _CH4)
+#endif // STM32_DMA_SUPPORTS_DMAMUX == false
+
+#if DSHOT_CHANNELS == 1
+#define DSHOTS_CAPTURE_STREAMS(tim) DSHOTS_1STREAM(tim)
+#elif DSHOT_CHANNELS == 2
+#define DSHOTS_CAPTURE_STREAMS(tim) DSHOTS_2STREAMS(tim)
+#elif DSHOT_CHANNELS == 3
+#define DSHOTS_CAPTURE_STREAMS(tim) DSHOTS_3STREAMS(tim)
+#elif DSHOT_CHANNELS == 4
+#define DSHOTS_CAPTURE_STREAMS(tim) DSHOTS_4STREAMS(tim)
+#else // DSHOT_CHANNELS = X
+#error DSHOT_CHANNELS must be 1 to 4
+#endif // DSHOT_CHANNELS = X
+
+
+#if !defined STM32_SYSCLK && !defined STM32_SYS_CK
+#error neither STM32_SYSCLK or STM32_SYS_CK defined
+#endif
+#if !defined STM32_SYSCLK
+#define STM32_SYSCLK STM32_SYS_CK
+#endif
+
+
+/**
+ * @brief structure defining dma channel
+ * @note does not apply for DMAMUX MCU
+ */
+typedef struct {
+ uint32_t stream;
+#if STM32_DMA_SUPPORTS_DMAMUX
+ uint8_t dmamux;
+#else
+ uint8_t channel;
+#endif
+} DshotDmaStreamChan;
+
+
+/**
+ * @brief DMA capture if (errFrame > 2000) {
+ chSysHalt("p *dshotd.config.dma_capt_cfg.dma_capture");
+ }
+ buffer
+ */
+typedef struct {
+ __attribute__((aligned(32)))
+ uint16_t buf[DSHOT_DMA_DATA_LEN + DSHOT_DMA_EXTRADATA_LEN];
+} DshotRpmCaptureOneChannelDmaBuffer;
+typedef struct {
+ DshotRpmCaptureOneChannelDmaBuffer dma_buf[DSHOT_CHANNELS];
+} DshotRpmCaptureDmaBuffer;
+
+/**
+ * @brief : DSHOT Rpm Capture Driver configuration structure.
+ */
+typedef struct {
+ /**
+ * @brief : GPT Driver to manage microseconds timeout
+ * @note : timeout is too narrow to be managed by RTOS
+ */
+ GPTDriver *gptd;
+ /**
+ * @brief : array of DMA stream for each capture channel
+ */
+ DshotDmaStreamChan dma_streams[DSHOT_CHANNELS];
+ /**
+ * @brief : pointer to the input capture DMA buffer
+ * @note : dma_capture must point to DMA compatible memory
+ * DCACHE disabled section for F7
+ */
+ DshotRpmCaptureDmaBuffer *dma_capture;
+#if __DCACHE_PRESENT
+ /**
+ * @brief DMA memory is in a cached section and need to be flushed
+ */
+ bool dcache_memory_in_use;
+#endif
+} DshotRpmCaptureConfig;
+
+
+typedef struct
+{
+ /**
+ * @brief : pointer to configuration structure
+ * @note : must be valid for the object life
+ */
+ const DshotRpmCaptureConfig *config;
+ /**
+ * @brief : array of erps frames
+ */
+ uint32_t erps[DSHOT_CHANNELS];
+ /**
+ * @brief : input capture timer configuration
+ */
+ TimICConfig icCfg;
+ /**
+ * @brief : input capture timer driver
+ */
+ TimICDriver icd;
+ /**
+ * @brief : dma input capture configuration
+ */
+ DMAConfig dmaCfgs[DSHOT_CHANNELS];
+ /**
+ * @brief : dma input capture drivers
+ */
+ DMADriver dmads[DSHOT_CHANNELS];
+ /**
+ * @brief : cache for timer and dma configuration
+ * @note : cached configuration are restored via memcpy which is
+ * faster than field by field setting
+ */
+ TimerDmaCache cache;
+ /**
+ * @brief : array of rpms
+ */
+ uint32_t rpms[DSHOT_CHANNELS];
+#ifdef DSHOT_STATISTICS
+ /**
+ * @brief : total of time taken to decode erps frame
+ */
+ uint64_t accumDecodeTime;
+ /**
+ * @brief : total number of decoded erps frame
+ */
+ uint32_t nbDecode;
+#endif
+} DshotRpmCapture;
+
+
+
+void dshotRpmCaptureStart(DshotRpmCapture *drcp, const DshotRpmCaptureConfig *cfg,
+ stm32_tim_t *timer);
+void dshotRpmCaptureStop(DshotRpmCapture *drcp);
+void dshotRpmCatchErps(DshotRpmCapture *drcp);
+#if DSHOT_STATISTICS
+void dshotRpmTrace(DshotRpmCapture *drcp, uint8_t index);
+uint32_t dshotRpmGetDmaErr(void) ;
+#endif
+/**
+ * @brief return last collected erps frame
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @param[in] index index of the channel [1 .. 4]
+ * @api
+ */
+static inline uint32_t dshotRpmGetFrame(const DshotRpmCapture *drcp, uint8_t index) {
+ return drcp->rpms[index];
+}
+#if DSHOT_STATISTICS
+/**
+ * @brief return total time to decode erps frame in nanoseconds
+ *
+ * @param[in] drcp pointer to the @p DshotRpmCapture object
+ * @api
+ */
+
+static inline uint32_t dshotRpmGetAverageDecodeTimeNs(DshotRpmCapture *drcp) {
+ return (drcp->accumDecodeTime * 1e9f) / STM32_SYSCLK / drcp->nbDecode;
+}
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
index ea1661eac82..cb6dd3d8266 100644
--- a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
+++ b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
@@ -42,12 +42,6 @@
# \__,_| \___| |_| |_| |_| |_| |_| \__| |_| \___/ |_| |_|
*/
-/** Base freq of DSHOT signal (in kHz)
- * Possible values are: 150, 300, 600
- */
-#ifndef DSHOT_SPEED
-#define DSHOT_SPEED 300U
-#endif
/** Baudrate of the serial link used for telemetry data
* Can depend on the ESC, but only 115k have been used so far
@@ -56,6 +50,10 @@
#define DSHOT_TELEMETRY_BAUD 115200U
#endif
+#ifndef DSHOT_BIDIR_EXTENTED_TELEMETRY
+#define DSHOT_BIDIR_EXTENTED_TELEMETRY FALSE
+#endif
+
/** Telemetry timeout in ms
*/
#ifndef DSHOT_TELEMETRY_TIMEOUT_MS
@@ -88,19 +86,28 @@
#define DSHOT_BIT0_DUTY_RATIO 373U
#endif
-// Some extra defines and macros
-#define DSHOT_FREQ (DSHOT_SPEED*1000) // in Hz
+#if DSHOT_SPEED != 0 // statically defined
+# define DSHOT_FREQ (DSHOT_SPEED*1000)
+# define DSHOT_BIT0_DUTY (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000U)
+# define DSHOT_BIT1_DUTY (DSHOT_BIT0_DUTY*2)
+#else // dynamically defined
+# define DSHOT_FREQ (driver->config->speed_khz * 1000U)
+# define DSHOT_BIT0_DUTY (driver->bit0Duty)
+# define DSHOT_BIT1_DUTY (driver->bit1Duty)
+#endif
#define TICK_FREQ (PWM_FREQ * TICKS_PER_PERIOD)
#define DSHOT_PWM_PERIOD (TICK_FREQ/DSHOT_FREQ)
-#define DSHOT_BIT0_DUTY (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000)
-#define DSHOT_BIT1_DUTY (DSHOT_BIT0_DUTY*2)
-#define DCR_DBL ((DSHOT_CHANNELS-1) << 8) // DSHOT_CHANNELS transfert(s), first register to get is CCR1
+
+
+#define DCR_DBL ((DSHOT_CHANNELS-1) << 8) // DSHOT_CHANNELS transfert(s)
+// first register to get is CCR1
#define DCR_DBA(pwmd) (((uint32_t *) (&pwmd->tim->CCR) - ((uint32_t *) pwmd->tim)))
+#define DSHOT_MAX_VALUE ((1U<<11U)-1U) // 11 bits used to send command, so maximum value is 2047
+
#define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
#define Min(x,y) (x < y ? x : y)
-#define DSHOT_MAX_VALUE ((1<<11)-1) // 11 bits used to send command, so maximum value is 2047
/*
# _ __ _ _ _ _ _ __
@@ -111,13 +118,22 @@
# |_| |_| \___/ \__| \___/ \__| |___/ |_| \___|
*/
static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest);
+static void setCrc4(DshotPacket *dp);
static inline void setDshotPacketThrottle(DshotPacket * const dp, const uint16_t throttle);
static inline void setDshotPacketTlm(DshotPacket * const dp, const bool tlmRequest);
-static void buildDshotDmaBuffer(DshotPackets * const dsp, DshotDmaBuffer * const dma, const size_t timerWidth);
+static void buildDshotDmaBuffer(DSHOTDriver *driver);
+//static void buildDshotDmaBuffer(DshotPackets * const dsp, DshotDmaBuffer * const dma, const size_t timerWidth);
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
-static noreturn void dshotTlmRec(void *arg);
+static noreturn void dshotTlmRec (void *arg);
static size_t getTimerWidth(const PWMDriver *pwmp);
+#if DSHOT_BIDIR
+static void processBidirErpm(DSHOTDriver *driver);
+static void dshotRestart(DSHOTDriver *driver);
+#if DSHOT_BIDIR_EXTENTED_TELEMETRY
+static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm);
+#endif
+#endif
/*
# _ __ _
@@ -137,6 +153,12 @@ static size_t getTimerWidth(const PWMDriver *pwmp);
*/
void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
{
+ chDbgAssert(config->dma_buf != NULL, ".dma_buf must reference valid DshotDmaBuffer object");
+
+#if DSHOT_BIDIR
+ dshotRpmCaptureStart(&driver->rpm_capture, &config->dma_capt_cfg, config->pwmp->tim);
+#endif
+
memset((void *) config->dma_buf, 0, sizeof(*(config->dma_buf)));
const size_t timerWidthInBytes = getTimerWidth(config->pwmp);
@@ -146,6 +168,8 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
.cr2 = USART_CR2_STOP1_BITS, // 1 bit de stop
.cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
};
+ // when dshot is bidir, the polarity is inverted
+ static const uint32_t pwmPolarity = DSHOT_BIDIR ? PWM_OUTPUT_ACTIVE_LOW : PWM_OUTPUT_ACTIVE_HIGH;
driver->config = config;
// use pburst, mburst only if buffer size satisfy aligmnent requirement
@@ -166,7 +190,7 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
#endif
.inc_peripheral_addr = false,
.inc_memory_addr = true,
- .circular = false,
+ .op_mode = DMA_ONESHOT,
.error_cb = NULL,
.end_cb = NULL,
.pburst = 0,
@@ -179,13 +203,13 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
.period = TICKS_PER_PERIOD,
.callback = NULL,
.channels = {
- {.mode = PWM_OUTPUT_ACTIVE_HIGH,
+ {.mode = pwmPolarity,
.callback = NULL},
- {.mode = DSHOT_CHANNELS > 1 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
+ {.mode = DSHOT_CHANNELS > 1 ? pwmPolarity : PWM_OUTPUT_DISABLED,
.callback = NULL},
- {.mode = DSHOT_CHANNELS > 2 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
+ {.mode = DSHOT_CHANNELS > 2 ? pwmPolarity : PWM_OUTPUT_DISABLED,
.callback = NULL},
- {.mode = DSHOT_CHANNELS > 3 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
+ {.mode = DSHOT_CHANNELS > 3 ? pwmPolarity : PWM_OUTPUT_DISABLED,
.callback = NULL},
},
.cr2 = STM32_TIM_CR2_CCDS,
@@ -193,6 +217,14 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
};
driver->crc_errors = 0;
+ driver->tlm_frame_nb = 0;
+#if DSHOT_SPEED == 0
+ driver->bit0Duty = (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000U);
+ driver->bit1Duty = (driver->bit0Duty*2U) ;
+#endif
+
+
+
dmaObjectInit(&driver->dmap);
chMBObjectInit(&driver->mb, driver->_mbBuf, ARRAY_LEN(driver->_mbBuf));
@@ -209,14 +241,53 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
- for (size_t j = 0; j < DSHOT_CHANNELS; j++) {
+ for (size_t j=0; jconfig->pwmp, j, 0);
- driver->dshotMotors.dp[j] = makeDshotPacket(0, 0);
+ driver->dshotMotors.dp[j] = makeDshotPacket(0,0);
+ chMtxObjectInit(&driver->dshotMotors.tlmMtx[j]);
}
driver->dshotMotors.onGoingQry = false;
driver->dshotMotors.currentTlmQry = 0U;
}
+/**
+ * @brief stop the DSHOT driver and free the
+ * related resources : pwm driver and dma driver.
+ *
+ * @param[in] driver pointer to the @p DSHOTDriver object
+ * @api
+ */
+void dshotStop(DSHOTDriver *driver)
+{
+ pwmStop(driver->config->pwmp);
+ dmaStopTransfert(&driver->dmap);
+ dmaStop(&driver->dmap);
+}
+
+#if DSHOT_BIDIR
+/**
+ * @brief restart the driver in outgoing mode
+ *
+ *
+ * @param[in] driver pointer to the @p DSHOTDriver object
+ * @api
+ */
+static void dshotRestart(DSHOTDriver *driver)
+{
+ const bool dmaOk = dmaStart(&driver->dmap, &driver->dma_conf);
+ chDbgAssert(dmaOk == true, "dshot dma start error");
+
+ pwmStart(driver->config->pwmp, &driver->pwm_conf);
+ driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
+ pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
+
+ for (size_t j=0; jconfig->pwmp, j, 0);
+ driver->dshotMotors.dp[j] = makeDshotPacket(0,0);
+ }
+}
+#endif
+
/**
* @brief prepare throttle order for specified ESC
*
@@ -262,16 +333,9 @@ void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
if (specmd > DSHOT_CMD_MAX) {
return; // Don't apply special commands from this function
}
- if (index < DSHOT_CHANNELS) {
- setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
- setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
- } else if (index == DSHOT_ALL_MOTORS) {
- for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
- setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
- setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
- }
- }
+ // some dangerous special commands need to be repeated 6 times
+ // to avoid catastrophic failure
uint8_t repeat;
switch (specmd) {
case DSHOT_CMD_SPIN_DIRECTION_1:
@@ -282,6 +346,8 @@ void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
case DSHOT_CMD_SETTINGS_REQUEST:
case DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF:
case DSHOT_CMD_SILENT_MODE_ON_OFF:
+ case DSHOT_CMD_BIDIR_EDT_MODE_ON:
+ case DSHOT_CMD_BIDIR_EDT_MODE_OFF:
repeat = 10;
break;
default:
@@ -289,8 +355,21 @@ void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
}
while (repeat--) {
+ systime_t now = chVTGetSystemTimeX();
+ if (index < DSHOT_CHANNELS) {
+ setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
+ setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
+ } else if (index == DSHOT_ALL_MOTORS) {
+ for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
+ setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
+ setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
+ }
+ } else {
+ chDbgAssert(false, "dshotSetThrottle index error");
+ }
dshotSendFrame(driver);
- chThdSleepMilliseconds(1);
+ if (repeat)
+ chThdSleepUntilWindowed(now, now + TIME_US2I(500));
}
}
@@ -312,6 +391,9 @@ void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHA
dshotSendFrame(driver);
}
+
+
+
/**
* @brief send throttle order
*
@@ -322,20 +404,31 @@ void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHA
void dshotSendFrame(DSHOTDriver *driver)
{
if (driver->dmap.state == DMA_READY) {
+#if DSHOT_BIDIR
+ const tprio_t currentPrio = chThdSetPriority(HIGHPRIO);
+#endif
if ((driver->config->tlm_sd != NULL) &&
(driver->dshotMotors.onGoingQry == false)) {
driver->dshotMotors.onGoingQry = true;
- const uint32_t index = (driver->dshotMotors.currentTlmQry + 1) % DSHOT_CHANNELS;
- driver->dshotMotors.currentTlmQry = index;
+ const msg_t index = (driver->dshotMotors.currentTlmQry + 1U) % DSHOT_CHANNELS;
+ driver->dshotMotors.currentTlmQry = (uint8_t) index;
setDshotPacketTlm(&driver->dshotMotors.dp[index], true);
- chMBPostTimeout(&driver->mb, driver->dshotMotors.currentTlmQry, TIME_IMMEDIATE);
+ chMBPostTimeout(&driver->mb, index, TIME_IMMEDIATE);
}
- buildDshotDmaBuffer(&driver->dshotMotors, driver->config->dma_buf, getTimerWidth(driver->config->pwmp));
- dmaStartTransfert(&driver->dmap,
+ //buildDshotDmaBuffer(&driver->dshotMotors, driver->config->dma_buf, getTimerWidth(driver->config->pwmp));
+ buildDshotDmaBuffer(driver);
+ dmaTransfert(&driver->dmap,
&driver->config->pwmp->tim->DMAR,
driver->config->dma_buf, DSHOT_DMA_BUFFER_SIZE * DSHOT_CHANNELS);
+#if DSHOT_BIDIR
+ dshotStop(driver);
+ dshotRpmCatchErps(&driver->rpm_capture);
+ processBidirErpm(driver);
+ dshotRestart(driver);
+ chThdSetPriority(currentPrio);
+#endif
}
}
@@ -346,23 +439,140 @@ void dshotSendFrame(DSHOTDriver *driver)
* @return number of CRC errors
* @api
*/
-uint32_t dshotGetCrcErrorsCount(DSHOTDriver *driver)
+uint32_t dshotGetCrcErrorCount(const DSHOTDriver *driver)
{
return driver->crc_errors;
}
+/**
+ * @brief return number of telemetry succesfull frame since dshotStart
+ *
+ * @param[in] driver pointer to the @p DSHOTDriver object
+ * @return number of frames
+ * @api
+ */
+uint32_t dshotGetTelemetryFrameCount(const DSHOTDriver *driver)
+{
+ return driver->tlm_frame_nb;
+}
+
+
/**
* @brief return last received telemetry data
*
* @param[in] driver pointer to the @p DSHOTDriver object
* @param[in] index channel : [0..3] or [0..1] depending on driver used
- * @return pointer on a telemetry structure
+ * @return telemetry structure by copy
* @api
*/
-const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index)
+DshotTelemetry dshotGetTelemetry(DSHOTDriver *driver, uint32_t index)
+{
+ index -= DSHOT_CHANNEL_FIRST_INDEX;
+ chDbgAssert(index < DSHOT_CHANNELS, "dshot index error");
+ chMtxLock(&driver->dshotMotors.tlmMtx[index]);
+ const DshotTelemetry tlm = driver->dshotMotors.dt[index];
+ chMtxUnlock(&driver->dshotMotors.tlmMtx[index]);
+ return tlm;
+}
+
+/*
+
+ [2] 0010 mmmm mmmm - Temperature frame in degree Celsius, just like Blheli_32 and KISS [0, 1, ..., 255]
+ [4] 0100 mmmm mmmm - Voltage frame with a step size of 0,25V [0, 0.25 ..., 63,75]
+ [6] 0110 mmmm mmmm - Current frame with a step size of 1A [0, 1, ..., 255]
+ [8] 1000 mmmm mmmm - Debug frame 1 not associated with any specific value, can be used to debug ESC firmware
+ [10] 1010 mmmm mmmm - Debug frame 2 not associated with any specific value, can be used to debug ESC firmware
+ [12] 1100 mmmm mmmm - Stress level frame [0, 1, ..., 255] (since v2.0.0)
+ [14] 1110 mmmm mmmm - Status frame: Bit[7] = alert event, Bit[6] = warning event, Bit[5] = error event, Bit[3-1] - Max. stress level [0-15] (since v2.0.0)
+
+ */
+#if DSHOT_BIDIR && DSHOT_BIDIR_EXTENTED_TELEMETRY
+static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm)
+{
+ switch(DshotErpsEdtType(erps)) {
+ case EDT_TEMP:
+ tlm->frame.temp = DshotErpsEdtTempCentigrade(erps); break;
+
+ case EDT_VOLT:
+ tlm->frame.voltage = DshotErpsEdtCentiVolts(erps); break;
+
+ case EDT_CURRENT:
+ tlm->frame.current = DshotErpsEdtCurrentAmp(erps) * 100U; break;
+
+ case EDT_STRESS:
+ tlm->stress = DshotErpsEdtStress(erps); break;
+
+ case EDT_STATUS:
+ tlm->status = DshotErpsEdtStatus(erps);break;
+
+ default: {};
+ }
+ tlm->ts = chVTGetSystemTimeX();
+}
+#endif
+
+#if DSHOT_BIDIR
+/**
+ * @brief return last received telemetry ERPM data
+ *
+ * @param[in] driver pointer to the @p DSHOTDriver object
+ * @param[in] index channel : [0..3]
+ * @return eperiod or special values DSHOT_BIDIR_ERR_CRC, DSHOT_BIDIR_TLM_EDT
+ * @note ° special values DSHOT_BIDIR_ERR_CRC, DSHOT_BIDIR_TLM_EDT
+ * must be checked after every call to dshotGetEperiod
+ * ° this fonction is less cpu intensive than dshotGetRpm
+ */
+uint32_t dshotGetEperiod(DSHOTDriver *driver, const uint32_t index)
+{
+ chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
+ DshotErpsSetFromFrame(&driver->erps, driver->rpms_frame[index]);
+ if (DshotErpsCheckCrc4(&driver->erps)) {
+#if DSHOT_BIDIR_EXTENTED_TELEMETRY
+ if (DshotErpsIsEdt(&driver->erps)) {
+ if (driver->config->tlm_sd == NULL) {
+ DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
+ updateTelemetryFromBidirEdt(&driver->erps, tlm);
+ }
+ return DSHOT_BIDIR_TLM_EDT;
+ }
+#endif
+ return DshotErpsGetEperiod(&driver->erps);
+ } else {
+ return DSHOT_BIDIR_ERR_CRC;
+ }
+}
+/**
+ * @brief return last received telemetry ERPM data
+ *
+ * @param[in] driver pointer to the @p DSHOTDriver object
+ * @param[in] index channel : [0..3]
+ * @return erpm or special values DSHOT_BIDIR_ERR_CRC, DSHOT_BIDIR_TLM_EDT
+ * @note special values DSHOT_BIDIR_ERR_CRC, DSHOT_BIDIR_TLM_EDT
+ * must be checked after every call to dshotGetRpm
+ */
+
+uint32_t dshotGetRpm(DSHOTDriver *driver, uint32_t index)
{
- return &driver->dshotMotors.dt[index - DSHOT_CHANNEL_FIRST_INDEX];
+ index -= DSHOT_CHANNEL_FIRST_INDEX;
+ chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
+ DshotErpsSetFromFrame(&driver->erps, driver->rpms_frame[index]);
+ if (DshotErpsCheckCrc4(&driver->erps)) {
+#if DSHOT_BIDIR_EXTENTED_TELEMETRY
+ if (DshotErpsIsEdt(&driver->erps)) {
+ if (driver->config->tlm_sd == NULL) {
+ DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
+ updateTelemetryFromBidirEdt(&driver->erps, tlm);
+ }
+ return DSHOT_BIDIR_TLM_EDT;
+ }
+#endif
+ return DshotErpsGetRpm(&driver->erps);
+ } else {
+ return DSHOT_BIDIR_ERR_CRC;
+ }
}
+#endif
+
/*
@@ -374,6 +584,20 @@ const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_
# |_| |_| |_| \_/ \__,_| \__| \___|
*/
+static void setCrc4(DshotPacket *dp)
+{
+ // compute checksum
+ dp->crc = 0;
+ uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
+ for (int i = 0; i < 3; i++) {
+ dp->crc ^= csum; // xor data by nibbles
+ csum >>= 4;
+ }
+#if DSHOT_BIDIR
+ dp->crc = ~(dp->crc); // crc is inverted when dshot bidir protocol is choosed
+#endif
+}
+
static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmRequest)
{
DshotPacket dp = {.throttle = _throttle,
@@ -381,13 +605,7 @@ static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmReque
.crc = 0
};
- // compute checksum
- uint16_t csum = (_throttle << 1) | dp.telemetryRequest;
- for (int i = 0; i < 3; i++) {
- dp.crc ^= csum; // xor data by nibbles
- csum >>= 4;
- }
-
+ setCrc4(&dp);
return dp;
}
@@ -402,17 +620,16 @@ static inline void setDshotPacketTlm(DshotPacket *const dp, const bool tlmReques
dp->telemetryRequest = tlmRequest ? 1 : 0;
}
-static void buildDshotDmaBuffer(DshotPackets *const dsp, DshotDmaBuffer *const dma, const size_t timerWidth)
+static void buildDshotDmaBuffer(DSHOTDriver *driver)
{
+ DshotPackets *const dsp = &driver->dshotMotors;
+ DshotDmaBuffer *const dma = driver->config->dma_buf;
+ const size_t timerWidth = getTimerWidth(driver->config->pwmp);
+
for (size_t chanIdx = 0; chanIdx < DSHOT_CHANNELS; chanIdx++) {
// compute checksum
- DshotPacket *const dp = &dsp->dp[chanIdx];
- dp->crc = 0;
- uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
- for (int i = 0; i < 3; i++) {
- dp->crc ^= csum; // xor data by nibbles
- csum >>= 4;
- }
+ DshotPacket * const dp = &dsp->dp[chanIdx];
+ setCrc4(dp);
// generate pwm frame
for (size_t bitIdx = 0; bitIdx < DSHOT_BIT_WIDTHS; bitIdx++) {
const uint16_t value = dp->rawFrame &
@@ -432,6 +649,7 @@ static void buildDshotDmaBuffer(DshotPackets *const dsp, DshotDmaBuffer *const d
}
}
+
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
uint8_t crc_u = crc;
@@ -454,6 +672,17 @@ static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
return crc;
}
+
+#if DSHOT_BIDIR
+static void processBidirErpm(DSHOTDriver *driver)
+{
+ for (size_t idx = 0; idx < DSHOT_CHANNELS; idx++) {
+ driver->rpms_frame[idx] = dshotRpmGetFrame(&driver->rpm_capture, idx);
+ }
+}
+#endif
+
+
__attribute__((const))
static size_t getTimerWidth(const PWMDriver *pwmp)
{
@@ -479,28 +708,42 @@ static size_t getTimerWidth(const PWMDriver *pwmp)
# \__| |_| |_| |_| \___| \__,_| \__,_| |___/
*/
+
static noreturn void dshotTlmRec(void *arg)
{
DSHOTDriver *driver = (DSHOTDriver *) arg;
+ DshotTelemetry tlm;
- uint32_t escIdx = 0;
+ msg_t escIdx = 0;
chRegSetThreadName("dshotTlmRec");
while (true) {
- chMBFetchTimeout(&driver->mb, (msg_t *) &escIdx, TIME_INFINITE);
+ chMBFetchTimeout(&driver->mb, &escIdx, TIME_INFINITE);
const uint32_t idx = escIdx;
const bool success =
- (sdReadTimeout(driver->config->tlm_sd, driver->dshotMotors.dt[idx].rawData, sizeof(DshotTelemetry),
- TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetry));
+ (sdReadTimeout(driver->config->tlm_sd, tlm.frame.rawData, sizeof(DshotTelemetryFrame),
+ TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetryFrame));
if (!success ||
- (calculateCrc8(driver->dshotMotors.dt[idx].rawData,
- sizeof(driver->dshotMotors.dt[idx].rawData)) != driver->dshotMotors.dt[idx].crc8)) {
+ (calculateCrc8(tlm.frame.rawData, sizeof(tlm.frame.rawData)) != tlm.frame.crc8)) {
// empty buffer to resync
while (sdGetTimeout(driver->config->tlm_sd, TIME_IMMEDIATE) >= 0) {};
- memset(driver->dshotMotors.dt[idx].rawData, 0U, sizeof(DshotTelemetry));
+ memset(tlm.frame.rawData, 0U, sizeof(DshotTelemetry));
// count errors
- driver->crc_errors++;
+ if (success)
+ driver->crc_errors++;
+ } else {
+ // big-endian to little-endian conversion
+ tlm.frame.voltage = __builtin_bswap16(tlm.frame.voltage);
+ tlm.frame.current = __builtin_bswap16(tlm.frame.current);
+ tlm.frame.consumption = __builtin_bswap16(tlm.frame.consumption);
+ tlm.frame.rpm = __builtin_bswap16(tlm.frame.rpm);
+ tlm.ts = chVTGetSystemTimeX();
+ driver->tlm_frame_nb++;
}
+
+ chMtxLock(&driver->dshotMotors.tlmMtx[idx]);
+ driver->dshotMotors.dt[idx] = tlm;
+ chMtxUnlock(&driver->dshotMotors.tlmMtx[idx]);
driver->dshotMotors.onGoingQry = false;
}
}
diff --git a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.h b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.h
index 77da23d5c26..df735991fdd 100644
--- a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.h
+++ b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.h
@@ -32,14 +32,13 @@
#include
#include
#include "mcu_periph/hal_stm32_dma.h"
-
-/**
- * By default enable the possibility to mix 16 and 32 bits timer
- */
-#ifndef DSHOT_AT_LEAST_ONE_32B_TIMER
-#define DSHOT_AT_LEAST_ONE_32B_TIMER TRUE
+#include "modules/actuators/esc_dshot_config.h"
+#if DSHOT_BIDIR
+#include "modules/actuators/dshot_rpmCapture.h"
+#include "modules/actuators/dshot_erps.h"
#endif
+
#ifndef DSHOT_CHANNEL_FIRST_INDEX
#define DSHOT_CHANNEL_FIRST_INDEX 0U
#endif
@@ -48,12 +47,24 @@
*/
#define DSHOT_BIT_WIDTHS 16U
#define DSHOT_PRE_FRAME_SILENT_SYNC_BITS 2U
-#define DSHOT_POST_FRAME_SILENT_SYNC_BITS 2U
+#define DSHOT_POST_FRAME_SILENT_SYNC_BITS 4U
#define DSHOT_DMA_BUFFER_SIZE (DSHOT_BIT_WIDTHS + \
DSHOT_PRE_FRAME_SILENT_SYNC_BITS + \
DSHOT_POST_FRAME_SILENT_SYNC_BITS )
-#define DSHOT_CHANNELS 4U // depend on the number of channels per timer
+
+#if STM32_DMA_SUPPORTS_DMAMUX
+#define DSHOT_EMIT_STREAM_NX(tim) STM32_DMAMUX1_TIM ## tim ## _UP
+#define DSHOT_EMIT_STREAM(tim) DSHOT_EMIT_STREAM_NX(tim)
+#endif
+
+/**
+ * @brief special values returned by dshotGetRpm function
+ * @note must be checked after each call to dshotGetRpm
+ */
+#define DSHOT_BIDIR_ERR_CRC UINT32_MAX
+#define DSHOT_BIDIR_TLM_EDT (UINT32_MAX-1U)
+
/**
* @brief special value for index : send order to all channels
@@ -62,16 +73,16 @@
*/
#define DSHOT_ALL_MOTORS 255U
-/**
- * @brief Driver state machine possible states.
- */
-typedef enum {
- DSHOT_UNINIT = 0, /**< Not initialized. */
- DSHOT_STOP, /**< Stopped. */
- DSHOT_READY, /**< Ready. */
- DSHOT_ONGOING_TELEMETRY_QUERY, /**< Transfering. */
- DSHOT_ERROR /**< Transfert error. */
-} dshotstate_t;
+// /**
+// * @brief Driver state machine possible states.
+// */
+// typedef enum {
+// DSHOT_UNINIT = 0, /**< Not initialized. */
+// DSHOT_STOP, /**< Stopped. */
+// DSHOT_READY, /**< Ready. */
+// DSHOT_ONGOING_TELEMETRY_QUERY, /**< Transfering. */
+// DSHOT_ERROR /**< Transfert error. */
+// } dshotstate_t;
/*
DshotSettingRequest (KISS24). Spin direction,
@@ -89,7 +100,7 @@ typedef enum {
* @note commands 48-2047 are used to send motor power
*/
typedef enum {
- DSHOT_CMD_MOTOR_STOP = 0,
+ DSHOT_CMD_MOTOR_STOP = 0U,
DSHOT_CMD_BEACON1,
DSHOT_CMD_BEACON2,
DSHOT_CMD_BEACON3,
@@ -102,8 +113,10 @@ typedef enum {
DSHOT_CMD_3D_MODE_ON,
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
DSHOT_CMD_SAVE_SETTINGS,
- DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
- DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
+ DSHOT_CMD_BIDIR_EDT_MODE_ON,
+ DSHOT_CMD_BIDIR_EDT_MODE_OFF,
+ DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20U,
+ DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21U,
DSHOT_CMD_LED0_ON, // BLHeli32 only
DSHOT_CMD_LED1_ON, // BLHeli32 only
DSHOT_CMD_LED2_ON, // BLHeli32 only
@@ -112,9 +125,10 @@ typedef enum {
DSHOT_CMD_LED1_OFF, // BLHeli32 only
DSHOT_CMD_LED2_OFF, // BLHeli32 only
DSHOT_CMD_LED3_OFF, // BLHeli32 only
- DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
- DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
- DSHOT_CMD_MAX = 47
+ DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30U, // KISS audio Stream mode on/Off
+ DSHOT_CMD_SILENT_MODE_ON_OFF = 31U, // KISS silent Mode on/Off
+ DSHOT_CMD_MAX = 47U,
+ DSHOT_MIN_THROTTLE
} dshot_special_commands_t;
/**
@@ -130,11 +144,24 @@ typedef struct {
uint16_t current;
uint16_t consumption;
uint16_t rpm;
- } __attribute__((__packed__, scalar_storage_order("big-endian")));
+ }
+ __attribute__ ((__packed__));
uint8_t rawData[9];
};
uint8_t crc8;
-} __attribute__((__packed__)) DshotTelemetry ;
+} __attribute__ ((__packed__)) DshotTelemetryFrame ;
+
+
+/**
+ * @brief telemetry with timestamp
+ */
+typedef struct {
+ DshotTelemetryFrame frame; // fields shared by serial telemetry and EDT
+ uint8_t stress; // EDT additionnal field
+ uint8_t status; // EDT additionnal field
+ systime_t ts; // timestamp of last succesfull received frame
+} DshotTelemetry ;
+
typedef union {
#if DSHOT_AT_LEAST_ONE_32B_TIMER
@@ -178,7 +205,7 @@ typedef struct {
SerialDriver *tlm_sd;
/**
- * @brief dshot dma buffer, sgould be defined in a non Dcached region
+ * @brief dshot dma buffer, should be defined in a non Dcached region
*/
DshotDmaBuffer *dma_buf;
@@ -189,22 +216,41 @@ typedef struct {
DshotRpmCaptureConfig dma_capt_cfg;
#endif
+#if DSHOT_SPEED == 0
+ /**
+ * @brief dynamic dshot speed, when speed id not known at compilation
+ * @note incompatible with BIDIR
+ */
+ uint16_t speed_khz;
+#endif
+
#if __DCACHE_PRESENT
/**
- * @brief DMA memory is in a cached section and beed to be flushed
+ * @brief DMA memory is in a cached section and need to be flushed
*/
bool dcache_memory_in_use;
#endif
} DSHOTConfig;
+
+
+
+
+
void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config);
+void dshotStop(DSHOTDriver *driver);
void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index, const uint16_t throttle);
void dshotSendFrame(DSHOTDriver *driver);
void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS]);
void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index, const dshot_special_commands_t specmd);
-uint32_t dshotGetCrcErrorsCount(DSHOTDriver *driver);
-const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index);
+uint32_t dshotGetCrcErrorCount(const DSHOTDriver *driver);
+uint32_t dshotGetTelemetryFrameCount(const DSHOTDriver *driver);
+DshotTelemetry dshotGetTelemetry(DSHOTDriver *driver, const uint32_t index);
+#if DSHOT_BIDIR
+uint32_t dshotGetEperiod(DSHOTDriver *driver, const uint32_t index);
+uint32_t dshotGetRpm(DSHOTDriver *driver, const uint32_t index);
+#endif
/*
@@ -218,18 +264,20 @@ const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_
typedef union {
struct {
- uint16_t crc: 4;
- uint16_t telemetryRequest: 1;
- uint16_t throttle: 11;
+ uint16_t crc:4;
+ uint16_t telemetryRequest:1;
+ uint16_t throttle:11;
};
uint16_t rawFrame;
} DshotPacket;
+
typedef struct {
DshotPacket dp[DSHOT_CHANNELS];
DshotTelemetry dt[DSHOT_CHANNELS];
- volatile uint8_t currentTlmQry;
- volatile bool onGoingQry;
+ mutex_t tlmMtx[DSHOT_CHANNELS];
+ volatile bool onGoingQry;
+ uint8_t currentTlmQry;
} DshotPackets;
@@ -272,11 +320,39 @@ struct DSHOTDriver {
*/
uint32_t crc_errors;
+ /**
+ * @brief number of sucessful telemetry frame received
+ */
+ uint32_t tlm_frame_nb;
+
+#if DSHOT_SPEED == 0
+ uint16_t bit0Duty;
+ uint16_t bit1Duty;
+#endif
+#if DSHOT_BIDIR
+ /**
+ * @brief object managing capture of erpm frame
+ * using timer in input capture mode dans one dma stream
+ * for each channels
+ */
+ DshotRpmCapture rpm_capture;
+ /**
+ * @brief object managing decoding of erpm frame
+ */
+ DshotErps erps;
+ /**
+ * @brief half decoded rpms frame
+ */
+ uint32_t rpms_frame[DSHOT_CHANNELS];
+#endif
/**
* @brief stack working area for dshot telemetry thread
*/
THD_WORKING_AREA(waDshotTlmRec, 512);
+ /**
+ * @brief object managing dma control frame for outgoing command
+ */
DshotPackets dshotMotors;
};
diff --git a/sw/airborne/arch/chibios/modules/actuators/esc_dshot_config.h b/sw/airborne/arch/chibios/modules/actuators/esc_dshot_config.h
new file mode 100644
index 00000000000..c2f7d03f93a
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/esc_dshot_config.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#ifndef DSHOT_BIDIR
+#define DSHOT_BIDIR FALSE
+#endif
+
+/**
+ * By default enable the possibility to mix 16 and 32 bits timer
+ */
+#ifndef DSHOT_AT_LEAST_ONE_32B_TIMER
+#define DSHOT_AT_LEAST_ONE_32B_TIMER TRUE
+#endif
+
+#define DSHOT_CHANNELS 4U // depend on the number of channels per timer
+
+
+/** Base freq of DSHOT signal (in kHz)
+ * Possible values are: 150, 300, 600
+ */
+#ifndef DSHOT_SPEED
+#define DSHOT_SPEED 300U
+#endif
+
diff --git a/sw/airborne/boards/tawaki/chibios/v2.0/board.h b/sw/airborne/boards/tawaki/chibios/v2.0/board.h
index 8208aece6d4..2b241ea6198 100644
--- a/sw/airborne/boards/tawaki/chibios/v2.0/board.h
+++ b/sw/airborne/boards/tawaki/chibios/v2.0/board.h
@@ -71,7 +71,7 @@
#define SRVB2 7U
#define PA08 8U
#define USB_VBUS 9U
-#define DSHOT_TLM 10U
+#define DSHOT_RX 10U
#define OTG_FS_DM 11U
#define OTG_FS_DP 12U
#define SWDIO 13U
@@ -259,7 +259,7 @@
#define LINE_SRVB1 PAL_LINE(GPIOA, 6U)
#define LINE_SRVB2 PAL_LINE(GPIOA, 7U)
#define LINE_USB_VBUS PAL_LINE(GPIOA, 9U)
-#define LINE_DSHOT_TLM PAL_LINE(GPIOA, 10U)
+#define LINE_DSHOT_RX PAL_LINE(GPIOA, 10U)
#define LINE_OTG_FS_DM PAL_LINE(GPIOA, 11U)
#define LINE_OTG_FS_DP PAL_LINE(GPIOA, 12U)
#define LINE_SWDIO PAL_LINE(GPIOA, 13U)
@@ -347,7 +347,7 @@
PIN_MODE_ALTERNATE(SRVB2) | \
PIN_MODE_INPUT(PA08) | \
PIN_MODE_INPUT(USB_VBUS) | \
- PIN_MODE_ALTERNATE(DSHOT_TLM) | \
+ PIN_MODE_ALTERNATE(DSHOT_RX) | \
PIN_MODE_ALTERNATE(OTG_FS_DM) | \
PIN_MODE_ALTERNATE(OTG_FS_DP) | \
PIN_MODE_ALTERNATE(SWDIO) | \
@@ -364,7 +364,7 @@
PIN_OTYPE_PUSHPULL(SRVB2) | \
PIN_OTYPE_PUSHPULL(PA08) | \
PIN_OTYPE_OPENDRAIN(USB_VBUS) | \
- PIN_OTYPE_PUSHPULL(DSHOT_TLM) | \
+ PIN_OTYPE_PUSHPULL(DSHOT_RX) | \
PIN_OTYPE_PUSHPULL(OTG_FS_DM) | \
PIN_OTYPE_PUSHPULL(OTG_FS_DP) | \
PIN_OTYPE_PUSHPULL(SWDIO) | \
@@ -381,7 +381,7 @@
PIN_OSPEED_SPEED_HIGH(SRVB2) | \
PIN_OSPEED_SPEED_VERYLOW(PA08) | \
PIN_OSPEED_SPEED_VERYLOW(USB_VBUS) | \
- PIN_OSPEED_SPEED_HIGH(DSHOT_TLM) | \
+ PIN_OSPEED_SPEED_HIGH(DSHOT_RX) | \
PIN_OSPEED_SPEED_HIGH(OTG_FS_DM) | \
PIN_OSPEED_SPEED_HIGH(OTG_FS_DP) | \
PIN_OSPEED_SPEED_HIGH(SWDIO) | \
@@ -394,11 +394,11 @@
PIN_PUPDR_PULLDOWN(AUX_A4) | \
PIN_PUPDR_PULLDOWN(PA04) | \
PIN_PUPDR_FLOATING(SPI6_INTERNAL_CLK) | \
- PIN_PUPDR_FLOATING(SRVB1) | \
- PIN_PUPDR_FLOATING(SRVB2) | \
+ PIN_PUPDR_PULLUP(SRVB1) | \
+ PIN_PUPDR_PULLUP(SRVB2) | \
PIN_PUPDR_PULLDOWN(PA08) | \
PIN_PUPDR_PULLDOWN(USB_VBUS) | \
- PIN_PUPDR_FLOATING(DSHOT_TLM) | \
+ PIN_PUPDR_FLOATING(DSHOT_RX) | \
PIN_PUPDR_FLOATING(OTG_FS_DM) | \
PIN_PUPDR_FLOATING(OTG_FS_DP) | \
PIN_PUPDR_PULLUP(SWDIO) | \
@@ -415,7 +415,7 @@
PIN_ODR_LEVEL_LOW(SRVB2) | \
PIN_ODR_LEVEL_LOW(PA08) | \
PIN_ODR_LEVEL_LOW(USB_VBUS) | \
- PIN_ODR_LEVEL_HIGH(DSHOT_TLM) | \
+ PIN_ODR_LEVEL_HIGH(DSHOT_RX) | \
PIN_ODR_LEVEL_HIGH(OTG_FS_DM) | \
PIN_ODR_LEVEL_HIGH(OTG_FS_DP) | \
PIN_ODR_LEVEL_HIGH(SWDIO) | \
@@ -433,7 +433,7 @@
#define VAL_GPIOA_AFRH (PIN_AFIO_AF(PA08, 0) | \
PIN_AFIO_AF(USB_VBUS, 0) | \
- PIN_AFIO_AF(DSHOT_TLM, 7) | \
+ PIN_AFIO_AF(DSHOT_RX, 7) | \
PIN_AFIO_AF(OTG_FS_DM, 10) | \
PIN_AFIO_AF(OTG_FS_DP, 10) | \
PIN_AFIO_AF(SWDIO, 0) | \
@@ -491,8 +491,8 @@
PIN_OSPEED_SPEED_HIGH(SPI2_EXTERNAL_MISO) | \
PIN_OSPEED_SPEED_HIGH(SPI2_EXTERNAL_MOSI))
-#define VAL_GPIOB_PUPDR (PIN_PUPDR_FLOATING(SRVB3) | \
- PIN_PUPDR_FLOATING(SRVB4) | \
+#define VAL_GPIOB_PUPDR (PIN_PUPDR_PULLUP(SRVB3) | \
+ PIN_PUPDR_PULLUP(SRVB4) | \
PIN_PUPDR_PULLDOWN(PB02) | \
PIN_PUPDR_FLOATING(UART7_RX) | \
PIN_PUPDR_FLOATING(SPI6_INTERNAL_MISO) | \
@@ -1476,8 +1476,8 @@
#define AF_LINE_SRVB1 2U
#define AF_SRVB2 2U
#define AF_LINE_SRVB2 2U
-#define AF_DSHOT_TLM 7U
-#define AF_LINE_DSHOT_TLM 7U
+#define AF_DSHOT_RX 7U
+#define AF_LINE_DSHOT_RX 7U
#define AF_OTG_FS_DM 10U
#define AF_LINE_OTG_FS_DM 10U
#define AF_OTG_FS_DP 10U
diff --git a/sw/airborne/boards/tawaki/chibios/v2.0/mcuconf.h b/sw/airborne/boards/tawaki/chibios/v2.0/mcuconf.h
index ff537545462..bb526e6e748 100644
--- a/sw/airborne/boards/tawaki/chibios/v2.0/mcuconf.h
+++ b/sw/airborne/boards/tawaki/chibios/v2.0/mcuconf.h
@@ -274,12 +274,32 @@
#define STM32_GPT_USE_TIM2 FALSE
#define STM32_GPT_USE_TIM3 FALSE
#define STM32_GPT_USE_TIM4 FALSE
+#if USE_GPT5
+#define STM32_GPT_USE_TIM5 TRUE
+#else
#define STM32_GPT_USE_TIM5 FALSE
+#endif
#define STM32_GPT_USE_TIM6 TRUE
+#if USE_GPT7
+#define STM32_GPT_USE_TIM7 TRUE
+#else
#define STM32_GPT_USE_TIM7 FALSE
+#endif
+#if USE_GPT8
+#define STM32_GPT_USE_TIM8 TRUE
+#else
#define STM32_GPT_USE_TIM8 FALSE
-#define STM32_GPT_USE_TIM12 FALSE
-#define STM32_GPT_USE_TIM13 FALSE
+#endif
+#if USE_GPT12
+#define STM32_GPT_USE_TIM12 TRUE
+#else
+#define STM32_GPT_USE_TIM12 FALSE
+#endif
+#if USE_GPT13
+#define STM32_GPT_USE_TIM13 TRUE
+#else
+#define STM32_GPT_USE_TIM13 FALSE
+#endif
#define STM32_GPT_USE_TIM14 FALSE
#define STM32_GPT_USE_TIM15 FALSE
#define STM32_GPT_USE_TIM16 FALSE
@@ -394,7 +414,7 @@
/*
* SERIAL driver system settings.
*/
-#define STM32_SERIAL_USE_USART1 FALSE
+#define STM32_SERIAL_USE_USART1 TRUE // enabled by default for dshot telemetry
#if USE_UART2
#define STM32_SERIAL_USE_USART2 TRUE
#else
diff --git a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_2.0.cfg b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_2.0.cfg
index dff72475fe2..ab050d294d0 100644
--- a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_2.0.cfg
+++ b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_2.0.cfg
@@ -92,16 +92,16 @@ PA01 AUX_A2 PASSIVE (AF:USART2_RTS, AF:TIM2_CH2, ADC1_INP17)
PA02 AUX_A3 PASSIVE (AF:TIM15_CH1)
PA03 AUX_A4 PASSIVE (AF:TIM15_CH2)
PA05 SPI6_INTERNAL_CLK SPI AF:SPI6_SCK
-PA06 SRVB1 PWM AF:TIM3_CH1 ()
-PA07 SRVB2 PWM AF:TIM3_CH2 ()
+PA06 SRVB1 PWM AF:TIM3_CH1 PULLUP ()
+PA07 SRVB2 PWM AF:TIM3_CH2 PULLUP ()
PA09 USB_VBUS INPUT PULLDOWN
-PA10 DSHOT_TLM UART AF:USART1_RX
+PA10 DSHOT_RX UART AF:USART1_RX
PA11 OTG_FS_DM OTG AF:USB_OTG_FS_DM
PA12 OTG_FS_DP OTG AF:USB_OTG_FS_DP
PA15 UART7_TX UART AF:UART7_TX
-PB00 SRVB3 PWM AF:TIM3_CH3 ()
-PB01 SRVB4 PWM AF:TIM3_CH4 ()
+PB00 SRVB3 PWM AF:TIM3_CH3 PULLUP ()
+PB01 SRVB4 PWM AF:TIM3_CH4 PULLUP ()
PB03 UART7_RX UART AF:UART7_RX
PB04 SPI6_INTERNAL_MISO SPI AF:SPI6_MISO
PB05 SPI6_INTERNAL_MOSI SPI AF:SPI6_MOSI
diff --git a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h
index fbcf5ec02b5..82dd59a60f2 100644
--- a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h
+++ b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h
@@ -391,14 +391,17 @@
#endif
// macros for possible dshot telemetry
-#define DSHOT_TLM_RX 5
+#define DSHOT_TLM_RX 1
#define DSHOT_TLM_AUX_RX 4
-#ifndef USE_DSHOT_TIM4
-#define USE_DSHOT_TIM4 1 // use SRVb for DShot by default
+#ifndef USE_DSHOT_TIM3
+#define USE_DSHOT_TIM3 1 // use SRVb for DShot by default
#endif
-#if USE_DSHOT_TIM4 // Servo B1, B2, B3, B4 on TIM4
+
+
+
+#if USE_DSHOT_TIM3 // Servo B1, B2, B3, B4 on TIM4
// Servo B1, B2, B3, B4 on TM4 are primary DSHOT connector
#define DSHOT_SERVO_1 1
@@ -430,13 +433,29 @@
#define DSHOT_SERVO_4_CHANNEL SRVB4_TIM_CH
#define DSHOT_CONF_TIM3 1
-#define DSHOT_CONF3_DEF { \
- .dma_stream = STM32_DMA_STREAM_ID_ANY, \
- .dmamux = STM32_DMAMUX1_TIM3_UP, \
- .pwmp = &PWMD3, \
- .tlm_sd = DSHOT_TIM3_TELEMETRY_DEV, \
- .dma_buf = &dshot3DmaBuffer, \
- .dcache_memory_in_use = false \
+
+
+#if DSHOT_BIDIR
+
+#define DSHOT_CAPT_CONF3_DEF \
+ .dma_capt_cfg = { \
+ .gptd = &DSHOT1_BIDIR_GPT, \
+ .dma_streams = {DSHOTS_CAPTURE_STREAMS(3)}, \
+ .dma_capture = &dshot3DmaCaptureBuffer, \
+ .dcache_memory_in_use = false, \
+ },
+#else
+#define DSHOT_CAPT_CONF3_DEF
+#endif
+
+#define DSHOT_CONF3_DEF { \
+ .dma_stream = STM32_DMA_STREAM_ID_ANY, \
+ .dmamux = STM32_DMAMUX1_TIM3_UP, \
+ .pwmp = &PWMD3, \
+ .tlm_sd = DSHOT_TIM3_TELEMETRY_DEV, \
+ .dma_buf = &dshot3DmaBuffer, \
+ DSHOT_CAPT_CONF3_DEF \
+ .dcache_memory_in_use = false \
}
#endif
@@ -472,13 +491,30 @@
#define DSHOT_SERVO_8_CHANNEL SRVA4_TIM_CH
#define DSHOT_CONF_TIM1 1
-#define DSHOT_CONF1_DEF { \
- .dma_stream = STM32_DMA_STREAM_ID_ANY, \
- .dmamux = STM32_DMAMUX1_TIM1_UP, \
- .pwmp = &PWMD1, \
- .tlm_sd = DSHOT_TIM1_TELEMETRY_DEV, \
- .dma_buf = &dshot1DmaBuffer, \
- .dcache_memory_in_use = false \
+
+#if DSHOT_BIDIR
+#ifndef DSHOT2_BIDIR_GPT
+#error "Select and activate DSHOT2_BIDIR_GPT for DSHOT_CONF1. Example: USE_GPT8=TRUE, DSHOT2_BIDIR_GPT=GPTD8"
+#endif
+#define DSHOT_CAPT_CONF1_DEF \
+ .dma_capt_cfg = { \
+ .gptd = &DSHOT2_BIDIR_GPT, \
+ .dma_streams = {DSHOTS_CAPTURE_STREAMS(1)}, \
+ .dma_capture = &dshot1DmaCaptureBuffer, \
+ .dcache_memory_in_use = false, \
+ },
+#else
+#define DSHOT_CAPT_CONF1_DEF
+#endif
+
+#define DSHOT_CONF1_DEF { \
+ .dma_stream = STM32_DMA_STREAM_ID_ANY, \
+ .dmamux = STM32_DMAMUX1_TIM1_UP, \
+ .pwmp = &PWMD1, \
+ .tlm_sd = DSHOT_TIM1_TELEMETRY_DEV, \
+ .dma_buf = &dshot1DmaBuffer, \
+ DSHOT_CAPT_CONF1_DEF \
+ .dcache_memory_in_use = false \
}
#endif
@@ -523,12 +559,12 @@
#define UART4_GPIO_AF AUX_A1_UART_AF
/**
- * UART5 on SRVB (DSHOT telemetry)
+ * UART1 on SRVB (DSHOT telemetry)
*/
-#define UART5_GPIO_PORT_RX PAL_PORT(LINE_DSHOT_RX)
-#define UART5_GPIO_RX PAL_PAD(LINE_DSHOT_RX)
-#define UART5_GPIO_AF AF_DSHOT_RX
+#define UART1_GPIO_PORT_RX PAL_PORT(LINE_DSHOT_RX)
+#define UART1_GPIO_RX PAL_PAD(LINE_DSHOT_RX)
+#define UART1_GPIO_AF AF_DSHOT_RX
/**
* SBUS / Spektrum port