Index: gr_fxpt_nco.h
===================================================================
RCS file: /cvsroot/gnuradio/gnuradio-core/src/lib/general/gr_fxpt_nco.h,v
retrieving revision 1.1
diff -u -r1.1 gr_fxpt_nco.h
--- gr_fxpt_nco.h	19 Dec 2004 05:48:39 -0000	1.1
+++ gr_fxpt_nco.h	15 Jun 2005 22:57:18 -0000
@@ -28,32 +28,64 @@
  * \brief Numerically Controlled Oscillator (NCO)
  */
 class gr_fxpt_nco {
+  gr_int32	d_phase_origin;
   gr_int32	d_phase;
   gr_int32	d_phase_inc;
+  gr_int32	d_step_error;
+  gr_uint32	d_period;
+  gr_uint32	d_step;
+  gr_uint32	d_period_frac;
 
 public:
-  gr_fxpt_nco () : d_phase (0), d_phase_inc (0) {}
+  gr_fxpt_nco () : d_phase_origin (0), d_phase (0), d_phase_inc (0), d_step_error (0), d_period (0), d_step (0), d_period_frac (0) {}
 
   ~gr_fxpt_nco () {}
 
   // radians
   void set_phase (float angle) {
-    d_phase = gr_fxpt::float_to_fixed (angle);
+    d_phase_origin = gr_fxpt::float_to_fixed (angle);
+    d_phase = d_phase_origin;
   }
 
   void adjust_phase (float delta_phase) {
     d_phase += gr_fxpt::float_to_fixed (delta_phase);
   }
 
+#define STEP_CHECK (1U<<8)
+
+  // step_rate is in steps / period
+  void set_period (float step_rate){
+    float	phase_inc_error;
+    float	step_rate_ceil;
+    float	phase_inc;
+
+    step_rate_ceil = ceilf(step_rate);
+    d_period = (gr_int32)step_rate_ceil;
+
+    phase_inc = 2*TWO_TO_THE_31 / step_rate;
+    d_phase_inc = (gr_int32)phase_inc;
+
+    d_period_frac = (gr_int32)(phase_inc*(step_rate_ceil - step_rate));
+
+    phase_inc_error =  (2*TWO_TO_THE_31 / step_rate)  -  (float)d_phase_inc;
+    d_step_error = (gr_int32)(phase_inc_error * (float)STEP_CHECK);
+
+    d_phase_origin += d_step_error/2;
+    d_phase = d_phase_origin;
+
+  }
+
+
   // angle_rate is in radians / step
   void set_freq (float angle_rate){
-    d_phase_inc = gr_fxpt::float_to_fixed (angle_rate);
+    set_period (1/(angle_rate/(2 * M_PI)));
   }
 
   // angle_rate is a delta in radians / step
   void adjust_freq (float delta_angle_rate)
   {
     d_phase_inc += gr_fxpt::float_to_fixed (delta_angle_rate);
+    // FIXME: calc phase_inc error
   }
 
   // increment current phase angle
@@ -61,11 +93,33 @@
   void step () 
   { 
     d_phase += d_phase_inc;
+
+    ++d_step;
+    if (d_step >= d_period) {
+	    d_phase_origin -= d_period_frac;
+	    d_phase = d_phase_origin;
+	    d_step = 0;
+    } else {
+	    if ((d_step % STEP_CHECK) == 0) {
+		d_phase += d_step_error;
+    	    }
+    }
   }
 
   void step (int n)
   {
     d_phase += d_phase_inc * n;
+
+    d_step += n;
+    if (d_step >= d_period) {
+	    d_phase_origin -= d_period_frac;
+	    d_phase = d_phase_origin + d_step_error * ((d_step % d_period)/STEP_CHECK);
+	    d_step %= d_period;
+    } else {
+	    if (n >= (int)STEP_CHECK || (d_step % STEP_CHECK) == 0) {
+		d_phase += d_step_error;
+	    }
+    }
   }
 
   // units are radians / step