diff --git a/src/main/config/config.c b/src/main/config/config.c index 802fbc7f09..02c9a389a7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -485,7 +485,7 @@ static void resetConf(void) masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; - masterConfig.use_fast_pwm = 0; + masterConfig.force_motor_pwm_rate = 0; masterConfig.use_oneshot42 = 0; #ifdef CC3D masterConfig.use_buzzer_p6 = 0; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e55cb8f6e1..e9931b6ee6 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -36,9 +36,9 @@ typedef struct master_t { uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) - uint8_t use_fast_pwm; // Use fast PWM implementation when oneshot enabled - uint8_t use_oneshot42; // Oneshot42 - uint8_t use_multiShot; // multishot + uint8_t force_motor_pwm_rate; // Use fixed motor update even when oneshot enabled + uint8_t use_oneshot42; // Oneshot42 + uint8_t use_multiShot; // multishot #ifdef USE_SERVOS servoMixer_t customServoMixer[MAX_SERVO_RULES]; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 02eb9dcb52..8a3be70fae 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -31,7 +31,7 @@ void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot); +void pwmFixedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot); void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42); void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); @@ -805,8 +805,8 @@ if (init->useBuzzerP6) { } #endif if (init->useOneshot) { - if (init->useFastPWM) { - fastPWMMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->useOneshot42, init->useMultiShot); + if (init->useFixedPWM) { + pwmFixedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->useOneshot42, init->useMultiShot); } else if (init->useMultiShot) { pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount); } else { diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index f77cbd0694..86e070a359 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -60,7 +60,7 @@ typedef struct drv_pwm_config_s { #endif bool useVbat; bool useOneshot; - bool useFastPWM; + bool useFixedPWM; bool useOneshot42; bool useMultiShot; bool useSoftSerial; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index aaeeed5a61..1a69377f46 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -205,7 +205,7 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot) +void pwmFixedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot) { uint32_t hz; if (useMultiShot) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ba2dd5c666..4b8873b232 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -633,7 +633,7 @@ void writeServos(void) } #endif -void writeMotors(void) +void writeMotors(uint8_t useMotorPwmRate) { uint8_t i; @@ -641,7 +641,7 @@ void writeMotors(void) pwmWriteMotor(i, motor[i]); - if (feature(FEATURE_ONESHOT125)) { + if (feature(FEATURE_ONESHOT125) && useMotorPwmRate) { pwmCompleteOneshotMotorUpdate(motorCount); } } @@ -653,7 +653,7 @@ void writeAllMotors(int16_t mc) // Sends commands to all motors for (i = 0; i < motorCount; i++) motor[i] = mc; - writeMotors(); + writeMotors(0); } void stopMotors(void) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index eb324c6768..12aec2ed05 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -218,6 +218,6 @@ int servoDirection(int servoIndex, int fromChannel); #endif void mixerResetDisarmedMotors(void); void mixTable(void); -void writeMotors(void); +void writeMotors(uint8_t useMotorPwmRate); void stopMotors(void); void StopPwmAllMotors(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 98c7ed051a..2e14ee9bae 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -550,7 +550,7 @@ const clivalue_t valueTable[] = { { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "use_fast_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_fast_pwm, .config.lookup = { TABLE_OFF_ON } }, + { "force_motor_pwm_rate", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.force_motor_pwm_rate, .config.lookup = { TABLE_OFF_ON } }, { "use_oneshot42", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_oneshot42, .config.lookup = { TABLE_OFF_ON } }, { "use_multishot", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_multiShot, .config.lookup = { TABLE_OFF_ON } }, #ifdef CC3D diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index deb8594ab1..c8364e2d19 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -165,10 +165,14 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); if (looptime < 1000) { - masterConfig.use_fast_pwm = 1; - masterConfig.motor_pwm_rate = lrintf(1.0f / (gyroSampleRate * masterConfig.gyro_sync_denom * masterConfig.pid_process_denom * 0.000001f)); + masterConfig.force_motor_pwm_rate = 1; + if (masterConfig.use_multiShot || masterConfig.use_oneshot42) { + masterConfig.motor_pwm_rate = 4200; + } else { + masterConfig.motor_pwm_rate = 2700; + } } else { - masterConfig.use_fast_pwm = 0; + masterConfig.force_motor_pwm_rate = 0; } } } diff --git a/src/main/main.c b/src/main/main.c index 58941dc315..6024e64674 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -310,7 +310,7 @@ void init(void) #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); - pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false; + pwm_params.useFixedPWM = masterConfig.force_motor_pwm_rate ? true : false; if (masterConfig.use_oneshot42) { pwm_params.useOneshot42 = masterConfig.use_oneshot42 ? true : false; masterConfig.use_multiShot = false; @@ -321,7 +321,7 @@ void init(void) pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - if (pwm_params.motorPwmRate > 500 && !masterConfig.use_fast_pwm) + if (pwm_params.motorPwmRate > 500 && !masterConfig.force_motor_pwm_rate) pwm_params.idlePulse = 0; // brushed motors #ifdef CC3D pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; diff --git a/src/main/mw.c b/src/main/mw.c index 2df6357676..dd5e8b7226 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -746,7 +746,7 @@ void taskMainPidLoopCheck(void) { debug[3] = motorCycleTime - targetPidLooptime; } - writeMotors(); + writeMotors(masterConfig.force_motor_pwm_rate); } while (true) {