diff --git a/docs/Cli.md b/docs/Cli.md index 0f2c5550ab..459060a832 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -116,15 +116,16 @@ Re-apply any new defaults as desired. | `rssi_ppm_invert` | | 0 | 1 | 0 | Master | UINT8 | | `input_filtering_mode` | | 0 | 1 | 0 | Master | INT8 | | `rc_smoothing ` | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | OFF | ON | ON | Master | INT8 | -| `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 | -| `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | 0 | 2000 | 1850 | Master | UINT16 | +| `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 | +| `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | 0 | 2000 | 1850 | Master | UINT16 | | `min_command` | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | 0 | 2000 | 1000 | Master | UINT16 | | `servo_center_pulse` | | 0 | 2000 | 1500 | Master | UINT16 | | `3d_deadband_low` | | 0 | 2000 | 1406 | Master | UINT16 | | `3d_deadband_high` | | 0 | 2000 | 1514 | Master | UINT16 | | `3d_neutral` | | 0 | 2000 | 1460 | Master | UINT16 | | `3d_deadband_throttle` | | 0 | 2000 | 50 | Master | UINT16 | -| `motor_pwm_rate` | Output frequency (in Hz) for motor pins. Defaults are 400Hz for motor. If setting above 500Hz, will switch to brushed (direct drive) motors mode. For example, setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set ```max_throttle``` to 2000. | 50 | 32000 | 400 | Master | UINT16 | +| `motor_pwm_protocol` | Protocol that is used to send motor updates to ESCs. Possible values - PWM, ONESHOT125, ONESHOT32, MULTISHOT, BRUSHED | PWM | BRUSHED | PWM | Master | UINT8 | +| `motor_pwm_rate` | Output frequency (in Hz) for motor pins. Defaults are 400Hz for motor. If setting above 500Hz, will switch to brushed (direct drive) motors mode. For example, setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set ```max_throttle``` to 2000. | 50 | 32000 | 400 | Master | UINT16 | | `servo_pwm_rate` | Output frequency (in Hz) servo pins. Default is 50Hz. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | 50 | 498 | 50 | Master | UINT16 | | `servo_lowpass_freq` | Selects the servo PWM output cutoff frequency. Valid values range from 10 to 400. This is a fraction of the loop frequency in 1/1000ths. For example, `40` means `0.040`. The cutoff frequency can be determined by the following formula: `Frequency = 1000 * servo_lowpass_freq / looptime` | 10 | 400 | 400 | Master | INT16 | | `servo_lowpass_enable` | Disabled by default. | OFF | ON | OFF | Master | INT8 | diff --git a/src/main/config/config.c b/src/main/config/config.c index de60995665..2d35337364 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -37,6 +37,8 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/rx_spi.h" +#include "drivers/pwm_output.h" +#include "drivers/rx_nrf24l01.h" #include "drivers/serial.h" #include "sensors/sensors.h" @@ -249,9 +251,11 @@ void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS motorConfig->minthrottle = 1000; + motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED; motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; #else motorConfig->minthrottle = 1150; + motorConfig->motorPwmProtocol = PWM_TYPE_CONVENTIONAL; motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; #endif motorConfig->maxthrottle = 1850; @@ -604,7 +608,6 @@ static void resetConf(void) masterConfig.rxConfig.rcmap[6] = 6; masterConfig.rxConfig.rcmap[7] = 7; - featureSet(FEATURE_ONESHOT125); featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP); featureSet(FEATURE_FAILSAFE); @@ -758,6 +761,9 @@ void activateConfig(void) void validateAndFixConfig(void) { + // Disable unused features + featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2); + if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { featureSet(DEFAULT_RX_FEATURE); } @@ -790,14 +796,9 @@ void validateAndFixConfig(void) #if defined(CC3D) // There is a timer clash between PWM RX pins and motor output pins - this forces us to have same timer tick rate for these timers // which is only possible when using brushless motors w/o oneshot (timer tick rate is PWM_TIMER_MHZ) - // On CC3D OneShot is incompatible with PWM RX - featureClear(FEATURE_ONESHOT125); - - // Brushed motors on CC3D are not possible when using PWM RX - if (masterConfig.motorConfig.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE) { - masterConfig.motorConfig.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; - } + masterConfig.motorConfig.motorPwmProtocol = PWM_TYPE_CONVENTIONAL; + masterConfig.motorConfig.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; #endif #endif @@ -918,13 +919,37 @@ void validateAndFixConfig(void) /* * If provided predefined mixer setup is disabled, fallback to default one */ - if (!isMixerEnabled(masterConfig.mixerMode)) { - masterConfig.mixerMode = DEFAULT_MIXER; - } + if (!isMixerEnabled(masterConfig.mixerMode)) { + masterConfig.mixerMode = DEFAULT_MIXER; + } + #if defined(NAV) // Ensure sane values of navConfig settings validateNavConfig(&masterConfig.navConfig); #endif + + /* Limitations of different protocols */ + switch (masterConfig.motorConfig.motorPwmProtocol) { + case PWM_TYPE_CONVENTIONAL: // Limited to 490 Hz + masterConfig.motorConfig.motorPwmRate = MIN(masterConfig.motorConfig.motorPwmRate, 490); + break; + + case PWM_TYPE_ONESHOT125: // Limited to 3900 Hz + masterConfig.motorConfig.motorPwmRate = MIN(masterConfig.motorConfig.motorPwmRate, 3900); + break; + + case PWM_TYPE_ONESHOT42: // 2-8 kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 2000, 8000); + break; + + case PWM_TYPE_MULTISHOT: // 2-16 kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 2000, 16000); + break; + + case PWM_TYPE_BRUSHED: // 500Hz - 32kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 500, 32000); + break; + } } void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch) @@ -980,17 +1005,6 @@ void changeControlRateProfile(uint8_t profileIndex) activateControlRateConfig(); } -void handleOneshotFeatureChangeOnRestart(void) -{ - // Shutdown PWM on all motors prior to soft restart - StopPwmAllMotors(); - delay(50); - // Apply additional delay when OneShot125 feature changed from on to off state - if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } -} - void persistentFlagClearAll() { masterConfig.persistentFlags = 0; diff --git a/src/main/config/config.h b/src/main/config/config.h index 3bdc77211f..d78f29bb57 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -32,7 +32,7 @@ typedef enum { FEATURE_RX_PPM = 1 << 0, FEATURE_VBAT = 1 << 1, - //FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_UNUSED_1 = 1 << 2, // Unused in INAV FEATURE_RX_SERIAL = 1 << 3, FEATURE_MOTOR_STOP = 1 << 4, FEATURE_SERVO_TILT = 1 << 5, @@ -48,7 +48,7 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_UNUSED_2 = 1 << 18, // Unused in INAV FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, @@ -64,7 +64,6 @@ typedef enum { FLAG_MAG_CALIBRATION_DONE = 1 << 0, } persistent_flags_e; -void handleOneshotFeatureChangeOnRestart(void); void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 62e0de1bcd..164380a691 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -32,9 +32,7 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -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 pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); /* @@ -331,12 +329,12 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #ifndef SKIP_RX_PWM_PPM #ifdef CC3D_PPM1 - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4); } #endif #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif @@ -363,7 +361,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } #if defined(CC3D) && !defined(CC3D_PPM1) - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed if (timerHardwarePtr->tim == TIM2) { addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); @@ -371,19 +369,13 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } } #endif - if (init->useOneshot) { - pwmOneshotMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount); - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM; - - } else if (isMotorBrushed(init->motorPwmRate)) { - - pwmBrushedMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + if (init->useFastPwm) { + pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; + } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else { - - pwmBrushlessMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 1e265d3ffc..84497c6fca 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -38,16 +38,12 @@ #define MAX_SERVOS 8 #endif +#define PWM_TIMER_MHZ 1 #define PULSE_1MS (1000) // 1ms pulse width #define MAX_INPUTS 8 -#define PWM_TIMER_MHZ 1 -#define ONESHOT125_TIMER_MHZ 8 -#define PWM_BRUSHED_TIMER_MHZ 8 - - typedef struct sonarIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; @@ -63,7 +59,7 @@ typedef struct drv_pwm_config_s { bool useUART3; bool useUART6; bool useVbat; - bool useOneshot; + bool useFastPwm; bool useSoftSerial; bool useLEDStrip; #ifdef SONAR @@ -76,6 +72,7 @@ typedef struct drv_pwm_config_s { uint16_t servoCenterPulse; #endif bool airplane; // fixed wing hardware config, lots of servos etc + uint8_t pwmProtocolType; uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. @@ -96,7 +93,7 @@ typedef enum { PWM_PF_SERVO = (1 << 1), PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4), + PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4), PWM_PF_PPM = (1 << 5), PWM_PF_PWM = (1 << 6) } pwmPortFlags_e; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e8ec2a16c2..3333997ade 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,14 +17,11 @@ #include #include - -#include +#include #include "platform.h" -#include "gpio.h" #include "io.h" -#include "io_impl.h" #include "timer.h" #include "pwm_mapping.h" #include "pwm_output.h" @@ -37,6 +34,21 @@ #include "fc/runtime_config.h" +#if defined(STM32F40_41xxx) // must be multiples of timer clock +#define ONESHOT125_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 21 +#define MULTISHOT_TIMER_MHZ 84 +#define PWM_BRUSHED_TIMER_MHZ 21 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) + typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { @@ -96,22 +108,15 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 } } -static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) -{ - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); -} - static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) { pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; configTimeBase(timerHardware->tim, period, mhz); - pwmGPIOConfig(IO_GPIOBYTAG(timerHardware->tag), IO_PINBYTAG(timerHardware->tag), Mode_AF_PP); + + const IO_t io = IOGetByTag(timerHardware->tag); + IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); + IOConfigGPIO(io, IOCFG_AF_PP); pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { @@ -151,6 +156,21 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) *motors[index]->ccr = value; } +static void pwmWriteOneShot125(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); +} + +static void pwmWriteOneShot42(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); +} + +static void pwmWriteMultiShot(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); +} + void pwmWriteMotor(uint8_t index, uint16_t value) { if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { @@ -176,47 +196,48 @@ void pwmEnableMotors(void) pwmMotorsEnabled = true; } -void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) -{ - TIM_TypeDef *lastTimerPtr = NULL; - - for (int index = 0; index < motorCount; index++) { - - // Force the timer to overflow if it's the first motor to output, or if we change timers - if (motors[index]->tim != lastTimerPtr) { - lastTimerPtr = motors[index]->tim; - timerForceOverflow(motors[index]->tim); - } - - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. - // This compare register will be set to the output value on the next main loop. - *motors[index]->ccr = 0; - } -} - bool isMotorBrushed(uint16_t motorPwmRate) { return (motorPwmRate > 500); } -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto) { - const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; -} + uint32_t timerMhzCounter; + pwmWriteFuncPtr pwmWritePtr; -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) -{ - const uint32_t hz = PWM_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; -} + switch (proto) { + case PWM_TYPE_BRUSHED: + timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; + pwmWritePtr = pwmWriteBrushed; + idlePulse = 0; + break; -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex) -{ - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; + case PWM_TYPE_ONESHOT125: + timerMhzCounter = ONESHOT125_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot125; + break; + + case PWM_TYPE_ONESHOT42: + timerMhzCounter = ONESHOT42_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot42; + break; + + case PWM_TYPE_MULTISHOT: + timerMhzCounter = MULTISHOT_TIMER_MHZ; + pwmWritePtr = pwmWriteMultiShot; + break; + + case PWM_TYPE_CONVENTIONAL: + default: + timerMhzCounter = PWM_TIMER_MHZ; + pwmWritePtr = pwmWriteStandard; + break; + } + + const uint32_t hz = timerMhzCounter * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); + motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7106f94a37..3dd801aaed 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -17,13 +17,18 @@ #pragma once +typedef enum { + PWM_TYPE_CONVENTIONAL = 0, + PWM_TYPE_ONESHOT125, + PWM_TYPE_ONESHOT42, + PWM_TYPE_MULTISHOT, + PWM_TYPE_BRUSHED +} motorPwmProtocolTypes_e; + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); -void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, uint16_t value); -bool isMotorBrushed(uint16_t motorPwmRate); - void pwmDisableMotors(void); void pwmEnableMotors(void); diff --git a/src/main/fc/msp_fc.c b/src/main/fc/msp_fc.c index 0ca61410c9..1aba9e619f 100644 --- a/src/main/fc/msp_fc.c +++ b/src/main/fc/msp_fc.c @@ -194,7 +194,10 @@ static void mspRebootFn(serialPort_t *serialPort) UNUSED(serialPort); stopMotors(); - handleOneshotFeatureChangeOnRestart(); + stopPwmAllMotors(); + + // extra delay before reboot to give ESCs chance to reset + delay(1000); systemReset(); // control should never return here. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e66d4fce1b..5d0a48b8f7 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -383,11 +383,6 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - - - if (feature(FEATURE_ONESHOT125)) { - pwmCompleteOneshotMotorUpdate(motorCount); - } } void writeAllMotors(int16_t mc) @@ -407,7 +402,7 @@ void stopMotors(void) delay(50); // give the timers and ESCs a chance to react. } -void StopPwmAllMotors() +void stopPwmAllMotors() { pwmShutdownPulsesForAllMotors(motorCount); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index b83a99b0d4..7fb3a6f5ac 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -124,6 +124,6 @@ void writeMotors(void); void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted); void processServoTilt(void); void stopMotors(void); -void StopPwmAllMotors(void); +void stopPwmAllMotors(void); bool isMixerEnabled(mixerMode_e mixerMode); diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 6b4b986a97..feb2c76bd5 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -23,4 +23,5 @@ typedef struct motorConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint8_t motorPwmProtocol; } motorConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 54fb261a3c..da4f8a79d7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -210,10 +210,10 @@ static const char * const mixerNames[] = { // sync this with features_e static const char * const featureNames[] = { - "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", + "RX_PPM", "VBAT", "UNUSED_1", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "UNUSED_2", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", NULL }; @@ -464,6 +464,10 @@ static const char * const lookupTableAuxOperator[] = { "OR", "AND" }; +static const char * const lookupTablePwmProtocol[] = { + "PWM", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -498,6 +502,7 @@ typedef enum { TABLE_NAV_RTH_ALT_MODE, #endif TABLE_AUX_OPERATOR, + TABLE_MOTOR_PWM_PROTOCOL, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -529,6 +534,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableNavRthAltMode, sizeof(lookupTableNavRthAltMode) / sizeof(char *) }, #endif { lookupTableAuxOperator, sizeof(lookupTableAuxOperator) / sizeof(char *) }, + { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, }; #define VALUE_TYPE_OFFSET 0 @@ -605,6 +611,7 @@ const clivalue_t valueTable[] = { { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 50, 32000 }, 0 }, + { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "fixed_wing_auto_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.fixed_wing_auto_arm, .config.lookup = { TABLE_OFF_ON }, 0 }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON }, 0 }, @@ -2492,8 +2499,11 @@ static void cliReboot(void) { cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); + stopMotors(); - handleOneshotFeatureChangeOnRestart(); + stopPwmAllMotors(); + + delay(1000); systemReset(); } diff --git a/src/main/main.c b/src/main/main.c index 1fca875663..c63e2bff5a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -50,6 +50,7 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" #include "drivers/pwm_rx.h" #include "drivers/pwm_output.h" #include "drivers/adc.h" @@ -278,13 +279,21 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servoConfig.servoPwmRate; #endif - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); + pwm_params.pwmProtocolType = masterConfig.motorConfig.motorPwmProtocol; + pwm_params.useFastPwm = (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_ONESHOT125) || + (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_ONESHOT42) || + (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_MULTISHOT); pwm_params.motorPwmRate = masterConfig.motorConfig.motorPwmRate; pwm_params.idlePulse = masterConfig.motorConfig.mincommand; - if (feature(FEATURE_3D)) + if (feature(FEATURE_3D)) { pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - if (pwm_params.motorPwmRate > 500) + } + + if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { + pwm_params.useFastPwm = false; + featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors + } #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); @@ -305,7 +314,7 @@ void init(void) mixerUsePWMIOConfiguration(); - if (!feature(FEATURE_ONESHOT125)) + if (!pwm_params.useFastPwm) motorControlEnable = true; addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE);