diff --git a/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c b/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c index ada6acfa36..9028a2d60e 100755 --- a/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c +++ b/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c @@ -1,3 +1,4 @@ +#include #include "stm32f10x.h" #define SYSCLK_FREQ_72MHz 72000000 @@ -86,7 +87,7 @@ enum { }; // Set system clock to 72 (HSE) or 64 (HSI) MHz -void SetSysClock(void) +void SetSysClock(bool overclock) { __IO uint32_t StartUpCounter = 0, status = 0, clocksrc = SRC_NONE; __IO uint32_t *RCC_CRH = &GPIOC->CRH; @@ -140,10 +141,18 @@ void SetSysClock(void) RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); *RCC_CRH |= (uint32_t)0x8 << (RCC_CFGR_PLLMULL9 >> 16); GPIOC->ODR &= (uint32_t)~(CAN_MCR_RESET); + RCC_CFGR_PLLMUL = GPIOC->IDR & CAN_MCR_RESET ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9; switch (clocksrc) { case SRC_HSE: - // PLL configuration: PLLCLK = HSE * 9 = 72 MHz + if (overclock) { + if (RCC_CFGR_PLLMUL == RCC_CFGR_PLLMULL6) + RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL7; + else if (RCC_CFGR_PLLMUL == RCC_CFGR_PLLMULL9) + RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL10; + } + // overclock=false : PLL configuration: PLLCLK = HSE * 9 = 72 MHz || HSE * 6 = 72 MHz + // overclock=true : PLL configuration: PLLCLK = HSE * 10 = 80 MHz || HSE * 7 = 84 MHz RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_HSE | RCC_CFGR_PLLMUL); break; case SRC_HSI: diff --git a/src/config.c b/src/config.c index 4ba626c86b..7ea48adf2c 100755 --- a/src/config.c +++ b/src/config.c @@ -79,6 +79,16 @@ static bool isEEPROMContentValid(void) return true; } +void activateConfig(void) +{ + generatePitchCurve(&cfg.controlRateConfig); + generateThrottleCurve(&cfg.controlRateConfig, mcfg.minthrottle, mcfg.maxthrottle); + + setPIDController(cfg.pidController); + gpsSetPIDs(); + useFailsafeConfig(&cfg.failsafeConfig); +} + void readEEPROM(void) { // Sanity check @@ -90,14 +100,9 @@ void readEEPROM(void) // Copy current profile if (mcfg.current_profile > 2) // sanity check mcfg.current_profile = 0; - memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); + memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); - generatePitchCurve(&cfg.controlRateConfig); - generateThrottleCurve(&cfg.controlRateConfig, mcfg.minthrottle, mcfg.maxthrottle); - - setPIDController(cfg.pidController); - gpsSetPIDs(); - useFailsafeConfig(&cfg.failsafeConfig); + activateConfig(); } void readEEPROMAndNotify(void) @@ -232,6 +237,7 @@ static void resetConf(void) mcfg.serialConfig.reboot_character = 'R'; mcfg.looptime = 3500; + mcfg.emfAvoidance = 0; mcfg.rssi_aux_channel = 0; cfg.pidController = 0; diff --git a/src/config_storage.h b/src/config_storage.h index 726fdec8f5..46cd000ccd 100644 --- a/src/config_storage.h +++ b/src/config_storage.h @@ -66,6 +66,7 @@ typedef struct master_t { uint8_t mixerConfiguration; uint32_t enabledFeatures; uint16_t looptime; // imu loop time in us + uint8_t emfAvoidance; // change pll settings to avoid noise in the uhf band motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; // custom mixtable // motor/esc/servo related stuff diff --git a/src/drivers/system_common.c b/src/drivers/system_common.c index 1dedaf1044..619042a166 100755 --- a/src/drivers/system_common.c +++ b/src/drivers/system_common.c @@ -18,7 +18,7 @@ static volatile uint32_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. static volatile uint32_t sysTickUptime = 0; // from system_stm32f10x.c -void SetSysClock(void); +void SetSysClock(bool overclock); static void cycleCounterInit(void) { @@ -50,7 +50,7 @@ uint32_t millis(void) return sysTickUptime; } -void systemInit(void) +void systemInit(bool overclock) { struct { GPIO_TypeDef *gpio; @@ -82,7 +82,7 @@ void systemInit(void) // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer - SetSysClock(); + SetSysClock(overclock); // Turn on clocks for stuff we use RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE); diff --git a/src/drivers/system_common.h b/src/drivers/system_common.h index ca2871a5d4..7501d3087c 100755 --- a/src/drivers/system_common.h +++ b/src/drivers/system_common.h @@ -1,6 +1,6 @@ #pragma once -void systemInit(void); +void systemInit(bool overclock); void delayMicroseconds(uint32_t us); void delay(uint32_t ms); diff --git a/src/main.c b/src/main.c index 6c2c43d470..1cfab3e922 100755 --- a/src/main.c +++ b/src/main.c @@ -37,7 +37,7 @@ int main(void) serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort2 = NULL; #endif - systemInit(); + systemInit(mcfg.emfAvoidance); initPrintfSupport(); ensureEEPROMContainsValidData(); diff --git a/src/mw.h b/src/mw.h index f64fd3a143..daa8d6cfa3 100755 --- a/src/mw.h +++ b/src/mw.h @@ -36,7 +36,7 @@ enum { #include "rx_common.h" #include "config.h" #include "config_storage.h" - + extern int16_t axisPID[3]; extern int16_t rcCommand[4]; diff --git a/src/serial_cli.c b/src/serial_cli.c index 9178aabdb9..84cdff563a 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -123,6 +123,7 @@ typedef struct { const clivalue_t valueTable[] = { { "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 }, + { "emf_avoidance", VAR_UINT8, &mcfg.emfAvoidance, 0, 1 }, { "midrc", VAR_UINT16, &mcfg.rxConfig.midrc, 1200, 1700 }, { "minthrottle", VAR_UINT16, &mcfg.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },