mirror of
https://github.com/m-labs/artiq.git
synced 2024-12-25 19:28:26 +08:00
suservo: support pure-I
This commit is contained in:
parent
ae50196186
commit
a100c73dfe
@ -11,7 +11,7 @@ WE = 1 << COEFF_DEPTH + 1
|
|||||||
STATE_SEL = 1 << COEFF_DEPTH
|
STATE_SEL = 1 << COEFF_DEPTH
|
||||||
CONFIG_SEL = 1 << COEFF_DEPTH - 1
|
CONFIG_SEL = 1 << COEFF_DEPTH - 1
|
||||||
CONFIG_ADDR = CONFIG_SEL | STATE_SEL
|
CONFIG_ADDR = CONFIG_SEL | STATE_SEL
|
||||||
F_CYCLE = 1/((2*(8 + 64) + 2 + 1)*8*ns)
|
T_CYCLE = (2*(8 + 64) + 2 + 1)*8*ns
|
||||||
COEFF_SHIFT = 11
|
COEFF_SHIFT = 11
|
||||||
|
|
||||||
|
|
||||||
@ -277,7 +277,7 @@ class Channel:
|
|||||||
:param b1: 18 bit signed B1 coefficient (old,
|
:param b1: 18 bit signed B1 coefficient (old,
|
||||||
X1 coefficient, feed forward, proportional gain)
|
X1 coefficient, feed forward, proportional gain)
|
||||||
:param dly: IIR update suppression time. In units of IIR cycles
|
:param dly: IIR update suppression time. In units of IIR cycles
|
||||||
(~1.1 µs)
|
(~1.2 µs, 0-255)
|
||||||
"""
|
"""
|
||||||
base = (self.servo_channel << 8) | (profile << 3)
|
base = (self.servo_channel << 8) | (profile << 3)
|
||||||
self.servo.write(base + 3, adc | (dly << 8))
|
self.servo.write(base + 3, adc | (dly << 8))
|
||||||
@ -305,45 +305,55 @@ class Channel:
|
|||||||
Where:
|
Where:
|
||||||
* :math:`s = \\sigma + i\\omega` is the complex frequency
|
* :math:`s = \\sigma + i\\omega` is the complex frequency
|
||||||
* :math:`K` is the proportional gain
|
* :math:`K` is the proportional gain
|
||||||
* :math:`\\omega_0` is the integrator corner frequency
|
* :math:`\\omega_0 = 2\\pi f_0` is the integrator corner frequency
|
||||||
* :math:`g` is the integrator gain limit
|
* :math:`g` is the integrator gain limit
|
||||||
|
|
||||||
:param profile: Profile number (0-31)
|
:param profile: Profile number (0-31)
|
||||||
:param adc: ADC channel to take IIR input from (0-7)
|
:param adc: ADC channel to take IIR input from (0-7)
|
||||||
:param gain: Proportional gain (1). This is usually negative (closed
|
:param gain: Proportional gain (1). This is usually negative (closed
|
||||||
loop, positive ADC voltage, positive setpoint).
|
loop, positive ADC voltage, positive setpoint). When 0, this
|
||||||
|
implements a pure I controller with unit gain frequency at
|
||||||
|
`corner` (use the sign of `corner` for overall gain sign).
|
||||||
:param corner: Integrator corner frequency (Hz). When 0 (the default)
|
:param corner: Integrator corner frequency (Hz). When 0 (the default)
|
||||||
this implements a pure P controller.
|
this implements a pure P controller.
|
||||||
:param limit: Integrator gain limit (1). When 0 (the default) the
|
:param limit: Integrator gain limit (1). When 0 (the default) the
|
||||||
integrator gain limit is infinite.
|
integrator gain limit is infinite. Positive.
|
||||||
:param delay: Delay (in seconds) before allowing IIR updates after
|
:param delay: Delay (in seconds, 0-300 µs) before allowing IIR updates
|
||||||
invoking :meth:`set`.
|
after invoking :meth:`set`.
|
||||||
"""
|
"""
|
||||||
B_NORM = 1 << COEFF_SHIFT + 1
|
B_NORM = 1 << COEFF_SHIFT + 1
|
||||||
A_NORM = 1 << COEFF_SHIFT
|
A_NORM = 1 << COEFF_SHIFT
|
||||||
PI_TS = 3.1415927/F_CYCLE
|
PI_TS = 3.1415927*T_CYCLE
|
||||||
COEFF_MAX = 1 << COEFF_WIDTH - 1
|
COEFF_MAX = 1 << COEFF_WIDTH - 1
|
||||||
|
|
||||||
k = B_NORM*gain
|
gain *= B_NORM
|
||||||
if corner == 0.:
|
corner *= PI_TS
|
||||||
a1_ = 0
|
|
||||||
b0_ = int(round(k))
|
|
||||||
b1_ = 0
|
|
||||||
else:
|
|
||||||
q = PI_TS*corner
|
|
||||||
kq = k*q
|
|
||||||
|
|
||||||
|
if corner == 0.:
|
||||||
|
# pure P
|
||||||
|
a1_ = 0
|
||||||
|
b1_ = 0
|
||||||
|
b0_ = int(round(gain))
|
||||||
|
else:
|
||||||
a1_ = A_NORM
|
a1_ = A_NORM
|
||||||
b0 = kq + k
|
if gain == 0.:
|
||||||
b1 = kq - k
|
# pure I
|
||||||
if limit != 0.:
|
b0 = (2*B_NORM)*corner
|
||||||
ql = q/limit
|
b1_ = 0
|
||||||
qlr = 1./(1. + ql)
|
else:
|
||||||
a1_ = int(round(a1_*(1. - ql)*qlr))
|
# PI
|
||||||
b0 *= qlr
|
k = gain*corner
|
||||||
b0 *= qlr
|
b1 = k - gain
|
||||||
|
b0 = k + gain
|
||||||
|
if limit != 0.:
|
||||||
|
# PI with limit
|
||||||
|
q = corner/limit
|
||||||
|
qr = 1./(1. + q)
|
||||||
|
a1_ = int(round(a1_*(1. - q)*qr))
|
||||||
|
b0 *= qr
|
||||||
|
b1 *= qr
|
||||||
|
b1_ = int(round(b1))
|
||||||
b0_ = int(round(b0))
|
b0_ = int(round(b0))
|
||||||
b1_ = int(round(b1))
|
|
||||||
|
|
||||||
if b1_ == -b0_:
|
if b1_ == -b0_:
|
||||||
raise ValueError("low corner, gain, limit")
|
raise ValueError("low corner, gain, limit")
|
||||||
@ -352,7 +362,7 @@ class Channel:
|
|||||||
b1_ >= COEFF_MAX or b1_ < -COEFF_MAX):
|
b1_ >= COEFF_MAX or b1_ < -COEFF_MAX):
|
||||||
raise ValueError("high corner, gain, limit")
|
raise ValueError("high corner, gain, limit")
|
||||||
|
|
||||||
dly = int(round(delay*F_CYCLE))
|
dly = int(round(delay/T_CYCLE))
|
||||||
self.set_iir_mu(profile, adc, a1_, b0_, b1_, dly)
|
self.set_iir_mu(profile, adc, a1_, b0_, b1_, dly)
|
||||||
|
|
||||||
@kernel
|
@kernel
|
||||||
|
@ -16,7 +16,7 @@ class SUServo(EnvExperiment):
|
|||||||
mask = 1 << 18 - 1
|
mask = 1 << 18 - 1
|
||||||
for name, val in zip("ftw1 b1 pow cfg offset a1 ftw0 b0".split(), d):
|
for name, val in zip("ftw1 b1 pow cfg offset a1 ftw0 b0".split(), d):
|
||||||
val = -(val & mask) + (val & ~mask)
|
val = -(val & mask) + (val & ~mask)
|
||||||
print("{}: {:x} = {}".format(name, val, val))
|
print("{}: {:#x} = {}".format(name, val, val))
|
||||||
|
|
||||||
@rpc(flags={"async"})
|
@rpc(flags={"async"})
|
||||||
def p1(self, adc, asf, st):
|
def p1(self, adc, asf, st):
|
||||||
|
Loading…
Reference in New Issue
Block a user