mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 17:38:32 -04:00
142 lines
3.8 KiB
C++
142 lines
3.8 KiB
C++
/*
|
|
example outputting DShot on 4 TIM1 channels on fmuv4 board
|
|
*/
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#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();
|