Skip to content

Update Communication Code #5

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
samuko-things wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Update Communication Code #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6f16087
update serial communication code
samuko-things Dec 31, 2025
9e5db74
update communication code
samuko-things Dec 31, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
289 changes: 84 additions & 205 deletions eimu.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,251 +1,130 @@
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
#---------------------------------------------



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)
#---------------------------------------------------------------------
27 changes: 19 additions & 8 deletions read_imu.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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__":
Expand Down