Im quite new in the field of 3d mapping and vectors. I have arduino mega with a slighty modified example sketch to comunicates via I2C with a SparkFun SEN-15335 (ICM-20948) .I have been trying to map the serial data coming from IMU into a 3d plot using python. I’m also taking advantage of the DMP inside of the IMU to make sensor fusion and the data im retreiving is the INV_ICM20948_SENSOR_LINEAR_ACCELERATION(16-bit accel + 32-bit 6-axis quaternion) + SENSOR_ORIENTATION (32-bit 9-axis quaternion + heading accuracy). i have tested alot and i know the cube rotation itself is working flawless the problem is the cube translations or cube translation combined with rotation.
// Enable the DMP Game Rotation Vector sensor (Quat6)
//success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ACCELEROMETER) == ICM_20948_Stat_Ok);
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_LINEAR_ACCELERATION) == ICM_20948_Stat_Ok);
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
// Configuring DMP to output data at multiple ODRs:
// DMP is capable of outputting multiple sensor data at different rates to FIFO.
// Setting value can be calculated as follows:
// Value = (DMP running rate / ODR ) - 1
// E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 4) == ICM_20948_Stat_Ok); // Set to 11Hz
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 4) == ICM_20948_Stat_Ok); // Set to 11Hz
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 4) == ICM_20948_Stat_Ok); // Set to 11Hz
// success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 4) == ICM_20948_Stat_Ok); // Set to 11Hz
ROTATION DATA:
double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
double q2sqr = q2 * q2;
// roll (x-axis rotation)
double t0 = +2.0 * (q0 * q1 + q2 * q3);
double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr);
double roll = atan2(t0, t1) * 180.0 / PI;
// pitch (y-axis rotation)
double t2 = +2.0 * (q0 * q2 - q3 * q1);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
double pitch = asin(t2) * 180.0 / PI;
// yaw (z-axis rotation)
double t3 = +2.0 * (q0 * q3 + q1 * q2);
double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
double yaw = atan2(t3, t4) * 180.0 / PI;
SERIAL_PORT.print(F("Roll:"));
SERIAL_PORT.print(roll, 1);
SERIAL_PORT.print(F(" Pitch:"));
SERIAL_PORT.print(pitch, 1);
SERIAL_PORT.print(F(" Yaw:"));
SERIAL_PORT.println(yaw, 1);
Linear accelaration quaternion data:
double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
SERIAL_PORT.print(F("Q0:"));
SERIAL_PORT.print(q0, 3);
SERIAL_PORT.print(F(" Q1:"));
SERIAL_PORT.print(q1, 3);
SERIAL_PORT.print(F(" Q2:"));
SERIAL_PORT.print(q2, 3);
SERIAL_PORT.print(F(" Q3:"));
SERIAL_PORT.println(q3, 3);
float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data
float acc_y = (float)data.Raw_Accel.Data.Y;
float acc_z = (float)data.Raw_Accel.Data.Z;
SERIAL_PORT.print(F("Accel: X:"));
SERIAL_PORT.print(acc_x);
SERIAL_PORT.print(F(" Y:"));
SERIAL_PORT.print(acc_y);
SERIAL_PORT.print(F(" Z:"));
SERIAL_PORT.println(acc_z);
I calibrate the sensor by moving in a circular motion but when I make it stand still I can visualize alot of random moviment. If I dont make the calibration motions i can see it just drifting.
Im using a thread to read serial data and parse inserting into a Queue while the matplotlib.animation FuncAnimation calling update.
def read_serial():
# Replace with your serial port and baud rate
serial_port = serial.Serial('COM7', 115200) # Adjust for your serial port and baud rate
while True:
if serial_port.in_waiting > 0:
line = serial_port.readline().decode().strip()
if line:
if line.startswith('Q0:'):
parts = line.split()
qw = float(parts[0].split(':')[1])
qx = float(parts[1].split(':')[1])
qy = float(parts[2].split(':')[1])
qz = float(parts[3].split(':')[1])
global Quaternion
Quaternion.put([qx, qy, qz, qw])
elif line.startswith('Accel:'):
parts = line.split()
X = float(parts[1].split(':')[1])
Y = float(parts[2].split(':')[1])
Z = float(parts[3].split(':')[1])
global Acceleration
Acceleration.put([X, Y, Z])
elif line.startswith('Roll:'):
parts = line.split()
roll = float(parts[0].split(':')[1])
pitch = float(parts[1].split(':')[1])
yaw = float(parts[2].split(':')[1])
global Rotation
Rotation.put([roll, pitch, yaw])
else:
if "Press" in line:
serial_port.write('1'.encode())
else:
print(line)
print("Error parsing serial console")
def update(frame):
global time_step
global last_rotation, last_velocity, last_translation, box_vertices, Acceleration, Quaternion
update = True
if not Acceleration.empty() and not Quaternion.empty() and not Rotation.empty():
acceleration = np.array(Acceleration.get(block=True)) # acceleration vector [x,y,z]
rotation = np.array(Rotation.get(block=True)) # sensor rotation vector[x,y,z] [-180,180[ degrees
quat_rotation = np.array(Quaternion.get(block=True)) # quaternion vector [qx,qy,qz,qw]
# Compute rotation matrices
# rot_current_rotation = R.from_quat(quat_rotation).as_matrix()
rot_current_rotation = R.from_euler('xyz', rotation, degrees=True).as_matrix()
# Compute rotation difference matrix
rotation_diff_matrix = np.dot(rot_current_rotation, np.linalg.inv(last_rotation))
if update:
update = False
# gravity = np.array([0, 0, np.sum(np.abs(acceleration))])
gravity = np.array([0, 0, 9810]) # remove grav???
last_velocity = (np.dot(rot_current_rotation, acceleration)- gravity) * time_step
last_translation = last_velocity * time_step
# Apply rotation to vertices
rotated_vertices = np.dot(box_vertices, rotation_diff_matrix.T)
# Apply translation to vertices
transformed_vertices = rotated_vertices + last_translation
# transformed_vertices = rotated_vertices
# Update plot with transformed vertices
#if (frame % 100 == 0):
#ax.clear()
plot_box(ax, transformed_vertices, edge_thicness=1, edge_color="red")
update_axes_limits(ax, transformed_vertices)
# Update last_rotation and box_vertices for next frame
last_rotation = rot_current_rotation
box_vertices = transformed_vertices