diff --git a/apsct_lib.py b/apsct_lib.py
index 905bd98d3d0f75ac0d57d79da8f1fa5e2c288e6d..5ff0196084be5c168de040942a41528d82440f4d 100644
--- a/apsct_lib.py
+++ b/apsct_lib.py
@@ -155,8 +155,9 @@ class PllClass:
         if DEBUG:
             stri = "IO expander wrote 0x{0:x}, read 0x{1}".format(0x2C, rd_bytes[1])
             print(stri)
-        data = (reg_address << 9) + (pll_rw << 8) + wr_data
-        bit_array = "{0:{fill}16b}".format(data, fill='0')
+        nof_bytes = 1
+        data = (pll_rw << 23) + (nof_bytes << 21) + (reg_address << 8)  + wr_data
+        bit_array = "{0:{fill}24b}".format(data, fill='0')
         self.dev_i2c_pll.write_bytes(0x02, 0x02 | (0x1 << APSCT_I2C.CS))
         for bit in bit_array:
             for clk in range(2):
@@ -178,8 +179,8 @@ class PllClass:
         #
         pll_rw = 0x01  # 0 for write, 1 for read
         self.dev_i2c_pll.write_bytes(0x06, 0x2C)
-        data = (reg_address << 7) + pll_rw
-        bit_array = "{0:{fill}8b}".format(data, fill='0')
+        data = (pll_rw << 15) + (nof_bytes << 13) + reg_address
+        bit_array = "{0:{fill}16b}".format(data, fill='0')
         for bit in bit_array:
             for clk in range(2):
                 write_data = 0x02 | (0 << APSCT_I2C.CS) | (clk << APSCT_I2C.SCLK) | (int(bit) << APSCT_I2C.SDI)
@@ -208,41 +209,45 @@ class PllClass:
         # Set registers on the PLL
         #
         print(f"Setup PPL {self.frequency}")
-        self.dev_i2c_pll.write_bytes(0x07, 0x00)
+        divider_r = 1
+        divider_a = 0
+        divider_p = 2
+        divider_b = (self.frequency * divider_r) / (10 * divider_p)
+        charge_pump_current = 3  # 0 is low (0.6 mA), 7 is high (4.8 mA)
         if self.frequency == '160MHz':
             i2c_address = APSCT_I2C.PLL_200M
             dev_i2c_pll_sel = I2C(i2c_address, BUSNR=I2CBUSNR)
             dev_i2c_pll_sel.write_bytes(0x03, 0x08)
         else:
             self.dev_i2c_pll.write_bytes(0x03, 0x28)
-        self.write_byte_pll(0x03, 0x0C)
-        sleep(0.5)
-        self.write_byte_pll(0x03, 0x08)
-        self.write_byte_pll(0x03, 0x08)
-        self.write_byte_pll(0x04, 0xCF)  # CF disable not used outputs, 00 enable all
-        self.write_byte_pll(0x05, 0x97)
-        self.write_byte_pll(0x06, 0x10)  # cp inv = 0xF4 other 0xE4
-        self.write_byte_pll(0x07, 0x04)  # Divider R = 1 dec
-        self.write_byte_pll(0x08, 0x01)
-        self.write_byte_pll(0x07, 0x00)
-        self.write_byte_pll(0x09, 0x10)  # reset
-        if self.frequency == '160MHz':
-            self.write_byte_pll(0x0A, 0x10)
-        else:
-            self.write_byte_pll(0x0A, 0x14)
-        self.write_byte_pll(0x09, 0x00)
-        self.write_byte_pll(0x0C, 0x8F)
-        self.write_byte_pll(0x0D, 0x88)  # Dig CLK     = 200/1 = 200 MHz
-        self.write_byte_pll(0x0F, 0x08)  # RCU CLK     = 200/1 = 200 MHz
-        self.write_byte_pll(0x11, 0x08)  # PPS ref CLK = 200/1 = 200 MHz
-        self.write_byte_pll(0x13, 0x88)  # T.P.    CLK = 200/1 = 200 MHz
+        self.write_byte_pll(0x04, (divider_a & 0x3F))
+        self.write_byte_pll(0x05, (divider_b & 0x1F00) >> 8)
+        self.write_byte_pll(0x06, (divider_b & 0x00FF))
+        self.write_byte_pll(0x07, 0x04)  # Lock detect
+        self.write_byte_pll(0x08, 0x03)  # Charge pump normal + Status bit
+        self.write_byte_pll(0x09, (charge_pump_current & 0x7) << 4)
+        self.write_byte_pll(0x0A, 0x04)  # Fixed Divide 2
+        self.write_byte_pll(0x0B, 0x00)
+        self.write_byte_pll(0x45, 0x00)  # CLK2 as feedback clock input
+        self.write_byte_pll(0x3D, 0x08)  # OUT0 ON LVDS Standard
+        self.write_byte_pll(0x3E, 0x0A)  # OUT1 OFF
+        self.write_byte_pll(0x3F, 0x0A)  # OUT2 OFF
+        self.write_byte_pll(0x40, 0x03)  # OUT3 OFF
+        self.write_byte_pll(0x41, 0x02)  # OUT4 ON LVDS Standard
+        self.write_byte_pll(0x4B, 0x80)  # OUT0 bypass divider
+        self.write_byte_pll(0x4D, 0x80)  # OUT1 bypass divider
+        self.write_byte_pll(0x4F, 0x80)  # OUT2 bypass divider
+        self.write_byte_pll(0x51, 0x80)  # OUT3 bypass divider
+        self.write_byte_pll(0x53, 0x80)  # OUT4 bypass divider
+        self.write_byte_pll(0x5A, 0x01)  # Update registers
+
 
     def read_all_regs_pll(self):
         #
         # Read all registers on the PLL and print on screen
         #
         self.dev_i2c_pll.write_bytes(0x07, 0x00)
-        bytes_to_read = 24
+        bytes_to_read = 12
         ret_value = self.read_byte_pll(0, nof_bytes=bytes_to_read)
         for cnt in range(bytes_to_read):
             start = cnt*8