|
| 1 | +from maix import display, app, image |
| 2 | +disp = display.Display() |
| 3 | +print("display init done") |
| 4 | +print(f"display size: {disp.width()}x{disp.height()}") |
| 5 | + |
| 6 | +###################################### |
| 7 | +import cv2 |
| 8 | +import numpy as np |
| 9 | +# 画布大小 |
| 10 | +w, h = disp.width(), disp.height() |
| 11 | +# 坐标中心 |
| 12 | +center = np.array([w//2, h//2]) |
| 13 | + |
| 14 | +# 构造旋转矩阵: 把原始Z轴(0,0,1)转到acc_norm方向 |
| 15 | +def rotation_matrix_from_vectors(a, b): |
| 16 | + a = a / np.linalg.norm(a) |
| 17 | + b = b / np.linalg.norm(b) |
| 18 | + v = np.cross(a, b) |
| 19 | + c = np.dot(a, b) |
| 20 | + s = np.linalg.norm(v) |
| 21 | + if s < 1e-8: |
| 22 | + return np.eye(3) |
| 23 | + kmat = np.array([[ 0, -v[2], v[1]], |
| 24 | + [ v[2], 0, -v[0]], |
| 25 | + [-v[1], v[0], 0]]) |
| 26 | + return np.eye(3) + kmat + kmat @ kmat * ((1-c)/(s**2)) |
| 27 | + |
| 28 | +# 原始坐标轴单位向量 |
| 29 | +axes = { |
| 30 | + 'X': np.array([1,0,0]), |
| 31 | + 'Y': np.array([0,1,0]), |
| 32 | + 'Z': np.array([0,0,1]) |
| 33 | +} |
| 34 | +colors = {'X': (0,0,255), 'Y': (0,255,0), 'Z': (255,0,0)} |
| 35 | + |
| 36 | +# 投影到画布（简单正交投影，忽略Z） |
| 37 | +def project(vec): |
| 38 | + scale = 150 |
| 39 | + x = int(center[0] + vec[0]*scale) |
| 40 | + y = int(center[1] - vec[1]*scale) |
| 41 | + return (x, y) |
| 42 | +###################################### |
| 43 | + |
| 44 | +import time |
| 45 | +import lsm6dsow |
| 46 | +imu = lsm6dsow.Lsm6dsow() |
| 47 | +imu.set_acc_odr(imu.ODR_TABLE[7]) |
| 48 | +imu.set_gyro_odr(imu.ODR_TABLE[7]) |
| 49 | +imu.set_acc_scale(imu.ACC_SENS_TABLE[0]) |
| 50 | +imu.set_gyro_scale(imu.GYRO_SENS_TABLE[0]) |
| 51 | + |
| 52 | +count = 0 |
| 53 | +duration = 180.0 # 统计180秒 |
| 54 | +start = time.time() |
| 55 | +while not app.need_exit(): |
| 56 | + # 新建画布 |
| 57 | + canvas = np.ones((h, w, 3), dtype=np.uint8) * 255 |
| 58 | + |
| 59 | + remain_time = start + duration - time.time() |
| 60 | + if remain_time < 0: |
| 61 | + break |
| 62 | + else: |
| 63 | + cv2.putText(canvas, f"Auto exit after {remain_time:.1f}s", (20,30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0), 2) |
| 64 | + |
| 65 | + # 同时采样加速度和陀螺仪 |
| 66 | + upd, acc_gyro_vals = imu.read() |
| 67 | + if upd: |
| 68 | + # print("ACC(m/s²):", " ".join("{:8.4f}".format(x) for x in acc_vals), |
| 69 | + # "GYRO(°/s):", " ".join("{:8.4f}".format(x) for x in gyro_vals)) |
| 70 | + count += 1 |
| 71 | + acc_vals = acc_gyro_vals[0] |
| 72 | + gyro_vals = acc_gyro_vals[1] |
| 73 | + |
| 74 | + # 1. 示例加速度（m/s²）和角速度（°/s），请替换为你的实际数据 |
| 75 | + acc = np.array(acc_vals) # X, Y, Z |
| 76 | + gyro = np.array(gyro_vals) # X, Y, Z |
| 77 | + |
| 78 | + # 2. 归一化加速度作为“新Z轴方向” |
| 79 | + acc_norm = acc / np.linalg.norm(acc) if np.linalg.norm(acc) > 1e-5 else np.array([0,0,1]) |
| 80 | + |
| 81 | + # 3. 计算旋转后的坐标轴 |
| 82 | + R = rotation_matrix_from_vectors(np.array([0,0,1]), acc_norm) |
| 83 | + rot_axes = {k: R @ v for k,v in axes.items()} |
| 84 | + |
| 85 | + # 4. 画三轴 |
| 86 | + for k in ['X', 'Y', 'Z']: |
| 87 | + tip = project(rot_axes[k]) |
| 88 | + cv2.arrowedLine(canvas, tuple(center), tip, colors[k], 3, tipLength=0.10) |
| 89 | + cv2.putText(canvas, k, tip, cv2.FONT_HERSHEY_SIMPLEX, 0.7, colors[k], 2) |
| 90 | + |
| 91 | + # 5. 角速度箭头可视化（方向和长度） |
| 92 | + gyro_body = gyro / (np.linalg.norm(gyro)+1e-8) if np.linalg.norm(gyro)>1e-8 else np.array([0,0,1]) |
| 93 | + gyro_len = min(float(np.linalg.norm(gyro)), 1.0) * 100 # 可调整箭头长度比例 |
| 94 | + # 6. 旋转到加速度系 |
| 95 | + gyro_proj = R @ gyro_body |
| 96 | + gyro_tip = project(gyro_proj * (gyro_len/150)) # 按长度比例缩放 |
| 97 | + |
| 98 | + cv2.arrowedLine(canvas, tuple(center), gyro_tip, (0,0,0), 4, tipLength=0.15) |
| 99 | + cv2.putText(canvas, "gyro", gyro_tip, cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,0), 2) |
| 100 | + |
| 101 | + # 7. 显示数值 |
| 102 | + cv2.putText(canvas, f"acc: [{acc[0]:.2f}, {acc[1]:.2f}, {acc[2]:.2f}]", (20,60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,0), 2) |
| 103 | + cv2.putText(canvas, f"gyro: [{gyro[0]:.2f}, {gyro[1]:.2f}, {gyro[2]:.2f}]", (20,90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,0), 2) |
| 104 | + |
| 105 | + fps = count / (time.time() - start) |
| 106 | + cv2.putText(canvas, f"fps: {fps:.2f}", (20,120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2) |
| 107 | + |
| 108 | + img = image.cv2image(canvas) |
| 109 | + disp.show(img) |
| 110 | + |
| 111 | +rate = count / duration |
| 112 | +print(f"有效采集速率: {rate:.2f} Hz") |
| 113 | + |
0 commit comments