add register for adc readout

Author: hubuke

.gitignore

@@ -1,2 +1,8 @@
 /build
 /py
+/__pycache__/
+/.pytest_cache/
+*.jou
+*.log
+*.vcd
+*.gtkw

iir.py

@@ -21,9 +21,15 @@ def __init__(self):
         self.p = p = Signal((48, True), reset_less=True)
         self.sync += [m.eq(a * b), p.eq(m + c), If(mux_p, p.eq(m + p))]
 
-
 class Iir(Module):
     def __init__(self, w_coeff, w_data, log2_a0, n_profiles, n_channels):
+        """
+        w_coeff: 16 bits
+        w_data: ADC_DATA_WIDTH = 16 bits
+        log2_a0: log2 of a0 coefficient = 14 bits
+        n_profiles: number of filter profiles = 4
+        n_channels: number of channels = 2
+        """
         # input strobe signal (start processing all channels)
         self.stb_in = stb_in = Signal()
         self.stb_out = stb_out = Signal()  # output strobe signal (all channels done)
@@ -77,21 +83,22 @@ def __init__(self, w_coeff, w_data, log2_a0, n_profiles, n_channels):
         self.submodules.dsp = dsp = Dsp()
         assert w_data <= len(dsp.b)
         assert w_coeff <= len(dsp.a)
-        shift_c = len(dsp.a) + len(dsp.b) - w_data - (w_data - log2_a0)
-        shift_a = len(dsp.a) - w_coeff
-        shift_b = len(dsp.b) - w_data
+        # shift_c = len(dsp.a) + len(dsp.b) - w_data - (w_data - log2_a0)  # 25 bits without -9; 16 bits with -9
+        shift_a = len(dsp.a) - w_coeff # 25-16 = 9 bits
+        shift_b = len(dsp.b) - w_data # 18-16 = 2 bits
+        shift_c = shift_a + shift_b # 9+2 = 11 bits
         # +1 from standard sign bit
-        n_sign = len(dsp.p) - len(dsp.a) - len(dsp.b) + w_data - log2_a0 + 1
-        c_rounding_offset = Constant((1 << shift_c - 1) - 1, shift_c)
+        n_sign = len(dsp.p) - len(dsp.a) - len(dsp.b) + w_data - log2_a0 + 1 # 48-25-18+16-14+1 = 8 bits 
+        c_rounding_offset = Constant((1 << shift_c - 1) - 1, shift_c) # 25 or 16 bits of 1s depending on -9
 
         self.sync += [
             # default to 0 and set to 1 further down if computation done in this cycle
             stb_out.eq(0),
-            dsp.a.eq(coeff[step][profile_index][channel_index] << shift_a),
+            dsp.a.eq(coeff[step][profile_index][channel_index] << shift_a), # 16 bits + 9 bits shift = 25 bits
             dsp.b.eq(
-                x[channel_index][step] << shift_b
+                x[channel_index][step] << shift_b # 16+2=18 bits
             ),  # overwritten later if at step==2
-            dsp.c.eq(Cat(c_rounding_offset, offset[profile_index][channel_index])),
+            dsp.c.eq(Cat(c_rounding_offset, offset[profile_index][channel_index])), # 16 bit offset + 25 bits rounding offsets = 41 bits
             If(
                 stb_in & ~busy,
                 busy.eq(1),
@@ -132,10 +139,10 @@ def __init__(self, w_coeff, w_data, log2_a0, n_profiles, n_channels):
             ch_profile_last_ch.eq(ch_profile[channel_index - 1]),
             [o.eq(y1[ch_profile[ch]][ch]) for ch, o in enumerate(outp)],
             # clipping to positive output range
-            y0_clipped.eq(dsp.p >> shift_c),
+            y0_clipped.eq(dsp.p >> (shift_c+22)), # 48 bit - 11 bits = 37 bits
             If(
-                dsp.p[-n_sign:] != 0,  # if out of output range
-                y0_clipped.eq((1 << w_data - 1) - 1),
+                dsp.p[-n_sign:] != 0,  # dsp.p[47:40] != 0
+                y0_clipped.eq((1 << w_data - 1) - 1), # 16-1 bits -1 = 32767
             ),
             If(dsp.p[-1] != 0, y0_clipped.eq(0)),  # if negative
         ]

phaser.py

@@ -12,6 +12,7 @@
 SERVO_PROFILES = 4  # number iir coefficient profiles per servo channel
 SERVO_CHANNELS = 2  # number servochannels
 
+ADC_DATA_WIDTH = 16  # bits per adc channel
 
 class PWM(Module):
     """Pulse width modulation"""
@@ -46,6 +47,7 @@ def __init__(self, platform):
         platform.add_period_constraint(eem.data0_p, 4.0 * 8)
         self.submodules.crg = CRG(platform, link=self.link.phy.clk)
         # Don't bother meeting s/h for the clk iserdes. We align it.
+        platform.add_false_path_constraint(eem.data0_p, self.crg.cd_sys.clk)
         platform.add_false_path_constraint(eem.data0_p, self.crg.cd_sys2.clk)
         self.submodules.decoder = Decode(
             b_sample=14, n_channel=2, n_mux=8, t_frame=8 * 10
@@ -154,6 +156,27 @@ def __init__(self, platform):
                             Register(read=False),
                         )
                     )
+        
+        # place holder for miqro register
+        # 0x72 - 0x78 Miqro channel profile/window memories
+        for i in range(7):
+            phaser_registers.append(
+                (
+                    f"miqro_mem{i}",
+                    Register(),
+                )
+            )
+
+        # add adc data registers
+        # 0x79-0x7c
+        for ch in range(SERVO_CHANNELS):
+            for loc in ["lo", "hi"]:
+                phaser_registers.append(
+                    (
+                        f"adc_data{ch}_{loc}",
+                        Register(write=False),
+                )
+            )
 
         self.decoder.map_registers(phaser_registers)
 
@@ -242,7 +265,7 @@ def __init__(self, platform):
         # Note that there is one extra cycle (4 ns) at the end of a transaction.
         # Total: 264 ns -> 3.788 MSps
         adc_parameters = AdcParams(
-            width=16, channels=2, lanes=2, t_cnvh=8, t_conv=3, t_rtt=6
+            width=ADC_DATA_WIDTH, channels=2, lanes=2, t_cnvh=8, t_conv=3, t_rtt=6
         )
 
         self.submodules.adc = adc = Adc(platform.request("adc"), adc_parameters)
@@ -255,8 +278,8 @@ def __init__(self, platform):
 
         # log2_a0 = 14 bit for an effective fixedpoint a0 of 0.5
         self.submodules.iir = iir = Iir(
-            w_coeff=16,
-            w_data=16,
+            w_coeff=16, 
+            w_data=ADC_DATA_WIDTH,
             log2_a0=14,
             n_profiles=SERVO_PROFILES,
             n_channels=SERVO_CHANNELS,
@@ -266,6 +289,23 @@ def __init__(self, platform):
             iir.stb_in.eq(adc.done),
         ]
 
+        # add adc data to adc data register
+        for ch in range(SERVO_CHANNELS):
+            assert SERVO_CHANNELS <= len(adc.data)
+            sample_reg = Signal((16, True), name=f"adc_sample_reg_{ch}")
+            self.sync += If(adc.done, sample_reg.eq(adc.data[ch]))
+            # self.sync += If(adc.done, sample_reg.eq(0xFFFF))
+
+            # expose the registered value to the decoder read ports combinatorially
+            # split into low / high bytes to match the two Register() entries
+            self.comb += [
+                self.decoder.get(f"adc_data{ch}_lo", "read").eq(sample_reg[0:8]),
+                self.decoder.get(f"adc_data{ch}_hi", "read").eq(sample_reg[8:16]),
+                # self.decoder.get(f"adc_data{ch}_lo", "read").eq(0xaa),
+                # self.decoder.get(f"adc_data{ch}_hi", "read").eq(0xbb),
+            ]
+
+
         # connect iir to servo data registers
         for i in range(SERVO_CHANNELS):
             for j in range(SERVO_PROFILES):
@@ -368,39 +408,41 @@ def __init__(self, platform):
             ]
 
         # use liberally for debugging
-        self.comb += [
-            Cat([platform.request("test_point", i) for i in range(6)]).eq(
-                Cat(
-                    ClockSignal("clk125"),
-                    ClockSignal("link"),
-                    # ClockSignal(),
-                    # ResetSignal(),
-                    # # self.link.slip.bitslip,
-                    # # self.link.unframe.data[0],
-                    # # self.link.unframe.data[1],
-                    # # self.link.unframe.clk_stb,
-                    # # self.link.unframe.marker_stb,
-                    # # self.link.unframe.end_of_frame,
-                    # self.link.checker.frame_stb,
-                    # # self.decoder.bus.bus.we,
-                    # # self.decoder.bus.bus.re,
-                    # # self.decoder.bus.bus.adr[0],
-                    # self.link.checker.miso,
-                    # # self.dac.data_sync,
-                    # self.dac.istr,
-                    # dac_ctrl.alarm,
-                    ClockSignal("ret"),
-                    adc.cnvn,
-                    adc.sdo[0],
-                    adc.data[1][0],
-                )
-            )
-        ]
+        self.comb += [platform.request("test_point", 0).eq(adc.data[1][0])]
+        # self.comb += [
+        #     Cat([platform.request("test_point", i) for i in range(6)]).eq(
+        #         Cat(
+        #             ClockSignal("clk125"),
+        #             ClockSignal("link"),
+        #             # ClockSignal(),
+        #             # ResetSignal(),
+        #             # # self.link.slip.bitslip,
+        #             # # self.link.unframe.data[0],
+        #             # # self.link.unframe.data[1],
+        #             # # self.link.unframe.clk_stb,
+        #             # # self.link.unframe.marker_stb,
+        #             # # self.link.unframe.end_of_frame,
+        #             # self.link.checker.frame_stb,
+        #             # # self.decoder.bus.bus.we,
+        #             # # self.decoder.bus.bus.re,
+        #             # # self.decoder.bus.bus.adr[0],
+        #             # self.link.checker.miso,
+        #             # # self.dac.data_sync,
+        #             # self.dac.istr,
+        #             # dac_ctrl.alarm,
+        #             ClockSignal("ret"),
+        #             adc.cnvn,
+        #             adc.sdo[0],
+        #             adc.data[1][0],
+        #         )
+        #     )
+        # ]
 
 
 if __name__ == "__main__":
     from migen.build.platforms.sinara.phaser import Platform
 
+    # platform = Platform(speed_grade="-3")
     platform = Platform()
     # platform.toolchain.additional_commands.extend([
     #     "set argv phaser.bit",

test_iir.py

@@ -87,24 +87,65 @@ def profile_switch(dut, inp, coeff, offset, outp):
     o = yield dut.outp[0]
     outp.append(o)
 
+def servo_operation(dut, inp_list, coeff, offset, outp):
+    yield
+    yield dut.inp[0].eq(inp_list[0])
+    yield dut.coeff[0][0][0].eq(coeff) # b0
+    # yield dut.coeff[1][0][0].eq(coeff) # b1
+    # yield dut.coeff[2][0][0].eq(0)
+    yield dut.offset[0][0].eq(offset)
+    yield dut.stb_in.eq(1)
+    yield
+    yield dut.stb_in.eq(0)
+    for i in range(100):
+        yield dut.inp[0].eq(inp_list[i % len(inp_list)])
+        for _ in range(delay_cycles):
+            yield
+        yield dut.stb_in.eq(1)
+        yield
+        yield dut.stb_in.eq(0)
+        for _ in range(delay_cycles):
+            yield
+        o = yield dut.outp[0]
+        outp.append(o)
+        yield
+
 
 class TestIir(unittest.TestCase):
     def setUp(self):
-        self.dut = Iir(w_coeff=24, w_data=16, log2_a0=15, n_profiles=2, n_channels=2)
+        self.dut = Iir(w_coeff=16, w_data=16, log2_a0=14, n_profiles=4, n_channels=2)
 
-    def test_profile_switch(self):
-        inp = 1000
-        coeff = [0x200000, 0x300000]
-        offset = [10, 30]
-        outp = []
-        run_simulation(self.dut, profile_switch(self.dut, inp, coeff, offset, outp))
-        self.assertEqual(inp // 2 + offset[0], outp[0])
-        self.assertEqual(inp * 3 // 4 + offset[1], outp[1])
+    # def test_profile_switch(self):
+    #     inp = 1000
+    #     coeff = [0x200000, 0x300000]
+    #     offset = [10, 30]
+    #     outp = []
+    #     run_simulation(self.dut, profile_switch(self.dut, inp, coeff, offset, outp))
+    #     self.assertEqual(inp // 2 + offset[0], outp[0])
+    #     self.assertEqual(inp * 3 // 4 + offset[1], outp[1])
 
-    def test_rounding(self):
-        inp = 2345
-        coeff = 0x200000  # 0.25 for both b0, b1
+    # def test_rounding(self):
+    #     inp = 2345
+    #     coeff = 0x200000  # 0.25 for both b0, b1
+    #     outp = []
+    #     run_simulation(self.dut, rounding(self.dut, inp, coeff, outp))
+    #     self.assertEqual(inp // 2, outp[0])
+    #     self.assertEqual((inp // 2) + 1, outp[1])
+
+    def test_servo_operation(self):
+        global delay_cycles
+        delay_cycles = 25
+        # inp_list = [512, 1024, 2048, 4096, 8192, 16384, 32768]
+        # inp_list = []
+        # start = 0
+        # stop = 10
+        # step = 10
+        # for i in range(step):
+        #     inp_list.append(start + i * (stop - start) // step)
+        inp_list = [32763]
+        coeff = 32765  # 0.5
+        offset = 0
         outp = []
-        run_simulation(self.dut, rounding(self.dut, inp, coeff, outp))
-        self.assertEqual(inp // 2, outp[0])
-        self.assertEqual((inp // 2) + 1, outp[1])
+        run_simulation(self.dut, servo_operation(self.dut, inp_list, coeff, offset, outp), vcd_name="iir_servo.vcd")
+
+        print(outp)