Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Fixup Dshot / Proshot implementation.

Browse source
This commit is contained in:
mikeller 2017-06-20 01:33:22 +12:00
parent 7b762e640c
commit 559079ff31
5 changed files with 78 additions and 131 deletions
Download patch file Download diff file Expand all files Collapse all files

64
src/main/drivers/pwm_output.c
View file

@ -31,12 +31,14 @@
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
#define DSHOT_MAX_COMMAND 47
static pwmWriteFuncPtr pwmWritePtr;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
#ifdef USE_DSHOT
loadDmaBufferFuncPtr loadDmaBufferPtr;
#endif
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif
@ -47,6 +49,7 @@ static uint16_t freqBeep=0;
#endif
bool pwmMotorsEnabled = false;
bool isDigital = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -156,6 +159,33 @@ static void pwmWriteMultiShot(uint8_t index, float value)
*motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
}
#ifdef USE_DSHOT
static void pwmWriteDigital(uint8_t index, float value)
{
pwmWriteDigitalInt(index, lrintf(value));
}
static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
{
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
return DSHOT_DMA_BUFFER_SIZE;
}
static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet)
{
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
return PROSHOT_DMA_BUFFER_SIZE;
}
#endif
void pwmWriteMotor(uint8_t index, float value)
{
if (pwmMotorsEnabled) {
@ -218,7 +248,6 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
switch (motorConfig->motorPwmProtocol) {
default:
@ -248,7 +277,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
break;
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
pwmWritePtr = pwmWriteProShot;
pwmWritePtr = pwmWriteDigital;
loadDmaBufferPtr = loadDmaBufferProshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
@ -256,7 +286,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDshot;
pwmWritePtr = pwmWriteDigital;
loadDmaBufferPtr = loadDmaBufferDshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
@ -343,7 +374,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
void pwmWriteDshotCommand(uint8_t index, uint8_t command)
{
if (command <= DSHOT_MAX_COMMAND) {
if (isDigital && (command <= DSHOT_MAX_COMMAND)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
unsigned repeats;
@ -364,13 +395,32 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
for (; repeats; repeats--) {
motor->requestTelemetry = true;
pwmWritePtr(index, command);
pwmWriteDigitalInt(index, command);
pwmCompleteMotorUpdate(0);
delay(1);
}
}
}
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
{
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
return packet;
}
#endif
#ifdef USE_SERVOS

11
src/main/drivers/pwm_output.h
View file

@ -28,6 +28,8 @@
#define MAX_SUPPORTED_SERVOS 8
#endif
#define DSHOT_MAX_COMMAND 47
typedef enum {
DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEEP1,
@ -165,10 +167,15 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
#ifdef USE_DSHOT
typedef uint8_t(*loadDmaBufferFuncPtr)(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
extern loadDmaBufferFuncPtr loadDmaBufferPtr;
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
void pwmWriteProShot(uint8_t index, float value);
void pwmWriteDshot(uint8_t index, float value);
void pwmWriteDigitalInt(uint8_t index, uint16_t value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif

61
src/main/drivers/pwm_output_dshot.c
View file

@ -54,70 +54,19 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount-1;
}
void pwmWriteDshot(uint8_t index, float value)
void pwmWriteDigitalInt(uint8_t index, uint16_t value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
uint16_t packet = prepareDshotPacket(motor, value);
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for whole packet
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
uint8_t bufferSize = loadDmaBufferPtr(motor, packet);
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
}
void pwmWriteProShot(uint8_t index, float value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for Proshot
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE);
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
}

72
src/main/drivers/pwm_output_dshot_hal.c
View file

@ -49,85 +49,25 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount - 1;
}
void pwmWriteDshot(uint8_t index, float value)
void pwmWriteDigitalInt(uint8_t index, uint16_t value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
uint16_t packet = prepareDshotPacket(motor, value);
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for whole packet
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
uint8_t bufferSize = loadDmaBufferPtr(motor, packet);
if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
} else {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
}
}
void pwmWriteProShot(uint8_t index, float value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for Proshot
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
} else {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) {
/* Starting PWM generation Error */
return;
}

1
src/main/flight/mixer.c
View file

@ -339,6 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
switch(motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300: