#include <stdint.h>

#ifdef __XC16
#include <libq.h>
#include <dsp.h>
#endif

#include "demodulator.h"

int pll_frequency_servo(fractional frequency_error) {
   fractional frequency_servo_feedback;

   if (_Q15abs(frequency_error) < 1) frequency_servo_feedback = frequency_error;
   else {
      frequency_servo_feedback =  _Q15shr(frequency_error, 6);
   }

   return frequency_servo_feedback;
}