diff --git a/flight/pios/common/pios_servo.c b/flight/pios/common/pios_servo.c
index 635273434..d0ca5f6b9 100644
--- a/flight/pios/common/pios_servo.c
+++ b/flight/pios/common/pios_servo.c
@@ -142,19 +142,22 @@ static void PIOS_Servo_SetupBank(uint8_t bank_nr)
             continue;
         }
 
+        GPIO_InitTypeDef init = chan->pin.init;
+
         switch (bank->mode) {
         case PIOS_SERVO_BANK_MODE_PWM:
         case PIOS_SERVO_BANK_MODE_SINGLE_PULSE:
-            GPIO_Init(chan->pin.gpio, &chan->pin.init);
 #if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
             GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap);
 #elif defined(STM32F10X_MD)
+            init.GPIO_Mode = GPIO_Mode_AF_PP;
             if (chan->remap) {
                 GPIO_PinRemapConfig(chan->remap, ENABLE);
             }
 #else
 #error Unsupported MCU
 #endif
+            GPIO_Init(chan->pin.gpio, &init);
 
             /* Set up for output compare function */
             switch (chan->timer_chan) {
@@ -180,8 +183,6 @@ static void PIOS_Servo_SetupBank(uint8_t bank_nr)
 
         case PIOS_SERVO_BANK_MODE_DSHOT:
         {
-            GPIO_InitTypeDef init = chan->pin.init;
-
 #if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE)
             init.GPIO_Mode = GPIO_Mode_OUT;
 #elif defined(STM32F10X_MD)
diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c
index 012e6b730..849bb83cd 100644
--- a/flight/pios/stm32f10x/pios_sys.c
+++ b/flight/pios/stm32f10x/pios_sys.c
@@ -53,63 +53,14 @@ void PIOS_SYS_Init(void)
     /* Init the delay system */
     PIOS_DELAY_Init();
 
-    /*
-     * Turn on all the peripheral clocks.
-     * Micromanaging clocks makes no sense given the power situation in the system, so
-     * light up everything we might reasonably use here and just leave it on.
-     */
-
+    /* Enable DMA */
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 |
                           RCC_AHBPeriph_DMA2,
                           ENABLE);
 
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 |
-                           RCC_APB1Periph_TIM3 |
-                           RCC_APB1Periph_TIM4 |
-                           RCC_APB1Periph_TIM5 |
-                           RCC_APB1Periph_TIM6 |
-                           RCC_APB1Periph_TIM7 |
-                           RCC_APB1Periph_TIM12 |
-                           RCC_APB1Periph_TIM13 |
-                           RCC_APB1Periph_TIM14 |
-                           RCC_APB1Periph_WWDG |
-                           RCC_APB1Periph_SPI2 |
-                           RCC_APB1Periph_SPI3 |
-                           RCC_APB1Periph_USART2 |
-                           RCC_APB1Periph_USART3 |
-                           RCC_APB1Periph_UART4 |
-                           RCC_APB1Periph_UART5 |
-                           RCC_APB1Periph_I2C1 |
-                           RCC_APB1Periph_I2C2 |
-                           RCC_APB1Periph_USB |
-// RCC_APB1Periph_CAN1 | /* bxCAN unfortunately interferes with USB */
-// RCC_APB1Periph_CAN2 |
-                           RCC_APB1Periph_BKP |
-                           RCC_APB1Periph_PWR |
-                           RCC_APB1Periph_DAC,
-                           ENABLE);
-
-
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA |
-                           RCC_APB2Periph_GPIOB |
-                           RCC_APB2Periph_GPIOC |
-                           RCC_APB2Periph_GPIOD |
-                           RCC_APB2Periph_GPIOE |
-                           RCC_APB2Periph_ADC1 |
-                           RCC_APB2Periph_ADC2 |
-                           RCC_APB2Periph_ADC3 |
-                           RCC_APB2Periph_TIM1 |
-                           RCC_APB2Periph_TIM8 |
-                           RCC_APB2Periph_TIM9 |
-                           RCC_APB2Periph_TIM10 |
-                           RCC_APB2Periph_TIM11 |
-                           RCC_APB2Periph_TIM15 |
-                           RCC_APB2Periph_TIM16 |
-                           RCC_APB2Periph_TIM17 |
-                           RCC_APB2Periph_SPI1 |
-                           RCC_APB2Periph_USART1 |
-                           RCC_APB2Periph_AFIO,
-                           ENABLE);
+    /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE |
+                           RCC_APB2Periph_AFIO, ENABLE);
 
 #if (PIOS_USB_ENABLED)
     /*  Ensure that pull-up is active on detect pin */
diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c
index 4f3881ac3..5b051efad 100644
--- a/flight/pios/stm32f10x/pios_usart.c
+++ b/flight/pios/stm32f10x/pios_usart.c
@@ -211,14 +211,17 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg)
     case (uint32_t)USART1:
         PIOS_USART_1_id = (uint32_t)usart_dev;
         usart_dev->irq_channel = USART1_IRQn;
+        RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
         break;
     case (uint32_t)USART2:
         PIOS_USART_2_id = (uint32_t)usart_dev;
         usart_dev->irq_channel = USART2_IRQn;
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
         break;
     case (uint32_t)USART3:
         PIOS_USART_3_id = (uint32_t)usart_dev;
         usart_dev->irq_channel = USART3_IRQn;
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
         break;
     }