void PLL_step(PLL_TypeDef *p) {
    float err = *p->th - p->thf - p->S * p->dt;
    utils_norm_angle_rad(&err);
    p->thf += (p->S + p->Kp * err) * p->dt;
    utils_norm_angle_rad(&p->thf);
    p->S += p->Ki * err * p->dt;
}