softpll_ng: phase shift bugfix

dev/softpll_ng.c

@@ -151,7 +151,7 @@ int softpll_busy(int channel)
 
 void softpll_set_phase( int ps)
 {
-	mpll.phase_shift_target = ((int32_t) ((int64_t)ps * (long long)(1<<HPLL_N) / 8000LL));
+    mpll_set_phase_shift(&mpll, ((int32_t) ((int64_t)ps * (long long)(1<<HPLL_N) / 8000LL)));
 }
 
 void softpll_disable()
@@ -164,3 +164,18 @@ int softpll_get_setpoint()
   return mpll.phase_shift_target;
 }
 
+
+void softpll_test()
+{
+    softpll_enable();
+    for(;;)
+    {
+        mprintf("L!\n\n");
+        softpll_set_phase(1000000);
+        while(softpll_busy(0)) mprintf("%d %d %d%d %d %d\n", mpll.phase_shift_current, mpll.phase_shift_target, mpll.ld.locked, helper.ld.locked, lerr, ly);
+        mprintf("R!\n\n");
+        softpll_set_phase(10000);
+        while(softpll_busy(0)) mprintf("%d %d %d%d %d\n", mpll.phase_shift_current, mpll.phase_shift_target, mpll.ld.locked, helper.ld.locked, lerr);
+        
+    }
+}
\ No newline at end of file

include/spll_defs.h

@@ -35,4 +35,8 @@ WARNING: These parameters must be in sync with the generics of the HDL instantia
 
 /* Number of bits of the DAC(s) driving the oscillator(s). Must be the same for
    all the outputs. */
-#define DAC_BITS 16
\ No newline at end of file
+#define DAC_BITS 16
+
+/* 1.0 / (Speed of the phase shifter) - the higher value, the slower phase shifting.
+   Used to prevent de-locking PLLs when shifting large offsets. */
+#define PHASE_SHIFTER_SPEED 1
\ No newline at end of file

include/spll_main.h

 
-#define MPLL_TAG_WRAPAROUND 100000000
+#define MPLL_TAG_WRAPAROUND 1000000000
 
 /* State of the Main PLL */
 struct spll_main_state {
@@ -13,8 +13,10 @@ struct spll_main_state {
  	int phase_shift_current;
 	int id_ref, id_out; /* IDs of the reference and the output channel */
 	int sample_n;
+	int shift_div;
 };
 
+volatile int lerr = 0, ly=0;
 
 static void mpll_init(struct spll_main_state *s, int id_ref, int id_out)
 {
@@ -41,7 +43,7 @@ static void mpll_init(struct spll_main_state *s, int id_ref, int id_out)
 	s->id_ref = id_ref;
 	s->id_out = id_out;
 	s->sample_n=  0;
-
+    s->shift_div = 0;
 	pi_init(&s->pi);
 	ld_init(&s->ld);
 }
@@ -68,38 +70,61 @@ static int mpll_update(struct spll_main_state *s, int tag, int source)
 			s->adder_ref += (1<<TAG_BITS);
 		if(s->tag_out_d >= 0 && s->tag_out_d > s->tag_out)
 			s->adder_out += (1<<TAG_BITS);
-			
-		s->tag_ref_d = s->tag_ref;
-		s->tag_out_d = s->tag_out;
-	
+
 		err = s->adder_ref + s->tag_ref -  s->adder_out - s->tag_out;
-		
+
 		if(s->adder_ref > MPLL_TAG_WRAPAROUND && s->adder_out > MPLL_TAG_WRAPAROUND)
 		{
 			s->adder_ref -= MPLL_TAG_WRAPAROUND;
 			s->adder_out -= MPLL_TAG_WRAPAROUND;
 		}
+
+
+         /* Hack: the PLL is locked, so the tags are close to each other. But when we start phase shifting, after reaching
+         full clock period, one of the reference tags will flip before the other, causing a suddent 2**HPLL_N jump in the error.
+         So, once the PLL is locked, we just mask out everything above 2**HPLL_N.
+         
+         Proper solution: tag sequence numbers */
+		if(s->ld.locked)
+		{
+		    err &= (1<<HPLL_N)-1;
+		    if(err & (1<<(HPLL_N-1)))
+		        err |= ~((1<<HPLL_N)-1);
+		}
+		
 		
     y = pi_update(&s->pi, err);
+    lerr = err;
+    ly=y;
 		SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y) | SPLL_DAC_MAIN_DAC_SEL_W(s->id_out);
 
-		spll_debug(DBG_MAIN | DBG_REF, s->tag_ref, 0);
-		spll_debug(DBG_MAIN | DBG_TAG, s->tag_out, 0);
 		spll_debug(DBG_MAIN | DBG_ERR, err, 0);
 		spll_debug(DBG_MAIN | DBG_SAMPLE_ID, s->sample_n++, 0);
 		spll_debug(DBG_MAIN | DBG_Y, y, 1);
+		
+		s->tag_ref_d = s->tag_ref;
+		s->tag_out_d = s->tag_out;
+
 
 		s->tag_out = -1;
 		s->tag_ref = -1;	
 
 	  if(s->phase_shift_current < s->phase_shift_target)
 	  {
-      s->phase_shift_current++;
-      s->adder_ref++;
-		} else if(s->phase_shift_current > s->phase_shift_target) {
-      s->phase_shift_current--;
-      s->adder_ref--;
-		}	
+        if(s->shift_div == PHASE_SHIFTER_SPEED-1)
+        {
+          s->phase_shift_current++;
+          s->adder_ref++;
+          s->shift_div = 0;
+        } else s->shift_div++;
+      } else if(s->phase_shift_current > s->phase_shift_target) {
+        if(s->shift_div == PHASE_SHIFTER_SPEED-1)
+        {
+          s->phase_shift_current--;
+          s->adder_ref--;
+          s->shift_div = 0;
+        } else s->shift_div++;
+	  }	
 		
    	if(ld_update(&s->ld, err))
    		return SPLL_LOCKED;
@@ -111,6 +136,7 @@ static int mpll_update(struct spll_main_state *s, int tag, int source)
 
 static int mpll_set_phase_shift(struct spll_main_state *s, int desired_shift)
 {
+    s->shift_div = 0;
 	s->phase_shift_target = desired_shift;
 }