How to implement PLL in STM32?

[zeekzhen]

I am trying to implement a Phase Locked Loop in a microcontroller STM32. I have a reference signal and feedback signal which has $\phi$ difference wrt reference signal. For the moment, I am not using a real time feedback signal, rather simulating it inside the microcontroller itself, but later extend this to Real time. 

1. I have a timer interrupt to generate a reference signal where sampling rate = 100KHz, and the target frequency = 1KHz. 

2. I used a DDS inside this timer which has a phase accumulator and a LUT for both reference and shifted sine. 

3. In the beginning when no control loop is running, the ref and feedback would be generated. 

4. now I implement a PLL, for that I have a multiplier phase detector and i use averaging integrator to remove the higher frequency components to get error. 

5. Here, I also implement a PI control loop which runs at a slower clock rate compared to the timer interrupt. so I take for example, 50*1 sine period steps. I send the error to the PI control when the triggers starts(as it reaches 5000 steps). 

6. In the PI loop, i calculated error and also the sign of the phase difference if it is laging or leading. Calculating the sign of theta with cos was tricky so I used sign. 

7. now I calculate the PI output and send to the interrupt handler again. 

The problem that I am facing is, the feedback signal keeps on shifting and when aligns with the reference signal, it is not locked. It starts shifting continuously. 

what am I doing wrong here? I assume the thing that i am doing wrong is that I am taking output_val2 from a LUT and it will be changing continuously even if the phases are locked. But I could be wrong. 

Also, if want to extend this to a real time, suppose my output_val2 is not from a LUT but directly from a ADC, how do I modify it? 

 void timer_interrupt(){

   // Check which version of the timer triggered this callback and toggle LED

   //

 static int64_t phase1 = 0;

static int64_t phase2 = 0;

static uint64_t temp = 0;

static int error = 0;

static int idx1 = 0;

static int idx2 = 0;

static int output_val1 = 0;

static int output_val2 = 0;

 // Check which version of the timer triggered this callback and toggle LED

 //

if (htim == &htim16 )

 {

//initializing variables

//implementing DDS for VCO of PLL

 phase1 += tuning; //the phase for reference signal

 phase2 += tuning+ tuning_word_int;//the phase for feedback signal

 tuning_word_int = 0; //after you get tuning_word_int for once, you set it to zero until next time it becomes nonzero

 //the tuning_word_int is the correction from PI above. It is zero before the start of control loop

 idx1 = (phase1>> 20) & 0xFFF;  //the index for reference

 idx2 = ((phase2>> 20) & 0xFFF) ; //the index for feedback

 output_val1 = LUT_r[idx1]; //the output value of the indices on LUT

 if(i == 0){  //for the first time when the feedback signal is taken from the LUT_s

 output_val2 = LUT_s[idx2];

 }

 else if(i == 1){ //once the PI loop starts, the feedback signal would now be the

 output_val2 = LUT_r[idx2];

 }

 if(PI_loop != 1){ //when no PI loop is running, only then start Phase detector

 static int p = 0;

 //.............................................

 //start of the phase detector

 //use of multiplier phase detector

 error = output_val1 * output_val2;

 //incrementing error to remove the higher frequency component. It is the mean averaging is done in while loop.

 temp += error;

 //triggers a counter to send signal to the PI controller

 if(p >= 500000-1){ // 5000 is the number of steps for averaging or say to start thr trigger

     counter = temp;

     p = 0; //resetting the inner counter

     temp = 0; //resetting the temp value

     PI_loop = 1;

 }

   p++;

 }

//...........................................

 //waveform display

 //to send the value to the oscilloscope

}

HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, output_val1);

HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_2, DAC_ALIGN_12B_R, output_val2);

 }

  int main(){

 while (1)

 {

 //phase detector part

 //if PI loop is on, perform the calculation to find the error

 if(PI_loop == 1 ){

 test++;

 //this is the integrated product from Phase detector

 pd_out = 2 * (counter / ( 500000*AMP * AMP)-1);

 //phase wrapping, sign test

 err = acos(pd_out);

//.....................................................

 //PI loop

 phase_inc += err/Fs ;

 //output of PI loop in terms of phase error

 phase_error = Kp * err + Ki * phase_inc;

 i = 1;

 PI_loop = 0;

 //to update the phase accumulator for feedback signal

 tuning_word_int = (int64_t)((phase_error)*(double)(1LL << 32));

 }

  /* USER CODE END WHILE */

 }

  /* USER CODE BEGIN 3 */

 /* USER CODE END 3 */

}

LUT_s is a shifted sine table. 

Output

https://imgur.com/a/FhddqOv

PS: The frequency is also oscillating around the original frequency and not locking. 

for the phase detection, i followed this post, rf - Phase Detector for PLL: Operation and Realization - Electrical Engineering Stack Exchange. This is how I got acos.

to sync phase_inc, i have used some triggers as updated in the code. I am just unsure if the PI output fed to the DDS in timer interrupt is correct. I assumed the first time, the feedback would be my shifted output from LUT_s, and once the PI runs, the phase would run into LUT_r(which is the refernce LUT with no phase)

The problem that I see now is, the while loop is for some reason stuck after a couple of steps. The ref and feedback are locked but at err Pi/2. It should be at err = 0.

One more thing I am doubtful about is, suppose when my integrator finished with 5000 or say any number steps, and send the error value to the PI loop, it takes some time to complete the PI loop, by that time, the DDS would already be starting. But this DDS wouldn't include the PI output in the feedback signal.