From 779823fb7957efefb2fd8a317c22f8d5503768ef Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 23 Jun 2017 20:06:24 +1200 Subject: [PATCH] Cleaned up Dshot naming, containment. --- src/main/drivers/pwm_output.c | 33 ++++++++++++++----------- src/main/drivers/pwm_output.h | 17 ++++++++++--- src/main/drivers/pwm_output_dshot.c | 6 ++--- src/main/drivers/pwm_output_dshot_hal.c | 6 ++--- src/main/flight/mixer.c | 17 ------------- src/main/flight/mixer.h | 19 -------------- src/main/target/SITL/target.c | 4 +++ 7 files changed, 43 insertions(+), 59 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 232589060..1f725554e 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -49,7 +49,7 @@ static uint16_t freqBeep=0; #endif bool pwmMotorsEnabled = false; -bool isDigital = false; +bool isDshot = false; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -160,9 +160,9 @@ static void pwmWriteMultiShot(uint8_t index, float value) } #ifdef USE_DSHOT -static void pwmWriteDigital(uint8_t index, float value) +static void pwmWriteDshot(uint8_t index, float value) { - pwmWriteDigitalInt(index, lrintf(value)); + pwmWriteDshotInt(index, lrintf(value)); } static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet) @@ -277,24 +277,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 break; #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: - pwmWrite = &pwmWriteDigital; + pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferProshot; - pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; - isDigital = true; + pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; + isDshot = true; break; case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWrite = &pwmWriteDigital; + pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferDshot; - pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; - isDigital = true; + pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; + isDshot = true; break; #endif } - if (!isDigital) { + if (!isDshot) { pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate; } @@ -313,8 +313,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 motors[motorIndex].io = IOGetByTag(tag); #ifdef USE_DSHOT - if (isDigital) { - pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + if (isDshot) { + pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; @@ -354,6 +354,11 @@ pwmOutputPort_t *pwmGetMotors(void) return motors; } +bool isMotorProtocolDshot(void) +{ + return isDshot; +} + #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { @@ -374,7 +379,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) void pwmWriteDshotCommand(uint8_t index, uint8_t command) { - if (isDigital && (command <= DSHOT_MAX_COMMAND)) { + if (isDshot && (command <= DSHOT_MAX_COMMAND)) { motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; @@ -395,7 +400,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) for (; repeats; repeats--) { motor->requestTelemetry = true; - pwmWriteDigitalInt(index, command); + pwmWriteDshotInt(index, command); pwmCompleteMotorUpdate(0); delay(1); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 16d7f37c3..c37fa960f 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -30,6 +30,15 @@ #define DSHOT_MAX_COMMAND 47 +/* + DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings + + 3D Mode: + 0 = stop + 48 (low) - 1047 (high) -> negative direction + 1048 (low) - 2047 (high) -> positive direction + */ + typedef enum { DSHOT_CMD_MOTOR_STOP = 0, DSHOT_CMD_BEEP1, @@ -166,6 +175,8 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig); void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); +bool isMotorProtocolDshot(void); + #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 @@ -175,9 +186,9 @@ extern loadDmaBufferFunc *loadDmaBuffer; uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -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); +void pwmWriteDshotInt(uint8_t index, uint16_t value); +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); +void pwmCompleteDshotMotorUpdate(uint8_t motorCount); #endif #ifdef BEEPER diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 2c97bc0ef..305876102 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,7 +54,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDigitalInt(uint8_t index, uint16_t value) +void pwmWriteDshotInt(uint8_t index, uint16_t value) { motorDmaOutput_t *const motor = &dmaMotors[index]; @@ -70,7 +70,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value) DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } -void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) +void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); @@ -94,7 +94,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index de270044b..c4a65cfed 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,7 +49,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDigitalInt(uint8_t index, uint16_t value) +void pwmWriteDshotInt(uint8_t index, uint16_t value) { motorDmaOutput_t *const motor = &dmaMotors[index]; @@ -74,7 +74,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value) } } -void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) +void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); } @@ -85,7 +85,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fa3382475..4f8d8c2d4 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -336,23 +336,6 @@ bool mixerIsOutputSaturated(int axis, float errorRate) return false; } -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: - case PWM_TYPE_DSHOT150: - return true; - default: - return false; - } -#else - return false; -#endif -} - // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 665b39fd2..64535fd7c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -25,24 +25,6 @@ #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 480 -/* - DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings - 0 = stop - 1-5: beep - 6: ESC info request (FW Version and SN sent over the tlm wire) - 7: spin direction 1 - 8: spin direction 2 - 9: 3d mode off - 10: 3d mode on - 11: ESC settings request (saved settings over the TLM wire) - 12: save Settings - - 3D Mode: - 0 = stop - 48 (low) - 1047 (high) -> negative direction - 1048 (low) - 2047 (high) -> positive direction -*/ - // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -140,6 +122,5 @@ void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); -bool isMotorProtocolDshot(void); float convertExternalToMotor(uint16_t externalValue); uint16_t convertMotorToExternal(float motorValue); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index bb0d756b0..efe3e1865 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -392,6 +392,10 @@ pwmOutputPort_t *pwmGetMotors(void) { bool pwmAreMotorsEnabled(void) { return pwmMotorsEnabled; } +bool isMotorProtocolDshot(void) +{ + return false; +} void pwmWriteMotor(uint8_t index, float value) { motorsPwm[index] = value - idlePulse; }