suservo: add SI units functions and document

m-labs/artiq#788

artiq/coredevice/suservo.py

@@ -1,9 +1,6 @@
-from artiq.language.core import kernel, delay, portable, now_mu, delay_mu
+from artiq.language.core import kernel, delay, now_mu, delay_mu
-from artiq.language.units import us, ms
+from artiq.language.units import us, ns
 from artiq.coredevice.rtio import rtio_output, rtio_input_data
-
-from numpy import int32, int64
-
 from artiq.coredevice import spi2 as spi
 from artiq.coredevice import urukul, sampler
 
@@ -14,6 +11,8 @@ WE = 1 << COEFF_DEPTH + 1
 STATE_SEL = 1 << COEFF_DEPTH
 CONFIG_SEL = 1 << COEFF_DEPTH - 1
 CONFIG_ADDR = CONFIG_SEL | STATE_SEL
+F_CYCLE = 1/((2*(8 + 64) + 2 + 1)*8*ns)
+COEFF_SHIFT = 11
 
 
 class SUServo:
@@ -49,7 +48,7 @@ class SUServo:
         This method does not alter the profile configuration memory
         or the channel controls.
         """
-        self.set_config(0)
+        self.set_config(enable=0)
         delay(3*us)  # pipeline flush
 
         self.pgia.set_config_mu(
@@ -118,13 +117,16 @@ class SUServo:
 
     @kernel
     def get_adc_mu(self, adc):
-        """Get an ADC reading (IIR filter input X0).
+        """Get an ADC reading (IIR filter input X0) in machine units.
 
         This method does not advance the timeline but consumes all slack.
 
         :param adc: ADC channel number (0-7)
-        :return: 16 bit signed Y0
+        :return: 17 bit signed X0
         """
+        # State memory entries are 25 bits. Due to the pre-adder dynamic
+        # range, X0/X1/OFFSET are only 24 bits. Finally, the RTIO interface
+        # only returns the 18 MSBs (the width of the coefficient memory).
         return self.read(STATE_SEL | (adc << 1) | (1 << 8))
 
     @kernel
@@ -144,11 +146,24 @@ class SUServo:
         self.gains = gains
 
     @kernel
-    def get_adc(self, adc):
+    def get_adc(self, channel):
-        raise NotImplementedError  # FIXME
+        """Get an ADC reading (IIR filter input X0).
+
+        This method does not advance the timeline but consumes all slack.
+
+        :param adc: ADC channel number (0-7)
+        :return: ADC voltage
+        """
+        val = (self.get_adc_mu(channel) >> 1) & 0xffff
+        mask = 1 << 15
+        val = -(val & mask) + (val & ~mask)
+        gain = (self.gains >> (channel*2)) & 0b11
+        return sampler.adc_mu_to_volt(val, gain)
 
 
 class Channel:
+    """SU-Servo channel"""
+
     kernel_invariants = {"channel", "core", "servo", "servo_channel"}
 
     def __init__(self, dmgr, channel, servo_device,
@@ -165,6 +180,9 @@ class Channel:
         """Operate channel.
 
         This method does not advance the timeline.
+        Output RF switch setting takes effect immediately.
+        IIR updates take place once the RF switch has been enabled for the
+        configured delay and the profile setting has been stable.
 
         :param en_out: RF switch enable
         :param en_iir: IIR updates enable
@@ -174,54 +192,154 @@ class Channel:
                     en_out | (en_iir << 1) | (profile << 2))
 
     @kernel
-    def set_dds_mu(self, profile, ftw, offset, pow=0):
+    def set_dds_mu(self, profile, ftw, offs, pow_=0):
-        """Set profile DDS coefficients.
+        """Set profile DDS coefficients in machine units.
 
-        This method advances the timeline by four Servo memory accesses.
+        .. seealso:: :meth:`set_amplitude`
 
         :param profile: Profile number (0-31)
         :param ftw: Frequency tuning word (32 bit unsigned)
-        :param offset: IIR offset (setpoint)
+        :param offs: IIR offset (17 bit signed)
-        :param pow: Phase offset word (16 bit unsigned)
+        :param pow_: Phase offset word (16 bit)
         """
         base = (self.servo_channel << 8) | (profile << 3)
         self.servo.write(base + 0, ftw >> 16)
         self.servo.write(base + 6, ftw)
-        self.servo.write(base + 4, offset)
+        self.servo.write(base + 4, offs)
-        self.servo.write(base + 2, pow)
+        self.servo.write(base + 2, pow_)
 
     @kernel
     def set_dds(self, profile, frequency, offset, phase=0.):
-        raise NotImplementedError  # FIXME
+        """Set profile DDS coefficients.
-
-    @kernel
-    def set_iir_mu(self, profile, adc, a1, b0, b1, delay=0):
-        """Set profile IIR coefficients.
 
         This method advances the timeline by four Servo memory accesses.
+        Profile parameter changes are not synchronized. Activate a different
+        profile or stop the servo to ensure synchronous changes.
 
         :param profile: Profile number (0-31)
-        :param adc: ADC channel to use (0-7)
+        :param frequency: DDS frequency in Hz
+        :param offset: IIR offset (negative setpoint) in units of full scale.
+            For positive ADC voltages as setpoints, this should be negative.
+        :param phase: DDS phase in turns
+        """
+        if self.servo_channel < 4:
+            dds = self.servo.dds0
+        else:
+            dds = self.servo.dds1
+        ftw = dds.frequency_to_ftw(frequency)
+        pow_ = dds.turns_to_pow(phase)
+        offs = int(round(offset*(1 << COEFF_WIDTH - 1)))
+        self.set_dds_mu(profile, ftw, offs, pow_)
+
+    @kernel
+    def set_iir_mu(self, profile, adc, a1, b0, b1, dly=0):
+        """Set profile IIR coefficients in machine units.
+
+        The recurrence relation is (all data signed and MSB aligned):
+
+        .. math::
+            a_0 y_n = a_1 y_{n - 1} + b_0 (x_n + o)/2 + b_1 (x_{n - 1} + o)/2
+
+        Where:
+
+            * :math:`y_n` and :math:`y_{n-1}` are the current and previous
+                filter outputs, clipped to :math:`[0, 1]`.
+            * :math:`x_n` and :math:`x_{n-1}` are the current and previous
+                filter inputs
+            * :math:`o` is the offset
+            * :math:`a_0` is the normalization factor :math:`2^{11}`
+            * :math:`a_1` is the feedback gain
+            * :math:`b_0` and :math:`b_1` are the feedforward gains for the two
+                delays
+
+        .. seealso:: :meth:`set_iir`
+
+        :param profile: Profile number (0-31)
+        :param adc: ADC channel to take IIR input from (0-7)
         :param a1: 18 bit signed A1 coefficient (Y1 coefficient,
             feedback, integrator gain)
         :param b0: 18 bit signed B0 coefficient (recent,
             X0 coefficient, feed forward, proportional gain)
         :param b1: 18 bit signed B1 coefficient (old,
             X1 coefficient, feed forward, proportional gain)
-        :param delay: Number of Servo cycles (~1.1 µs each) to suppress
+        :param dly: IIR update suppression time. In units of IIR cycles
-            IIR updates for after either (1) enabling or disabling RF output,
+            (~1.1 µs)
-            (2) enabling or disabling IIR updates, or (3) setting the active
-            profile number: i.e. after invoking :meth:`set`.
         """
         base = (self.servo_channel << 8) | (profile << 3)
+        self.servo.write(base + 3, adc | (dly << 8))
         self.servo.write(base + 1, b1)
-        self.servo.write(base + 3, adc | (delay << 8))
         self.servo.write(base + 5, a1)
         self.servo.write(base + 7, b0)
 
     @kernel
-    def set_iir(self, profile, adc, i_gain, p_gain, delay=0.):
+    def set_iir(self, profile, adc, gain, corner=0., limit=0., delay=0.):
-        raise NotImplementedError  # FIXME
+        """Set profile IIR coefficients.
+
+        This method advances the timeline by four Servo memory accesses.
+        Profile parameter changes are not synchronized. Activate a different
+        profile or stop the servo to ensure synchronous changes.
+
+        Gains are given in units of output full per scale per input full scale.
+
+        The transfer function is (up to time discretization and
+        coefficient quantization errors):
+
+        .. math::
+            H(s) = K \\frac{1 + \\frac{s}{\\omega_0}}
+                {\\frac{1}{g} + \\frac{s}{\\omega_0}}
+
+        Where:
+            * :math:`s = \\sigma + i\\omega` is the complex frequency
+            * :math:`K` is the proportional gain
+            * :math:`\\omega_0` is the integrator corner frequency
+            * :math:`g` is the integrator gain limit
+
+        :param profile: Profile number (0-31)
+        :param adc: ADC channel to take IIR input from (0-7)
+        :param gain: Proportional gain (1). This is usually negative (closed
+            loop, positive ADC voltage, positive setpoint).
+        :param corner: Integrator corner frequency (Hz). When 0 (the default)
+            this implements a pure P controller.
+        :param limit: Integrator gain limit (1). When 0 (the default) the
+            integrator gain limit is infinite.
+        :param delay: Delay (in seconds) before allowing IIR updates after
+            invoking :meth:`set`.
+        """
+        B_NORM = 1 << COEFF_SHIFT + 1
+        A_NORM = 1 << COEFF_SHIFT
+        PI_TS = 3.1415927/F_CYCLE
+        COEFF_MAX = 1 << COEFF_WIDTH - 1
+
+        k = B_NORM*gain
+        if corner == 0.:
+            a1_ = 0
+            b0_ = int(round(k))
+            b1_ = 0
+        else:
+            q = PI_TS*corner
+            kq = k*q
+
+            a1_ = A_NORM
+            b0 = kq + k
+            b1 = kq - k
+            if limit != 0.:
+                ql = q/limit
+                qlr = 1./(1. + ql)
+                a1_ = int(round(a1_*(1. - ql)*qlr))
+                b0 *= qlr
+                b0 *= qlr
+            b0_ = int(round(b0))
+            b1_ = int(round(b1))
+
+            if b1_ == -b0_:
+                raise ValueError("low corner, gain, limit")
+
+        if (b0_ >= COEFF_MAX or b0_ < -COEFF_MAX or
+                b1_ >= COEFF_MAX or b1_ < -COEFF_MAX):
+            raise ValueError("high corner, gain, limit")
+
+        dly = int(round(delay*F_CYCLE))
+        self.set_iir_mu(profile, adc, a1_, b0_, b1_, dly)
 
     @kernel
     def get_profile_mu(self, profile, data):
@@ -231,6 +349,7 @@ class Channel:
         `[ftw >> 16, b1, pow, adc | (delay << 8), offset, a1, ftw, b0]`.
 
         This method advances the timeline by 32 µs and consumes all slack.
+        Profile data is returned 
 
         :param profile: Profile number (0-31)
         :param data: List of 8 integers to write the profile data into
@@ -242,7 +361,7 @@ class Channel:
 
     @kernel
     def get_y_mu(self, profile):
-        """Get a profile's IIR state (filter output, Y0).
+        """Get a profile's IIR state (filter output, Y0) in machine units.
 
         The IIR state is also know as the "integrator", or the DDS amplitude
         scale factor. It is 18 bits wide and unsigned.
@@ -256,10 +375,40 @@ class Channel:
 
     @kernel
     def get_y(self, profile):
-        raise NotImplementedError  # FIXME
+        """Get a profile's IIR state (filter output, Y0).
+
+        The IIR state is also know as the "integrator", or the DDS amplitude
+        scale factor. It is 18 bits wide and unsigned.
+
+        This method does not advance the timeline but consumes all slack.
+
+        :param profile: Profile number (0-31)
+        :return: IIR filter output in Y0 units of full scale
+        """
+        return self.get_y_mu(profile)*(1./(1 << COEFF_WIDTH - 1))
 
     @kernel
     def set_y_mu(self, profile, y):
+        """Set a profile's IIR state (filter output, Y0) in machine units.
+
+        The IIR state is also know as the "integrator", or the DDS amplitude
+        scale factor. It is 18 bits wide and unsigned.
+
+        This method must not be used when the Servo
+        could be writing to the same location. Either deactivate the profile,
+        or deactivate IIR updates, or disable Servo iterations.
+
+        This method advances the timeline by one Servo memory access.
+
+        :param profile: Profile number (0-31)
+        :param y: 17 bit unsigned Y0
+        """
+        # State memory is 25 bits wide and signed.
+        # Reads interact with the 18 MSBs (coefficient memory width)
+        self.servo.write(STATE_SEL | (self.servo_channel << 5) | profile, y)
+
+    @kernel
+    def set_y(self, profile, y):
         """Set a profile's IIR state (filter output, Y0).
 
         The IIR state is also know as the "integrator", or the DDS amplitude
@@ -272,11 +421,6 @@ class Channel:
         This method advances the timeline by one Servo memory access.
 
         :param profile: Profile number (0-31)
-        :param y: 18 bit unsigned Y0
+        :param y: IIR state in units of full scale
         """
-        return self.servo.write(
+        self.set_y_mu(profile, int(round((1 << COEFF_WIDTH - 1)*y)))
-            STATE_SEL | (self.servo_channel << 5) | profile, y)
-
-    @kernel
-    def set_y(self, profile, y):
-        raise NotImplementedError  # FIXME

artiq/examples/kasli_suservo/repository/suservo.py

@@ -10,40 +10,47 @@ class SUServo(EnvExperiment):
             self.setattr_device("suservo0_ch{}".format(i))
 
     def run(self):
-        # self.led()
         self.init()
 
     def p(self, d):
-        for name, value in zip("ftw1 b1 pow cfg offset a1 ftw0 b0".split(), d):
+        mask = 1 << 18 - 1
-            print(name, hex(value))
+        for name, val in zip("ftw1 b1 pow cfg offset a1 ftw0 b0".split(), d):
+            val = -(val & mask) + (val & ~mask)
+            print(name, hex(val), val)
+
+    @rpc(flags={"async"})
+    def p1(self, adc, asf, st):
+        print("{:10s}".format("#"*int(adc*10)))
 
     @kernel
     def init(self):
         self.core.break_realtime()
         self.core.reset()
+        self.led()
 
         self.suservo0.init()
         delay(1*us)
         # ADC PGIA gain
-        self.suservo0.set_pgia_mu(0, 0)
+        for i in range(8):
+            self.suservo0.set_pgia_mu(i, 0)
+            delay(10*us)
         # DDS attenuator
-        self.suservo0.cpld0.set_att_mu(0, 64)
+        self.suservo0.cpld0.set_att(0, 10.)
         delay(1*us)
-        # Servo is done
+        # Servo is done and disabled
         assert self.suservo0.get_status() & 0xff == 2
-        delay(10*us)
 
         # set up profile 0 on channel 0
-        self.suservo0_ch0.set_y_mu(0, 0)
+        delay(100*us)
-        self.suservo0_ch0.set_iir_mu(
+        self.suservo0_ch0.set_y(0, 0.)
-                profile=0, adc=0, a1=-0x800, b0=0x1000, b1=0, delay=0)
+        self.suservo0_ch0.set_iir(
-        delay(10*us)
+                profile=0, adc=7, gain=-.1, corner=7000*Hz, limit=0., delay=0.)
-        self.suservo0_ch0.set_dds_mu(
+        self.suservo0_ch0.set_dds(
-                profile=0, ftw=0x12345667, offset=0x1000, pow=0xaa55)
+                profile=0, offset=-.5, frequency=71*MHz, phase=0.)
         # enable channel
         self.suservo0_ch0.set(en_out=1, en_iir=1, profile=0)
         # enable servo iterations
-        self.suservo0.set_config(1)
+        self.suservo0.set_config(enable=1)
 
         # read back profile data
         data = [0] * 8
@@ -51,40 +58,26 @@ class SUServo(EnvExperiment):
         self.p(data)
         delay(10*ms)
 
-        # check servo status
+        # check servo enabled
-        assert self.suservo0.get_status() & 0xff == 1
+        assert self.suservo0.get_status() & 0x01 == 1
         delay(10*us)
 
-        # reach back ADC data
-        assert self.suservo0.get_adc_mu(0) == 0
-        delay(10*us)
-
-        # read out IIR data
-        assert self.suservo0_ch0.get_y_mu(0) == 0x1ffff
-        delay(10*us)
-
-        assert self.suservo0.get_status() & 0xff00 == 0x0100
-        delay(10*us)
-
-        # repeatedly clear the IIR state/integrator
-        # with the ADC yielding 0's and given the profile configuration,
-        # this will lead to a slow ram up of the amplitude over about 40µs
-        # followed by saturation and repetition.
         while True:
-            self.suservo0_ch0.set(1, 0, 0)
+            self.suservo0.set_config(0)
-            delay(1*us)
-            assert self.suservo0.get_status() & 0xff00 == 0x0100
             delay(10*us)
-            assert self.suservo0.get_status() & 0xff00 == 0x0000
+            v = self.suservo0.get_adc(7)
+            delay(30*us)
+            w = self.suservo0_ch0.get_y(0)
+            delay(20*us)
+            x = self.suservo0.get_status()
             delay(10*us)
-            self.suservo0_ch0.set_y_mu(0, 0)
+            self.suservo0.set_config(1)
-            delay(1*us)
+            self.p1(v, w, x)
-            self.suservo0_ch0.set(1, 1, 0)
+            delay(200*ms)
-            delay(60*us)
 
     @kernel
     def led(self):
         self.core.break_realtime()
-        for i in range(10):
+        for i in range(3):
             self.led0.pulse(.1*s)
             delay(.1*s)

artiq/gateware/suservo/iir.py

@@ -191,8 +191,8 @@ class IIR(Module):
     ADC: input value from the ADC
     ASF: output amplitude scale factor to DDS
     OFFSET: setpoint
-    A0: fixed factor
+    A0: fixed factor (normalization)
-    A1/B0/B1: coefficients
+    A1/B0/B1: coefficients (integers)
 
                     B0 --/-   A0: 2^11
                         18 |         |

artiq/gateware/suservo/servo.py

@@ -11,6 +11,7 @@ class Servo(Module):
         self.submodules.iir = IIR(iir_p)
         self.submodules.dds = DDS(dds_pads, dds_p)
 
+        # adc channels are reversed on Sampler
         for i, j, k, l in zip(reversed(self.adc.data), self.iir.adc,
                 self.iir.dds, self.dds.profile):
             self.comb += j.eq(i), l.eq(k)