diff --git a/artiq/coredevice/suservo.py b/artiq/coredevice/suservo.py index c5aa6f2d1..ef7698313 100644 --- a/artiq/coredevice/suservo.py +++ b/artiq/coredevice/suservo.py @@ -1,9 +1,6 @@ -from artiq.language.core import kernel, delay, portable, now_mu, delay_mu -from artiq.language.units import us, ms +from artiq.language.core import kernel, delay, now_mu, delay_mu +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): - raise NotImplementedError # FIXME + def get_adc(self, channel): + """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): - """Set profile DDS coefficients. + def set_dds_mu(self, profile, ftw, offs, pow_=0): + """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 pow: Phase offset word (16 bit unsigned) + :param offs: IIR offset (17 bit signed) + :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 + 2, pow) + self.servo.write(base + 4, offs) + self.servo.write(base + 2, pow_) @kernel def set_dds(self, profile, frequency, offset, phase=0.): - raise NotImplementedError # FIXME - - @kernel - def set_iir_mu(self, profile, adc, a1, b0, b1, delay=0): - """Set profile IIR coefficients. + """Set profile DDS 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 - IIR updates for after either (1) enabling or disabling RF output, - (2) enabling or disabling IIR updates, or (3) setting the active - profile number: i.e. after invoking :meth:`set`. + :param dly: IIR update suppression time. In units of IIR cycles + (~1.1 µs) """ 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.): - raise NotImplementedError # FIXME + def set_iir(self, profile, adc, gain, corner=0., limit=0., 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. + + 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( - STATE_SEL | (self.servo_channel << 5) | profile, y) - - @kernel - def set_y(self, profile, y): - raise NotImplementedError # FIXME + self.set_y_mu(profile, int(round((1 << COEFF_WIDTH - 1)*y))) diff --git a/artiq/examples/kasli_suservo/repository/suservo.py b/artiq/examples/kasli_suservo/repository/suservo.py index cf49a77fb..9074bdfbe 100644 --- a/artiq/examples/kasli_suservo/repository/suservo.py +++ b/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): - print(name, hex(value)) + mask = 1 << 18 - 1 + 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) - self.suservo0_ch0.set_iir_mu( - profile=0, adc=0, a1=-0x800, b0=0x1000, b1=0, delay=0) - delay(10*us) - self.suservo0_ch0.set_dds_mu( - profile=0, ftw=0x12345667, offset=0x1000, pow=0xaa55) + delay(100*us) + self.suservo0_ch0.set_y(0, 0.) + self.suservo0_ch0.set_iir( + profile=0, adc=7, gain=-.1, corner=7000*Hz, limit=0., delay=0.) + self.suservo0_ch0.set_dds( + 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 - assert self.suservo0.get_status() & 0xff == 1 + # check servo enabled + 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) - delay(1*us) - assert self.suservo0.get_status() & 0xff00 == 0x0100 + self.suservo0.set_config(0) 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) - delay(1*us) - self.suservo0_ch0.set(1, 1, 0) - delay(60*us) + self.suservo0.set_config(1) + self.p1(v, w, x) + delay(200*ms) @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) diff --git a/artiq/gateware/suservo/iir.py b/artiq/gateware/suservo/iir.py index 33ec96c99..72501afae 100644 --- a/artiq/gateware/suservo/iir.py +++ b/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 - A1/B0/B1: coefficients + A0: fixed factor (normalization) + A1/B0/B1: coefficients (integers) B0 --/- A0: 2^11 18 | | diff --git a/artiq/gateware/suservo/servo.py b/artiq/gateware/suservo/servo.py index 34b485d06..b19a6e73d 100644 --- a/artiq/gateware/suservo/servo.py +++ b/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)