diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index 37b9b113a8..94e355a184 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -202,6 +202,7 @@
+
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 e178d9ebcb..ef88ce8ea2 100644
--- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
+++ b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
@@ -27,11 +27,23 @@
#include "modules/actuators/actuators_dshot.h"
#include "modules/actuators/esc_dshot.h"
+#include "modules/core/abi.h"
#include "mcu_periph/ram_arch.h"
#include "mcu_periph/gpio.h"
#include BOARD_CONFIG
-uint16_t actuators_dshot_values[ACTUATORS_DSHOT_NB];
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+#include "modules/energy/electrical.h"
+#endif
+
+struct dshot_private {
+ DSHOTDriver *driver;
+ uint32_t channel;
+};
+
+struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB];
+struct dshot_private actuators_dshot_private[ACTUATORS_DSHOT_NB];
#if DSHOT_CONF_TIM1
static DSHOTDriver DSHOTD1;
@@ -69,47 +81,153 @@ static IN_DMA_SECTION_NOINIT(DshotDmaBuffer dshot9DmaBuffer);
static DSHOTConfig dshotcfg9 = DSHOT_CONF9_DEF;
#endif
+#if PERIODIC_TELEMETRY
+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);
+
+ 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;
+ pprz_msg_send_ESC(trans, dev, AC_ID,
+ &actuators_dshot_values[i].current,
+ &bat_voltage,
+ &power,
+ &actuators_dshot_values[i].rpm,
+ &actuators_dshot_values[i].voltage,
+ &energy,
+ &i);
+ }
+ }
+}
+#endif
+
+static void dshot_init_struct(struct dshot * ds)
+{
+ ds->cmd = 0;
+ ds->rpm = 0;
+ ds->current = 0;
+ ds->voltage = 0;
+ ds->activated = false;
+}
+
+static void dshot_set_struct(struct dshot * ds, struct dshot_private * dsp, DSHOTDriver * driver, uint32_t channel)
+{
+ ds->activated = true;
+ dsp->driver = driver;
+ dsp->channel = channel;
+}
+
+#define _CONCAT_GPIO(num, name) UART ## num ## _GPIO_ ## name
+#define CONCAT_GPIO(num, name) _CONCAT_GPIO(num, name)
void actuators_dshot_arch_init(void)
{
+ // init dshot structure to zero
+ for (int i = 0; i < ACTUATORS_DSHOT_NB; i++) {
+ dshot_init_struct(&actuators_dshot_values[i]);
+ }
+
+ // configure telemetry pin if needed
+ // the serial device interface might have to be activated
+ // by hand (ChibiOS HAL), but PPRZ one disabled
+#ifdef DSHOT_TIM1_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM1_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM1_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM1_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM2_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM2_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM2_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM2_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM3_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM3_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM3_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM3_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM4_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM4_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM4_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM4_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM5_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM5_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM5_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM5_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM8_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM8_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM8_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM8_TELEMETRY_NUM, AF), FALSE);
+#endif
+#ifdef DSHOT_TIM9_TELEMETRY_NUM
+ gpio_setup_pin_af(
+ CONCAT_GPIO(DSHOT_TIM9_TELEMETRY_NUM, PORT_RX),
+ CONCAT_GPIO(DSHOT_TIM9_TELEMETRY_NUM, RX),
+ CONCAT_GPIO(DSHOT_TIM9_TELEMETRY_NUM, AF), FALSE);
+#endif
+
/*----------------
* Configure GPIO
*----------------*/
#ifdef DSHOT_SERVO_0
gpio_setup_pin_af(DSHOT_SERVO_0_GPIO, DSHOT_SERVO_0_PIN, DSHOT_SERVO_0_AF, true);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ dshot_set_struct(&actuators_dshot_values[DSHOT_SERVO_11], &actuators_dshot_private[DSHOT_SERVO_11], &DSHOT_SERVO_11_DRIVER, DSHOT_SERVO_11_CHANNEL);
#endif
/*---------------
@@ -136,46 +254,50 @@ void actuators_dshot_arch_init(void)
#if DSHOT_CONF_TIM9
dshotStart(&DSHOTD9, &dshotcfg9);
#endif
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, esc_msg_send);
+#endif
}
void actuators_dshot_arch_commit(void)
{
#ifdef DSHOT_SERVO_0
- dshotSetThrottle(&DSHOT_SERVO_0_DRIVER, DSHOT_SERVO_0_CHANNEL, actuators_dshot_values[DSHOT_SERVO_0]);
+ dshotSetThrottle(&DSHOT_SERVO_0_DRIVER, DSHOT_SERVO_0_CHANNEL, actuators_dshot_values[DSHOT_SERVO_0].cmd);
#endif
#ifdef DSHOT_SERVO_1
- dshotSetThrottle(&DSHOT_SERVO_1_DRIVER, DSHOT_SERVO_1_CHANNEL, actuators_dshot_values[DSHOT_SERVO_1]);
+ dshotSetThrottle(&DSHOT_SERVO_1_DRIVER, DSHOT_SERVO_1_CHANNEL, actuators_dshot_values[DSHOT_SERVO_1].cmd);
#endif
#ifdef DSHOT_SERVO_2
- dshotSetThrottle(&DSHOT_SERVO_2_DRIVER, DSHOT_SERVO_2_CHANNEL, actuators_dshot_values[DSHOT_SERVO_2]);
+ dshotSetThrottle(&DSHOT_SERVO_2_DRIVER, DSHOT_SERVO_2_CHANNEL, actuators_dshot_values[DSHOT_SERVO_2].cmd);
#endif
#ifdef DSHOT_SERVO_3
- dshotSetThrottle(&DSHOT_SERVO_3_DRIVER, DSHOT_SERVO_3_CHANNEL, actuators_dshot_values[DSHOT_SERVO_3]);
+ dshotSetThrottle(&DSHOT_SERVO_3_DRIVER, DSHOT_SERVO_3_CHANNEL, actuators_dshot_values[DSHOT_SERVO_3].cmd);
#endif
#ifdef DSHOT_SERVO_4
- dshotSetThrottle(&DSHOT_SERVO_4_DRIVER, DSHOT_SERVO_4_CHANNEL, actuators_dshot_values[DSHOT_SERVO_4]);
+ dshotSetThrottle(&DSHOT_SERVO_4_DRIVER, DSHOT_SERVO_4_CHANNEL, actuators_dshot_values[DSHOT_SERVO_4].cmd);
#endif
#ifdef DSHOT_SERVO_5
- dshotSetThrottle(&DSHOT_SERVO_5_DRIVER, DSHOT_SERVO_5_CHANNEL, actuators_dshot_values[DSHOT_SERVO_5]);
+ dshotSetThrottle(&DSHOT_SERVO_5_DRIVER, DSHOT_SERVO_5_CHANNEL, actuators_dshot_values[DSHOT_SERVO_5].cmd);
#endif
#ifdef DSHOT_SERVO_6
- dshotSetThrottle(&DSHOT_SERVO_6_DRIVER, DSHOT_SERVO_6_CHANNEL, actuators_dshot_values[DSHOT_SERVO_6]);
+ dshotSetThrottle(&DSHOT_SERVO_6_DRIVER, DSHOT_SERVO_6_CHANNEL, actuators_dshot_values[DSHOT_SERVO_6].cmd);
#endif
#ifdef DSHOT_SERVO_7
- dshotSetThrottle(&DSHOT_SERVO_7_DRIVER, DSHOT_SERVO_7_CHANNEL, actuators_dshot_values[DSHOT_SERVO_7]);
+ dshotSetThrottle(&DSHOT_SERVO_7_DRIVER, DSHOT_SERVO_7_CHANNEL, actuators_dshot_values[DSHOT_SERVO_7].cmd);
#endif
#ifdef DSHOT_SERVO_8
- dshotSetThrottle(&DSHOT_SERVO_8_DRIVER, DSHOT_SERVO_8_CHANNEL, actuators_dshot_values[DSHOT_SERVO_8]);
+ dshotSetThrottle(&DSHOT_SERVO_8_DRIVER, DSHOT_SERVO_8_CHANNEL, actuators_dshot_values[DSHOT_SERVO_8].cmd);
#endif
#ifdef DSHOT_SERVO_9
- dshotSetThrottle(&DSHOT_SERVO_9_DRIVER, DSHOT_SERVO_9_CHANNEL, actuators_dshot_values[DSHOT_SERVO_9]);
+ dshotSetThrottle(&DSHOT_SERVO_9_DRIVER, DSHOT_SERVO_9_CHANNEL, actuators_dshot_values[DSHOT_SERVO_9].cmd);
#endif
#ifdef DSHOT_SERVO_10
- dshotSetThrottle(&DSHOT_SERVO_10_DRIVER, DSHOT_SERVO_10_CHANNEL, actuators_dshot_values[DSHOT_SERVO_10]);
+ dshotSetThrottle(&DSHOT_SERVO_10_DRIVER, DSHOT_SERVO_10_CHANNEL, actuators_dshot_values[DSHOT_SERVO_10].cmd);
#endif
#ifdef DSHOT_SERVO_11
- dshotSetThrottle(&DSHOT_SERVO_11_DRIVER, DSHOT_SERVO_11_CHANNEL, actuators_dshot_values[DSHOT_SERVO_11]);
+ dshotSetThrottle(&DSHOT_SERVO_11_DRIVER, DSHOT_SERVO_11_CHANNEL, actuators_dshot_values[DSHOT_SERVO_11].cmd);
#endif
#if DSHOT_CONF_TIM1
@@ -199,4 +321,13 @@ void actuators_dshot_arch_commit(void)
#if DSHOT_CONF_TIM9
dshotSendFrame(&DSHOTD9);
#endif
+
+ uint16_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 };
+ 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);
+ rpm_list[i] = dtelem->rpm;
+ }
+ }
+ AbiSendMsgRPM(RPM_DSHOT_ID, rpm_list, ACTUATORS_DSHOT_NB);
}
diff --git a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
index eeab3373a7..146a727695 100644
--- a/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
+++ b/sw/airborne/arch/chibios/modules/actuators/esc_dshot.c
@@ -126,9 +126,9 @@ void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
static const SerialConfig tlmcfg = {
.speed = DSHOT_TELEMETRY_BAUD,
- .cr1 = 0, // pas de parité
+ .cr1 = 0, // pas de parité
.cr2 = USART_CR2_STOP1_BITS, // 1 bit de stop
- .cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
+ .cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
};
driver->config = config;
@@ -341,7 +341,7 @@ uint32_t dshotGetCrcErrorsCount(DSHOTDriver *driver)
*/
const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index)
{
- return &driver->dshotMotors.dt[index];
+ return &driver->dshotMotors.dt[index - DSHOT_CHANNEL_FIRST_INDEX];
}
diff --git a/sw/airborne/boards/tawaki/chibios/common/mcuconf.h b/sw/airborne/boards/tawaki/chibios/common/mcuconf.h
index fc4d8db7a9..f3f86582b4 100644
--- a/sw/airborne/boards/tawaki/chibios/common/mcuconf.h
+++ b/sw/airborne/boards/tawaki/chibios/common/mcuconf.h
@@ -364,46 +364,65 @@
/*
* SERIAL driver system settings.
*/
+#ifndef STM32_SERIAL_USE_USART1
#if USE_UART1
#define STM32_SERIAL_USE_USART1 TRUE
#else
#define STM32_SERIAL_USE_USART1 FALSE
#endif
+#endif
+
+#ifndef STM32_SERIAL_USE_USART2
#if USE_UART2
#define STM32_SERIAL_USE_USART2 TRUE
#else
#define STM32_SERIAL_USE_USART2 FALSE
#endif
+#endif
+
+#ifndef STM32_SERIAL_USE_USART3
#if USE_UART3
#define STM32_SERIAL_USE_USART3 TRUE
#else
#define STM32_SERIAL_USE_USART3 FALSE
#endif
+#endif
+
+#ifndef STM32_SERIAL_USE_UART4
#if USE_UART4
#define STM32_SERIAL_USE_UART4 TRUE
#else
#define STM32_SERIAL_USE_UART4 FALSE
#endif
-#if USE_UART5
-#define STM32_SERIAL_USE_UART5 TRUE
-#else
-#define STM32_SERIAL_USE_UART5 FALSE
#endif
+
+#ifndef STM32_SERIAL_USE_UART5
+#define STM32_SERIAL_USE_UART5 TRUE // enabled by default for dshot telemetry
+#endif
+
+#ifndef STM32_SERIAL_USE_USART6
#if USE_UART6
#define STM32_SERIAL_USE_USART6 TRUE
#else
#define STM32_SERIAL_USE_USART6 FALSE
#endif
+#endif
+
+#ifndef STM32_SERIAL_USE_UART7
#if USE_UART7
#define STM32_SERIAL_USE_UART7 TRUE
#else
#define STM32_SERIAL_USE_UART7 FALSE
#endif
+#endif
+
+#ifndef STM32_SERIAL_USE_UART8
#if USE_UART8
#define STM32_SERIAL_USE_UART8 TRUE
#else
#define STM32_SERIAL_USE_UART8 FALSE
#endif
+#endif
/*
* SPI driver system settings.
diff --git a/sw/airborne/boards/tawaki/chibios/common/tawaki.h b/sw/airborne/boards/tawaki/chibios/common/tawaki.h
index 813311171a..cd9bd87d59 100644
--- a/sw/airborne/boards/tawaki/chibios/common/tawaki.h
+++ b/sw/airborne/boards/tawaki/chibios/common/tawaki.h
@@ -378,10 +378,22 @@
/**
* DSHOT
*/
-#ifndef DSHOT_TELEMETRY_DEV
-#define DSHOT_TELEMETRY_DEV NULL
+#ifdef DSHOT_TIM4_TELEMETRY_NUM
+#define DSHOT_TIM4_TELEMETRY_DEV CONCAT_BOARD_PARAM(&SD, DSHOT_TIM4_TELEMETRY_NUM)
+#else
+#define DSHOT_TIM4_TELEMETRY_DEV NULL
#endif
+#ifdef DSHOT_TIM1_TELEMETRY_NUM
+#define DSHOT_TIM1_TELEMETRY_DEV CONCAT_BOARD_PARAM(&SD, DSHOT_TIM1_TELEMETRY_NUM)
+#else
+#define DSHOT_TIM1_TELEMETRY_DEV NULL
+#endif
+
+// macros for possible dshot telemetry
+#define DSHOT_TLM_RX 5
+#define DSHOT_TLM_AUX_RX 4
+
#ifndef USE_DSHOT_TIM4
#define USE_DSHOT_TIM4 1 // use SRVb for DShot by default
#endif
@@ -422,7 +434,7 @@
.dma_stream = STM32_PWM4_UP_DMA_STREAM, \
.dma_channel = STM32_PWM4_UP_DMA_CHANNEL, \
.pwmp = &PWMD4, \
- .tlm_sd = DSHOT_TELEMETRY_DEV, \
+ .tlm_sd = DSHOT_TIM4_TELEMETRY_DEV, \
.dma_buf = &dshot4DmaBuffer, \
.dcache_memory_in_use = false \
}
@@ -464,7 +476,7 @@
.dma_stream = STM32_PWM1_UP_DMA_STREAM, \
.dma_channel = STM32_PWM1_UP_DMA_CHANNEL, \
.pwmp = &PWMD1, \
- .tlm_sd = DSHOT_TELEMETRY_DEV, \
+ .tlm_sd = DSHOT_TIM1_TELEMETRY_DEV, \
.dma_buf = &dshot1DmaBuffer, \
.dcache_memory_in_use = false \
}
@@ -510,6 +522,14 @@
#define UART4_GPIO_RX PAL_PAD(LINE_AUX_A2)
#define UART4_GPIO_AF AUX_A1_UART_AF
+/**
+ * UART5 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
+
/**
* SBUS / Spektrum port
*
diff --git a/sw/airborne/modules/actuators/actuators_dshot.h b/sw/airborne/modules/actuators/actuators_dshot.h
index e5b4afbb3b..c42c659fe1 100644
--- a/sw/airborne/modules/actuators/actuators_dshot.h
+++ b/sw/airborne/modules/actuators/actuators_dshot.h
@@ -46,7 +46,15 @@
#define ACTUATORS_DSHOT_NB (8+1)
#endif
-extern uint16_t actuators_dshot_values[ACTUATORS_DSHOT_NB];
+struct dshot {
+ float cmd; ///< command
+ float rpm; ///< rpm
+ float current; ///< current
+ float voltage; ///< motor current
+ bool activated; ///< current dshot channel is activated
+};
+
+extern struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB];
/** Arch dependent init
*/
@@ -55,8 +63,8 @@ extern void actuators_dshot_arch_commit(void);
/* Actuator macros */
#define ActuatorDShotSet(_i, _v) { \
- if (_v == 0) { actuators_dshot_values[_i] = 0; } \
- else { actuators_dshot_values[_i] = _v + ACTUATORS_DSHOT_OFFSET; } \
+ if (_v == 0) { actuators_dshot_values[_i].cmd = 0; } \
+ else { actuators_dshot_values[_i].cmd = _v + ACTUATORS_DSHOT_OFFSET; } \
}
#define ActuatorsDShotInit() actuators_dshot_arch_init()
#define ActuatorsDShotCommit() actuators_dshot_arch_commit()
diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h
index 93a8e8fb74..e76318ce96 100644
--- a/sw/airborne/modules/core/abi_sender_ids.h
+++ b/sw/airborne/modules/core/abi_sender_ids.h
@@ -463,6 +463,10 @@
#define RPM_SENSOR_ID 1
#endif
+#ifndef RPM_DSHOT_ID
+#define RPM_DSHOT_ID 2
+#endif
+
/*
* IDs of THRUST increment calculation (message 16)
*/