diff --git a/eimu.py b/eimu.py
index e3682a1..f704c52 100644
--- a/eimu.py
+++ b/eimu.py
@@ -1,29 +1,49 @@
 import serial
-import struct
-
-class EIMUSerialError(Exception):
-    """Custom exception for for EIMU Comm failure"""
-    pass
-
-START_BYTE = 0xBB
-READ_QUAT = 0x01
-READ_RPY = 0x02
-READ_RPY_VAR = 0x03
-READ_ACC = 0x05
-READ_ACC_VAR = 0x09
-READ_GYRO = 0x0B
-READ_GYRO_VAR = 0x0F
-READ_MAG = 0x11
-SET_FILTER_GAIN = 0x1D
-GET_FILTER_GAIN = 0x1E
-SET_FRAME_ID = 0x1F
-GET_FRAME_ID = 0x20
-READ_QUAT_RPY = 0x22
-READ_ACC_GYRO = 0x23
-CLEAR_DATA_BUFFER = 0x27
-READ_IMU_DATA = 0x28
-READ_LIN_ACC_RAW = 0x2B
-READ_LIN_ACC = 0x2C
+import time
+
+# Serial Protocol Command IDs -------------
+READ_RPY = 10
+READ_RPY_VAR = 11
+WRITE_RPY_VAR = 12
+
+READ_GYRO = 13
+READ_GYRO_RAW = 14
+READ_GYRO_OFF = 15
+WRITE_GYRO_OFF = 16
+READ_GYRO_VAR = 17
+WRITE_GYRO_VAR = 18
+
+READ_ACC = 19
+READ_ACC_RAW = 20
+READ_ACC_OFF = 21
+WRITE_ACC_OFF = 22
+READ_ACC_VAR = 23
+WRITE_ACC_VAR = 24
+READ_LIN_ACC_RAW = 25
+READ_LIN_ACC = 26
+SET_ACC_LPF_CUT_FREQ = 27
+GET_ACC_LPF_CUT_FREQ = 28
+
+READ_MAG = 29
+READ_MAG_RAW = 30
+READ_MAG_H_OFF = 31
+WRITE_MAG_H_OFF = 32
+READ_MAG_S_OFF0 = 33
+WRITE_MAG_S_OFF0 = 34
+READ_MAG_S_OFF1 = 35
+WRITE_MAG_S_OFF1 = 36
+READ_MAG_S_OFF2 = 37
+WRITE_MAG_S_OFF2 = 38
+
+SET_I2C_ADDR = 39
+GET_I2C_ADDR = 40
+SET_FILTER_GAIN = 41
+GET_FILTER_GAIN = 42
+SET_FRAME_ID = 43
+GET_FRAME_ID = 44
+RESET = 45
+CLEAR = 46
+#---------------------------------------------
 
 
 
@@ -31,221 +51,80 @@ class EIMU:
     def __init__(self):
         pass
 
-    def connect(self, port, baud=56700, timeOut=0.1):
+    def connect(self, port, baud=115200, timeOut=0.1):
         self.ser = serial.Serial(port, baud, timeout=timeOut)
-
+        time.sleep(0.1)
+        self.ser.reset_input_buffer()
+        self.ser.reset_output_buffer()
+    
     def disconnect(self):
         if self.ser.is_open:
             self.ser.close()
-    
-    #------------------------------------------------------------------------
-    def send_packet_without_payload(self, cmd):
-        length = 0
-        packet = bytearray([START_BYTE, cmd, length])
-        checksum = sum(packet) & 0xFF
-        packet.append(checksum)
-        self.ser.write(packet)
-        self.ser.flush()
-
-    def send_packet_with_payload(self, cmd, payload_bytes):
-        length = len(payload_bytes)
-        packet = bytearray([START_BYTE, cmd, length]) + payload_bytes
-        checksum = sum(packet) & 0xFF
-        packet.append(checksum)
-        self.ser.write(packet)
-        self.ser.flush()
 
-    def read_packet1(self):
-        """
-        Reads 4 bytes from the serial port and converts to a float (little-endian).
-        Returns (success, value-array)
-        """
-        try:
-            payload = self.ser.read(4)
-            if len(payload) != 4:
-                # print("[EPMC SERIAL ERROR]: Timeout while reading 1 values")
-                return False, 0.0
+    #------------------------------------------------------------------------
+    def send(self, cmd, arg1=0.0, arg2=0.0, arg3=0.0):
+        send_str = str(round(float(cmd),6))+" "+str(round(float(arg1),6))+" "+str(round(float(arg2),6))+" "+str(round(float(arg3),6))+"\r"
+        self.ser.write(send_str.encode())
 
-            # Unpack 4 bytes as little-endian float
-            (val,) = struct.unpack('<f', payload)
-            return True, val
-        except:
-            # print("[PYSERIAL ERROR]: Read Timeout")
-            return False, 0.0
-        
-    def read_packet3(self):
-        """
-        Reads 12 bytes from the serial port and converts to a float (little-endian).
-        Returns (success, value-array)
-        """
+    def recv(self, cmd, arg1=0):
         try:
-            payload = self.ser.read(12)
-            if len(payload) != 12:
-                # print("[EPMC SERIAL ERROR]: Timeout while reading 3 values")
-                return False, 0.0, 0.0, 0.0
-
-            # Unpack 4 bytes as little-endian float
-            a, b, c = struct.unpack('<fff', payload)
-            return True, a, b, c
+            self.send(cmd, arg1)
+            data = self.ser.readline().decode().strip().split(' ')
+            return True, round(float(data[0]),6), round(float(data[1]),6), round(float(data[2]),6)
         except:
-            # print("[PYSERIAL ERROR]: Read Timeout")
+            # self.ser.reset_input_buffer()
+            # self.ser.reset_output_buffer()
             return False, 0.0, 0.0, 0.0
     
-    def read_packet4(self):
-        """
-        Reads 16 bytes from the serial port and converts to a float (little-endian).
-        Returns (success, value-array)
-        """
-        try:
-            payload = self.ser.read(16)
-            if len(payload) != 16:
-                # print("[EPMC SERIAL ERROR]: Timeout while reading 4 values")
-                return False, 0.0, 0.0, 0.0, 0.0
-
-            # Unpack 4 bytes as little-endian float
-            a, b, c, d = struct.unpack('<ffff', payload)
-            return True, a, b, c, d
-        except:
-            # print("[PYSERIAL ERROR]: Read Timeout")
-            return False, 0.0, 0.0, 0.0, 0.0
-        
-    def read_packet6(self):
-        """
-        Reads 24 bytes from the serial port and converts to a float (little-endian).
-        Returns (success, value-array)
-        """
-        try:
-            payload = self.ser.read(24)
-            if len(payload) != 24:
-                # print("[EPMC SERIAL ERROR]: Timeout while reading 6 values")
-                return False, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
-
-            # Unpack 4 bytes as little-endian float
-            a, b, c, d, e, f = struct.unpack('<ffffff', payload)
-            return True, a, b, c, d, e, f
-        except:
-            # print("[PYSERIAL ERROR]: Read Timeout")
-            return False, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
-        
-    def read_packet9(self):
-        """
-        Reads 36 bytes from the serial port and converts to a float (little-endian).
-        Returns (success, value-array)
-        """
-        try:
-            payload = self.ser.read(36)
-            if len(payload) != 36:
-                # print("[EPMC SERIAL ERROR]: Timeout while reading 9 values")
-                return False, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
-
-            # Unpack 4 bytes as little-endian float
-            a, b, c, d, e, f, g, h, i = struct.unpack('<fffffffff', payload)
-            return True, a, b, c, d, e, f, g, h, i
-        except:
-            # print("[PYSERIAL ERROR]: Read Timeout")
-            return False, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
-    
-    #---------------------------------------------------------------------
-
-    def write_data1(self, cmd, val, pos=100):
-        payload = struct.pack('<Bf', pos, val)
-        self.send_packet_with_payload(cmd, payload)
-
-    def read_data1(self, cmd, pos=100):
-        payload = struct.pack('<Bf', pos, 0.0)  # big-endian
-        self.send_packet_with_payload(cmd, payload)
-        success, val = self.read_packet1()
-        return success, val
-    
-    def write_data3(self, cmd, a, b, c):
-        payload = struct.pack('<fff', a, b, c) 
-        self.send_packet_with_payload(cmd, payload)
-
-    def read_data3(self, cmd):
-        self.send_packet_without_payload(cmd)
-        success, a, b, c = self.read_packet3()
-        return success, a, b, c
-
-    def read_data4(self, cmd):
-        self.send_packet_without_payload(cmd)
-        suceess, a, b, c, d = self.read_packet4()
-        return suceess, a, b, c, d
-    
-    def read_data6(self, cmd):
-        self.send_packet_without_payload(cmd)
-        success, a, b, c, d, e, f = self.read_packet6()
-        return success, a, b, c, d, e, f
-    
-    def read_data9(self, cmd):
-        self.send_packet_without_payload(cmd)
-        success, a, b, c, d, e, f, g, h, i = self.read_packet9()
-        return success, a, b, c, d, e, f, g, h, i
-    
     #---------------------------------------------------------------------
         
     def clearDataBuffer(self):
-        success, res = self.read_data1(CLEAR_DATA_BUFFER)
+        success, _, _, _ = self.recv(CLEAR)
         return success
     
     def setWorldFrameId(self, frame_id):
-        self.write_data1(SET_FRAME_ID, frame_id)
+        self.send(SET_FRAME_ID, 0.0, frame_id)
     
     def getWorldFrameId(self):
-        success, frame_id = self.read_data1(GET_FRAME_ID)
+        success, frame_id, _, _ = self.recv(GET_FRAME_ID)
         return success, int(frame_id)
     
     def setFilterGain(self, gain):
-        self.write_data1(SET_FILTER_GAIN, gain)
+        self.send(SET_FILTER_GAIN, 0.0, gain)
 
     def getFilterGain(self):
-        success, gain = self.read_data1(GET_FILTER_GAIN)
+        success, gain, _, _ = self.recv(GET_FILTER_GAIN)
         return success, round(gain, 3)
     
-    def readQuat(self):
-        success, qw, qx, qy, qz = self.read_data4(READ_QUAT)
-        return success, round(qw, 6), round(qx, 6), round(qy, 6), round(qz, 6)
+    def setAccFilterCF(self, cf):
+        self.send(SET_ACC_LPF_CUT_FREQ, 0.0, cf)
+    
+    def getAccFilterCF(self):
+        success, cf, _, _ = self.recv(GET_ACC_LPF_CUT_FREQ)
+        return success, round(cf, 3)
     
     def readRPY(self):
-        success, r, p, y = self.read_data3(READ_RPY)
-        return success, round(r, 6), round(p, 6), round(y, 6)
+        success, r, p, y = self.recv(READ_RPY)
+        return success, r, p, y
     
     def readRPYVariance(self):
-        success, r, p, y = self.read_data3(READ_RPY_VAR)
-        return success, round(r, 6), round(p, 6), round(y, 6)
+        success, r, p, y = self.recv(READ_RPY_VAR)
+        return success, r, p, y
     
-    # def readAcc(self):
-    #     success, ax, ay, az = self.read_data3(READ_ACC)
-    #     return success, round(ax, 6), round(ay, 6), round(az, 6)
+    def readLinearAcc(self):
+        success, ax, ay, az = self.recv(READ_LIN_ACC)
+        return success, ax, ay, az
     
     def readAccVariance(self):
-        success, ax, ay, az = self.read_data3(READ_ACC_VAR)
-        return success, round(ax, 6), round(ay, 6), round(az, 6)
+        success, ax, ay, az = self.recv(READ_ACC_VAR)
+        return success, ax, ay, az
     
     def readGyro(self):
-        success, gx, gy, gz = self.read_data3(READ_GYRO)
-        return success, round(gx, 6), round(gy, 6), round(gz, 6)
+        success, gx, gy, gz = self.recv(READ_GYRO)
+        return success, gx, gy, gz
     
     def readGyroVariance(self):
-        success, gx, gy, gz = self.read_data3(READ_GYRO_VAR)
-        return success, round(gx, 6), round(gy, 6), round(gz, 6)
+        success, gx, gy, gz = self.recv(READ_GYRO_VAR)
+        return success, gx, gy, gz
     
-    # def readMag(self):
-    #     success, mx, my, mz = self.read_data3(READ_MAG)
-    #     return success, round(mx, 6), round(my, 6), round(mz, 6)
-    
-    def readAccGyro(self):
-        success, ax, ay, az, gx, gy, gz = self.read_data6(READ_ACC_GYRO)
-        return success, round(ax, 6), round(ay, 6), round(az, 6), round(gx, 6), round(gy, 6), round(gz, 6)
-    
-    def readImuData(self):
-        success, r, p, y, ax, ay, az, gx, gy, gz = self.read_data9(READ_IMU_DATA)
-        return success, round(r, 6), round(p, 6), round(y, 6), round(ax, 6), round(ay, 6), round(az, 6), round(gx, 6), round(gy, 6), round(gz, 6)
-
-    def readLinearAccRaw(self):
-        success, ax, ay, az = self.read_data3(READ_LIN_ACC_RAW)
-        return success, round(ax, 6), round(ay, 6), round(az, 6)
-    
-    def readLinearAcc(self):
-        success, ax, ay, az = self.read_data3(READ_LIN_ACC)
-        return success, round(ax, 6), round(ay, 6), round(az, 6)
     #---------------------------------------------------------------------
\ No newline at end of file
diff --git a/read_imu.py b/read_imu.py
index 95f8c88..086aa63 100644
--- a/read_imu.py
+++ b/read_imu.py
@@ -5,13 +5,17 @@
 eimu = EIMU()
 
 def main():
+  r=0.0; p=0.0; y=0.0
+  ax=0.0; ay=0.0; az=0.0
+  gx=0.0; gy=0.0; gz=0.0
+
   eimu.connect(port)
 
   for i in range(4):
     time.sleep(1.0)
     print(i+1, " sec")
 
-  success = eimu.clearDataBuffer()
+  # success = eimu.clearDataBuffer()
 
   # change the reference frame to ENU frame (0 - NWU,  1 - ENU,  2 - NED)
   eimu.setWorldFrameId(1)
@@ -33,15 +37,22 @@ def main():
 
   while True:
     if time.time() - prevTime > sampleTime:
-      success, r, p, y, ax, ay, az, gx, gy, gz = eimu.readImuData()
+      success, valx, valy, valz = eimu.readRPY()
+      if success:
+        r=valx; p=valy; y=valz
 
+      success, valx, valy, valz = eimu.readLinearAcc()
       if success:
-        print(f"r: {r}\tp: {p}\ty: {y}")
-        print(f"ax: {ax}\tay: {ay}\taz: {az}")
-        print(f"gx: {gx}\tgy: {gy}\tgz: {gz}")
-        print()
-      else:
-        print("Error reading IMU data")
+        ax=valx; ay=valy; az=valz
+
+      success, valx, valy, valz = eimu.readGyro()
+      if success:
+        gx=valx; gy=valy; gz=valz
+
+      print(f"r: {r}\tp: {p}\ty: {y}")
+      print(f"ax: {ax}\tay: {ay}\taz: {az}")
+      print(f"gx: {gx}\tgy: {gy}\tgz: {gz}")
+      print()
       prevTime = time.time()
 
 if __name__ == "__main__":