rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3318 from mikeller/fixup_dshot 

Fixup Dshot / Proshot implementation.
This commit is contained in:
borisbstyle 2017-06-21 15:34:46 +02:00 committed by GitHub
parent 0a17e416da 76869eb3dd
commit d4bd8a366d
5 changed files with 95 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
src/main/drivers/pwm_output.c
View File

@ -31,11 +31,13 @@
#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 pwmWriteFunc *pwmWrite;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT
loadDmaBufferFunc *loadDmaBuffer;
#endif
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
@ -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,10 +159,37 @@ 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) {
pwmWritePtr(index, value);
pwmWrite(index, value);
}
}
@ -182,7 +212,7 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void)
{
/* check motors can be enabled */
pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused);
pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
}
bool pwmAreMotorsEnabled(void)
@ -209,7 +239,7 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmCompleteMotorUpdate(uint8_t motorCount)
{
pwmCompleteWritePtr(motorCount);
pwmCompleteWrite(motorCount);
}
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
@ -218,53 +248,54 @@ 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:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot125;
pwmWrite = &pwmWriteOneShot125;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot42;
pwmWrite = &pwmWriteOneShot42;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWritePtr = pwmWriteMultiShot;
pwmWrite = &pwmWriteMultiShot;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWritePtr = pwmWriteBrushed;
pwmWrite = &pwmWriteBrushed;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWritePtr = pwmWriteStandard;
pwmWrite = &pwmWriteStandard;
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
pwmWritePtr = pwmWriteProShot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
pwmWrite = &pwmWriteDigital;
loadDmaBuffer = &loadDmaBufferProshot;
pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
pwmWrite = &pwmWriteDigital;
loadDmaBuffer = &loadDmaBufferDshot;
pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
#endif
}
if (!isDigital) {
pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate;
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
@ -273,8 +304,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
pwmWritePtr = pwmWriteUnused;
pwmCompleteWritePtr = pwmCompleteWriteUnused;
pwmWrite = &pwmWriteUnused;
pwmCompleteWrite = &pwmCompleteWriteUnused;
/* TODO: block arming and add reason system cannot arm */
return;
}
@ -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

15
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,
@ -131,8 +133,8 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
extern bool pwmMotorsEnabled;
struct timerHardware_s;
typedef void(*pwmWriteFuncPtr)(uint8_t index, float value); // function pointer used to write motors
typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written
typedef void pwmWriteFunc(uint8_t index, float value); // function pointer used to write motors
typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written
typedef struct {
volatile timCCR_t *ccr;
@ -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 loadDmaBufferFunc(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 loadDmaBufferFunc *loadDmaBuffer;
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 = loadDmaBuffer(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 = loadDmaBuffer(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: