diff --git a/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp b/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp new file mode 100644 index 0000000000..d4ba7cffd2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp @@ -0,0 +1,141 @@ +/* + example outputting DShot on 4 TIM1 channels on fmuv4 board + */ + +#include +#include +#include "ch.h" +#include "hal.h" +#include "hwdef.h" + +void setup(); +void loop(); + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +// use TIM1_CH1 to TIM1_CH4 +#define DMA_STREAM STM32_DMA_STREAM_ID(2,5) // TIM1_UP DMA +#define PWMD PWMD1 +#define DMA_CH 6 // TIM1_UP is on channel 6 + +// choose DShot rate. Can be 150, 300, 600 or 1200 +#define DSHOT_RATE 600U +#define DSHOT_BIT_PERIOD 19U + +#define DSHOT_PWM_FREQUENCY (DSHOT_RATE * 1000U * DSHOT_BIT_PERIOD) + +#define DSHOT_BIT_LENGTH 18 // includes two reset bits + +// pulse width for 0 and 1 bits +#define DSHOT_MOTOR_BIT_0 7 +#define DSHOT_MOTOR_BIT_1 14 + +static uint32_t *buffer; +static uint16_t buffer_length = DSHOT_BIT_LENGTH*4*sizeof(uint32_t); +const stm32_dma_stream_t *dma; + +/* + setup pwm for all 4 channels enabled + */ +static const PWMConfig pwm_config = { + .frequency = DSHOT_PWM_FREQUENCY, + .period = DSHOT_BIT_PERIOD, + .callback = NULL, + .channels = { + {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, + {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, + {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, + {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, + }, + .cr2 = 0, + .dier = TIM_DIER_UDE, // DMA on update event for next period +}; + + +void setup(void) { + hal.console->printf("Starting DShot test\n"); + dma = STM32_DMA_STREAM(DMA_STREAM); + buffer = (uint32_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE); +} + + +/* + create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight + */ +static uint16_t create_dshot_packet(const uint16_t value) +{ + uint16_t packet = (value << 1); // no telemetry request + + // compute checksum + uint16_t csum = 0; + uint16_t csum_data = packet; + for (uint8_t i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + return packet; +} + +/* + fill in a DMA buffer for dshot + */ +static void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet) +{ + for (int i = 0; i < 16; i++) { + buffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; + packet <<= 1; + } +} + +static void test_dshot_send(void) +{ + uint16_t packets[4]; + for (uint8_t i=0; i<4; i++) { + packets[i] = create_dshot_packet(200 * (i+1)); + fill_DMA_buffer_dshot(buffer + i, 4, packets[i]); + } + + dmaStreamAllocate(dma, 10, NULL, NULL); + dmaStreamSetPeripheral(dma, &(PWMD.tim->DMAR)); + dmaStreamSetMemory0(dma, buffer); + dmaStreamSetTransactionSize(dma, buffer_length/sizeof(uint32_t)); + dmaStreamSetFIFO(dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL); + dmaStreamSetMode(dma, + STM32_DMA_CR_CHSEL(DMA_CH) | + STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | + STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3)); + + pwmStart(&PWMD, &pwm_config); + for (uint8_t i=0; i<4; i++) { + pwmEnableChannel(&PWMD, i, 0); + } + + // setup for 4 burst strided transfers. 0x0D is the register + // address offset of the CCR registers in the timer peripheral + PWMD.tim->DCR = 0x0D | STM32_TIM_DCR_DBL(3); + + dmaStreamEnable(dma); +} + +static void test_dshot_cleanup(void) +{ + pwmStop(&PWMD); + dmaStreamRelease(dma); +} + +void loop(void) +{ + hal.console->printf("."); + test_dshot_send(); + hal.scheduler->delay(1); + test_dshot_cleanup(); + if (hal.console->available() > 10) { + hal.scheduler->reboot(false); + } +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_HAL_ChibiOS/examples/DShot/wscript b/libraries/AP_HAL_ChibiOS/examples/DShot/wscript new file mode 100644 index 0000000000..719ec151ba --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/examples/DShot/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_example( + use='ap', + )