STM32F4

int main (void) {
    /* Initialize system */
    SystemInit();

    /* Initialize USART1, 9600baud, TX: PB6 */
    TM_USART_Init(USART1, TM_USART_PinsPack_2, 9600);

    /* Initialize delay */
    TM_DELAY_Init();

    /* MOTOR 1, TIM3, Channel 1, Pinspack 1 = PA6 */
    TM_SERVO_Init(&Motor1, TIM3, TM_PWM_Channel_1, TM_PWM_PinsPack_1);
    /* MOTOR 1, TIM3, Channel 2, Pinspack 1 = PA7 */
    TM_SERVO_Init(&Motor2, TIM3, TM_PWM_Channel_2, TM_PWM_PinsPack_1);
    /* MOTOR 1, TIM3, Channel 3, Pinspack 1 = PB0 */
    TM_SERVO_Init(&Motor3, TIM3, TM_PWM_Channel_3, TM_PWM_PinsPack_1);
    /* MOTOR 1, TIM3, Channel 4, Pinspack 1 = PB1 */
    TM_SERVO_Init(&Motor4, TIM3, TM_PWM_Channel_4, TM_PWM_PinsPack_1);

//      - TM_I2C_Pinspack_2:
//      - I2C1: SCL: PB8, SDA: PB9
    TM_I2C_Init(I2C1, TM_I2C_PinsPack_2, 400000);

    TM_MPU6050_Init(&MPU6050_Data1, TM_MPU6050_Device_1, TM_MPU6050_Accelerometer_8G, TM_MPU6050_Gyroscope_250s);

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);

    Initiate_Tim4();
    M_WATCHDOG_Init(TM_WATCHDOG_Timeout_1s);
    while (1)
    {
         TM_WATCHDOG_Reset();
    }
}


void Initiate_Tim4(void) {
    TIM_TimeBaseInitTypeDef TIM_BaseStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    TIM_BaseStruct.TIM_Prescaler = 8399;
    TIM_BaseStruct.TIM_CounterMode = TIM_CounterMode_Up;

    TIM_BaseStruct.TIM_Period = 999; /* 100 ms */
    TIM_BaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_BaseStruct.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM4, &TIM_BaseStruct);

    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);

    TIM_Cmd(TIM4, ENABLE);
}

void TIM4_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
        readMPU();
        }
}

void readMPU()
{
    TM_MPU6050_ReadAll(&MPU6050_Data1);

    accX =  MPU6050_Data1.Accelerometer_X / 182.0;
    accY =  MPU6050_Data1.Accelerometer_Y / 182.0;
    accZ =  MPU6050_Data1.Accelerometer_Z / 182.0;

    gyroX =  MPU6050_Data1.Gyroscope_X / 131.0;
    gyroY =  MPU6050_Data1.Gyroscope_Y / 131.0;
    gyroZ =  MPU6050_Data1.Gyroscope_Z / 131.0;

    roll  = atan2(accY, accZ) * RAD_TO_DEG;
    pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
}


//in defines.h

#define MPU6050_I2C            I2C2
#define MPU6050_I2C_PINSPACK    TM_I2C_PinsPack_1

#define TM_USART1_USE_CUSTOM_IRQ
#define TM_USART_NVIC_PRIORITY 0