From 6716ecfe7e20d9c4922a233576d391762d5cbdff Mon Sep 17 00:00:00 2001 From: Johannes Kasberger Date: Mon, 14 Apr 2014 19:52:04 +0200 Subject: [PATCH] added overclock feature --- .../ST/STM32F10x/system_stm32f10x.c | 13 +++++++++++-- src/board.h | 1 + src/cli.c | 2 +- src/config.c | 16 ++++++++++++---- src/drv_system.c | 6 ++++-- src/main.c | 5 +++-- src/mw.h | 2 ++ 7 files changed, 34 insertions(+), 11 deletions(-) diff --git a/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c b/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c index ada6acfa3..9028a2d60 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/board.h b/src/board.h index a41e734cd..c9c8c0df5 100755 --- a/src/board.h +++ b/src/board.h @@ -75,6 +75,7 @@ typedef enum { FEATURE_POWERMETER = 1 << 12, FEATURE_VARIO = 1 << 13, FEATURE_3D = 1 << 14, + FEATURE_OVERCLOCK = 1 << 15, } AvailableFeatures; typedef enum { diff --git a/src/cli.c b/src/cli.c index 7f6b87d4f..9a18ebb2e 100644 --- a/src/cli.c +++ b/src/cli.c @@ -54,7 +54,7 @@ static const char * const mixerNames[] = { static const char * const featureNames[] = { "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", - "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", + "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "OVERCLOCK", NULL }; diff --git a/src/config.c b/src/config.c index e58115cb0..f859fed46 100755 --- a/src/config.c +++ b/src/config.c @@ -56,8 +56,6 @@ static uint8_t validEEPROM(void) void readEEPROM(void) { - uint8_t i; - // Sanity check if (!validEEPROM()) failureMode(10); @@ -67,8 +65,12 @@ 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)); +} +void activateConfig(void) +{ + uint8_t i; for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; @@ -87,6 +89,12 @@ void readEEPROM(void) gpsSetPIDs(); } +void loadAndActivateConfig(void) +{ + readEEPROM(); + activateConfig(); +} + void writeEEPROM(uint8_t b, uint8_t updateProfile) { FLASH_Status status; @@ -139,7 +147,7 @@ retry: } // re-read written data - readEEPROM(); + loadAndActivateConfig(); if (b) blinkLED(15, 20, 1); } diff --git a/src/drv_system.c b/src/drv_system.c index fa29ef8d2..805f96b84 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -5,7 +5,9 @@ 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); +// from config.h +bool feature(uint32_t mask); #ifdef BUZZER void systemBeep(bool onoff); static void beepRev4(bool onoff); @@ -75,7 +77,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(feature(FEATURE_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/main.c b/src/main.c index 58f60eb09..cb35559a8 100755 --- a/src/main.c +++ b/src/main.c @@ -36,13 +36,14 @@ int main(void) serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort2 = NULL; #endif + checkFirstTime(false); + readEEPROM(); systemInit(); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif - checkFirstTime(false); - readEEPROM(); + activateConfig(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) diff --git a/src/mw.h b/src/mw.h index 473a38c6c..7a89cc63a 100755 --- a/src/mw.h +++ b/src/mw.h @@ -437,6 +437,8 @@ void serialCom(void); // Config void parseRcChannels(const char *input); +void activateConfig(void); +void loadAndActivateConfig(void); void readEEPROM(void); void writeEEPROM(uint8_t b, uint8_t updateProfile); void checkFirstTime(bool reset);