stm32/servo: Only initialise TIM5 if it is needed, to save power.

1 files changed, 4 insertions, 2 deletions

diff --git a/ports/stm32/servo.c b/ports/stm32/servo.c
index 4eb5b3273..691c8037f 100644
--- a/ports/stm32/servo.c
+++ b/ports/stm32/servo.c
@@ -63,8 +63,6 @@ typedef struct _pyb_servo_obj_t {
 STATIC pyb_servo_obj_t pyb_servo_obj[PYB_SERVO_NUM];
 
 void servo_init(void) {
-    timer_tim5_init();
-
     // reset servo objects
     for (int i = 0; i < PYB_SERVO_NUM; i++) {
         pyb_servo_obj[i].base.type = &pyb_servo_type;
@@ -133,6 +131,10 @@ STATIC void servo_init_channel(pyb_servo_obj_t *s) {
     // GPIO configuration
     mp_hal_pin_config(s->pin, MP_HAL_PIN_MODE_ALT, MP_HAL_PIN_PULL_NONE, GPIO_AF2_TIM5);
 
+    if (__HAL_RCC_TIM5_IS_CLK_DISABLED()) {
+        timer_tim5_init();
+    }
+
     // PWM mode configuration
     TIM_OC_InitTypeDef oc_init;
     oc_init.OCMode = TIM_OCMODE_PWM1;