21 lines
398 B
C
21 lines
398 B
C
#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;
|
|
}
|
|
|