From f79896b1bd7de0c697100e914aa5488fc035b33a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 25 Oct 2016 01:09:06 +0200 Subject: [PATCH] Add DSHOT300 Bool check for dshot protocol Add dshot300 to init ident --- src/main/drivers/pwm_output.c | 1 + src/main/drivers/pwm_output.h | 1 + src/main/drivers/pwm_output_stm32f3xx.c | 17 +++++++++++++++-- src/main/drivers/pwm_output_stm32f4xx.c | 15 ++++++++++++++- src/main/drivers/pwm_output_stm32f7xx.c | 15 ++++++++++++++- src/main/fc/config.c | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/flight/mixer.c | 13 ++++++++++--- src/main/flight/mixer.h | 1 + src/main/io/serial_cli.c | 2 +- 10 files changed, 59 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 59ddad65e..ae9aa3ede 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -189,6 +189,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 0c1a61832..53a0a7fc4 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,7 @@ typedef enum { PWM_TYPE_MULTISHOT, PWM_TYPE_BRUSHED, PWM_TYPE_DSHOT600, + PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT150, PWM_TYPE_MAX } motorPwmProtocolTypes_e; diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index f45b0483e..80903ce19 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -33,6 +33,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 24 +#define MOTOR_DSHOT300_MHZ 12 #define MOTOR_DSHOT150_MHZ 6 #define MOTOR_BIT_0 14 @@ -121,8 +122,20 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index f5a92fb37..7e60c62f1 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -34,6 +34,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 12 +#define MOTOR_DSHOT300_MHZ 6 #define MOTOR_DSHOT150_MHZ 3 #define MOTOR_BIT_0 7 @@ -123,7 +124,19 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 4d47bfc99..523b993df 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -33,6 +33,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 12 +#define MOTOR_DSHOT300_MHZ 6 #define MOTOR_DSHOT150_MHZ 3 #define MOTOR_BIT_0 7 @@ -128,7 +129,19 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { RCC_ClockCmd(timerRCC(timer), ENABLE); - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + motor->TimHandle.Instance = timerHardware->tim; motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;; motor->TimHandle.Init.Period = MOTOR_BITLENGTH; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 92c9b27a5..d983143ff 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -259,7 +259,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) #endif motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; - motorConfig->digitalIdleOffset = 0; + motorConfig->digitalIdleOffset = 40; uint8_t motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c46917553..0c264ca6c 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -653,7 +653,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, 0); continue; } - if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT150 || masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT600) + if (isMotorProtocolDshot()) sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator else sbufWriteU16(dst, motor[i]); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c31f958a8..3c5bf25f6 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -336,9 +336,16 @@ static motorMixer_t *customMixers; static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow; static float rcCommandThrottleRange; +bool isMotorProtocolDshot(void) { + if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) + return true; + else + return false; +} + // Add here scaled ESC outputs for digital protol void initEscEndpoints(void) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) { + if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; @@ -843,7 +850,7 @@ void mixTable(void *pidProfilePtr) motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) ); if (failsafeIsActive()) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) + if (isMotorProtocolDshot()) motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); @@ -875,7 +882,7 @@ void mixTable(void *pidProfilePtr) // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) { + if (isMotorProtocolDshot()) { motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range if (motor_disarmed[i] != disarmMotorOutput) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 57f2ce0fc..43ef84185 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -230,3 +230,4 @@ void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); +bool isMotorProtocolDshot(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 70ba4fbda..88dcd5a0a 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -525,7 +525,7 @@ static const char * const lookupTableSuperExpoYaw[] = { static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", #ifdef USE_DSHOT - "DSHOT600", "DSHOT150" + "DSHOT600", "DSHOT300", "DSHOT150" #endif };