From 9e5c5e88c7ad81f6d55be92dee1c0caa2e60b371 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 7 May 2016 00:51:25 +0200 Subject: [PATCH] Rework Fast PWM protocol configuration and timing --- src/main/config/config.c | 13 ++++------- src/main/config/config_master.h | 4 ++-- src/main/drivers/pwm_mapping.c | 15 ++++-------- src/main/drivers/pwm_mapping.h | 10 ++++---- src/main/drivers/pwm_output.c | 51 ++++++++++++++++------------------------- src/main/drivers/pwm_output.h | 6 +++++ src/main/flight/mixer.c | 6 ++--- src/main/flight/mixer.h | 2 +- src/main/io/serial_cli.c | 14 +++++++---- src/main/io/serial_msp.c | 3 ++- src/main/main.c | 14 ++++------- src/main/mw.c | 2 +- 12 files changed, 65 insertions(+), 75 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index dc7abfa8a..d3759bdd3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -140,7 +140,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 134; +static const uint8_t EEPROM_CONF_VERSION = 135; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -190,8 +190,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 80; // filtering ON by default pidProfile->dynamic_pterm = 1; - pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes - #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune. @@ -485,7 +483,8 @@ static void resetConf(void) masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; - masterConfig.use_oneshot42 = 0; + masterConfig.fast_pwm_protocol = 0; + masterConfig.use_unsyncedPwm = 0; #ifdef CC3D masterConfig.use_buzzer_p6 = 0; #endif @@ -500,11 +499,7 @@ static void resetConf(void) resetSerialConfig(&masterConfig.serialConfig); -#if defined(STM32F10X) && !defined(CC3D) - masterConfig.emf_avoidance = 1; -#else - masterConfig.emf_avoidance = 0; -#endif + masterConfig.emf_avoidance = 0; // TODO - needs removal resetPidProfile(¤tProfile->pidProfile); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 281a7fa3d..25a21afba 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -34,8 +34,8 @@ 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_oneshot42; // Oneshot42 - uint8_t use_multiShot; // multishot + uint8_t fast_pwm_protocol; // Fast Pwm Protocol + uint8_t use_unsyncedPwm; // unsync fast pwm protocol from PID loop #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 9c5386327..0ce82b518 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -31,8 +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 pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42); -void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); +void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); /* @@ -912,7 +911,7 @@ if (init->useBuzzerP6) { if (type == MAP_TO_PPM_INPUT) { #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif @@ -923,18 +922,14 @@ if (init->useBuzzerP6) { } else if (type == MAP_TO_MOTOR_OUTPUT) { #ifdef CC3D - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)){ + if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)){ // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed if (timerHardwarePtr->tim == TIM2) continue; } #endif - if (init->useOneshot) { - if (init->useMultiShot) { - pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount); - } else { - pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->useOneshot42); - } + if (init->useFastPwm) { + pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->fastPwmProtocolType, init->motorPwmRate, init->useUnsyncedPwm, init->idlePulse); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; } else if (isMotorBrushed(init->motorPwmRate)) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ec8e05ce3..1edf1458d 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -36,9 +36,11 @@ #define MAX_INPUTS 8 #define PWM_TIMER_MHZ 1 -#define ONESHOT_TIMER_MHZ 24 + #define PWM_BRUSHED_TIMER_MHZ 8 #define MULTISHOT_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 24 +#define ONESHOT125_TIMER_MHZ 8 typedef struct sonarGPIOConfig_s { GPIO_TypeDef *gpio; @@ -59,9 +61,8 @@ typedef struct drv_pwm_config_s { bool useUART3; #endif bool useVbat; - bool useOneshot; - bool useOneshot42; - bool useMultiShot; + bool useFastPwm; + bool useUnsyncedPwm; bool useSoftSerial; bool useLEDStrip; #ifdef SONAR @@ -70,6 +71,7 @@ typedef struct drv_pwm_config_s { #ifdef USE_SERVOS bool useServos; bool useChannelForwarding; // configure additional channels as servos + uint8_t fastPwmProtocolType; uint16_t servoPwmRate; uint16_t servoCenterPulse; #endif diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 97a8fef65..56759cbd4 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -135,27 +135,6 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) { *motors[index]->ccr = value; } -#if defined(STM32F10X) && !defined(CC3D) -static void pwmWriteOneshot125(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = value * 21 / 6; // 24Mhz -> 8Mhz -} - -static void pwmWriteOneshot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = value * 7 / 6; -} -#else -static void pwmWriteOneshot125(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = value * 3; // 24Mhz -> 8Mhz -} - -static void pwmWriteOneshot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = value; -} -#endif static void pwmWriteMultiShot(uint8_t index, uint16_t value) { @@ -227,20 +206,30 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42) +void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse) { - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT_TIMER_MHZ, 0xFFFF, 0); - if (useOneshot42) { - motors[motorIndex]->pwmWritePtr = pwmWriteOneshot42; + uint32_t timerMhzCounter; + + switch (fastPwmProtocolType) { + default: + case (PWM_TYPE_ONESHOT125): + timerMhzCounter = ONESHOT125_TIMER_MHZ; + break; + case (PWM_TYPE_ONESHOT42): + timerMhzCounter = ONESHOT42_TIMER_MHZ; + break; + case (PWM_TYPE_MULTISHOT): + timerMhzCounter = MULTISHOT_TIMER_MHZ; + } + + if (useUnsyncedPwm) { + uint32_t hz = timerMhzCounter * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); } else { - motors[motorIndex]->pwmWritePtr = pwmWriteOneshot125; + motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0); } -} -void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex) -{ - motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, 0xFFFF, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot; + motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : pwmWriteStandard; } #ifdef USE_SERVOS diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 11036d527..615ffa7ac 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -17,6 +17,12 @@ #pragma once +typedef enum { + PWM_TYPE_ONESHOT125 = 0, + PWM_TYPE_ONESHOT42 = 1, + PWM_TYPE_MULTISHOT = 2 +} FastPwmProtocolTypes_e; + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6240d8553..17717247c 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 unsyncedPwm) { uint8_t i; @@ -641,7 +641,7 @@ void writeMotors(void) pwmWriteMotor(i, motor[i]); - if (feature(FEATURE_ONESHOT125)) { + if (feature(FEATURE_ONESHOT125) && !unsyncedPwm) { 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(1); } void stopMotors(void) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 36dca9226..50cb3eb1c 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 unsyncedPwm); void stopMotors(void); void StopPwmAllMotors(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 607328598..423c59bc4 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -442,6 +442,10 @@ static const char * const lookupTableSuperExpoYaw[] = { "OFF", "ON", "ALWAYS" }; +static const char * const lookupTableFastPwm[] = { + "ONESHOT125", "ONESHOT42", "MULTISHOT" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -468,6 +472,7 @@ typedef enum { TABLE_MAG_HARDWARE, TABLE_DEBUG, TABLE_SUPEREXPO_YAW, + TABLE_FAST_PWM, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -490,7 +495,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, - { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) } + { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, + { lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) }, }; #define VALUE_TYPE_OFFSET 0 @@ -566,12 +572,12 @@ 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_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 } }, + { "unsynced_fast_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_unsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, + { "fast_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.fast_pwm_protocol, .config.lookup = { TABLE_FAST_PWM } }, #ifdef CC3D { "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } }, #endif - { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 300, 32000 } }, + { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 200, 32000 } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 72a47a163..aa0d23b8c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -172,7 +172,8 @@ void setGyroSamplingSpeed(uint16_t looptime) { } #endif - if (!(masterConfig.use_multiShot || masterConfig.use_oneshot42) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125)) masterConfig.pid_process_denom = 3; + // Oneshot125 protection + if ((masterConfig.fast_pwm_protocol == 0) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125) && masterConfig.pid_process_denom < 3) masterConfig.pid_process_denom = 3; } } diff --git a/src/main/main.c b/src/main/main.c index ab0299443..06a7e99c3 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -176,7 +176,7 @@ void init(void) #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer - SetSysClock(masterConfig.emf_avoidance); + SetSysClock(0); // TODO - Remove from config in the future #endif //i2cSetOverclock(masterConfig.i2c_overclock); @@ -311,18 +311,14 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); - if (masterConfig.use_oneshot42) { - pwm_params.useOneshot42 = masterConfig.use_oneshot42 ? true : false; - masterConfig.use_multiShot = false; - } else { - pwm_params.useMultiShot = masterConfig.use_multiShot ? true : false; - } + pwm_params.useFastPwm = feature(FEATURE_ONESHOT125); // Configurator feature abused for enabling Fast PWM + pwm_params.fastPwmProtocolType = masterConfig.fast_pwm_protocol; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; + pwm_params.useUnsyncedPwm = masterConfig.use_unsyncedPwm; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - if (pwm_params.motorPwmRate > 500) + if (pwm_params.motorPwmRate > 500 && !pwm_params.useFastPwm) 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 9f226d153..59ba692fc 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -755,7 +755,7 @@ void subTaskMotorUpdate(void) #endif if (motorControlEnable) { - writeMotors(); + writeMotors(masterConfig.use_unsyncedPwm); } if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;} } -- 2.11.4.GIT