servo.c

#include "ch.h"
#include "hal.h"
#include "servo.h"

#define SERVO_PWM PWMD3

static PWMConfig pwm_servo = {
	100000, /* 100Khz PWM clock frequency*/
	2000, /* PWM period of 2000 ticks ~ 20ms */
	NULL, /* No callback at the end of the timer count */
	{
		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
		{PWM_OUTPUT_DISABLED, NULL},
		{PWM_OUTPUT_DISABLED, NULL},
		{PWM_OUTPUT_DISABLED, NULL}
	},
	0, /* CR2 register */
	0  /* DIER register */
};

void servo_init(void){
	palSetPadMode(GPIOB, 4, PAL_MODE_ALTERNATE(2)); // used function : TIM3_CH1
	pwmStart(&SERVO_PWM, &pwm_servo);
}

/*
 * Sets the servo angle between -90° and 90°
 */
void set_servo(int angle){
	// Moves the target angle in the proper range as 0° is 1520µs
	angle+=152;
	pwmEnableChannel(&SERVO_PWM, 0, angle);
}

servo.h

#ifndef SERVO_H
#define SERVO_H

void servo_init(void);
void set_servo(int angle);

#endif