IMU运动传感器简易测试平台搭建方案

从了解了我们给无人机行业提供的几款传感器后，小编对无人机相关的东西就更加关注了。

经典的，就是我们的一体式的空速、高度传感器(AUAV)，和攻角传感器(AAoA)(如上图所示)。尤其是在没有GPS或者北斗辅助获取速度或高度的情况下，只能凭借无人机自生的装备了。

当飞机“读错”了气流：从737 MAX事故到攻角/迎角(AOA)传感器技术要求讲起

位置传感器-舵机-摄像头-跟踪人体的简易云台的测试(1)：摄像头和舵机标定测试

位置传感器-舵机-摄像头-跟踪人体的简易云台的测试(2)：Yolo26 vs RF-DETR

对于无人机来说，高度，速度和攻角有了，不过相对于地球这个参考系来说，要了解无人机的其它姿态还需要更多的传感器。空速+高度+攻角能保证“气动上飞得安全”，但不能保证“飞到你想要的地方、保持你想要的姿态”，完整飞控还需要姿态(俯仰/滚转/偏航)和位置参考。对于俯仰/滚转/偏航角度，在不考虑其它更多传感器的情况下，小编就单纯地测试了一下一款6轴(3轴加速度+3轴陀螺仪[旋转角度])的IMU运动传感器，想了解一下这类传感器的精度和稳定性特性。各位可以在测试代码中看到相关型号。

不过事先可以告知一下这类IMU传感器的特点：

直线匀速或静止时，加速度计算获取的俯仰/滚转角度测量仍然可信;

加速运动时，加速度计模长偏离1g，俯仰/滚转姿态角度检测就会被污染;

陀螺仪在短期内测量各个旋转角度虽可信，但长期测量结果会飘;

高频时加速度计容易被干扰;低频时陀螺仪容易被干扰——这才会有互补滤波法，和卡尔曼滤波等信号处理的加入;

俯仰和滚转的偏移可以参考重力来纠正，而偏航旋转角度的偏移的纠正则要借助外部器件，比如如磁力计。

IMU传感器简易测试平台搭建

硬件配置：一个EPS32S3+I2C接口的IMU模块

ESP32S3固件：卡尔曼滤波，二进制串口输出@115200bps

以下是对应的固件，卡尔曼滤波器函数可以从github上从对应作者名下来下载(https://github.com/TKJElectronics/KalmanFilter)。关于卡尔曼滤波器，请参考[聊一点卡尔曼滤波器(Kalman Filter)及仿真]。

输出值的测试中，特别对短期内偏航旋转角度从45°>90°>...>0°的顺序测量进行了比较：

测试角度(°)45.00, 90.00, 45.00, 0.00, 45.00, 90.00, 45.00, 0.00

输出值(°) 44.09, 89.53, 44.55, 1.26, 44.67, 89.93, 45.26, 1.61

偏转角的测试结果看起来不错，不过十几分钟后，和规格书上说的那样，整体偏航角度就飘得无法接受了，而其它两个方向的角度(俯仰/滚转)则因为有重力矢量的参考，相对稳定。偏航角度的测量还是要依照厂家建议利用额外器件进行长期补偿纠正。

/*
* ESP32-S3 + MPU6050 + Kalman filter (Kristian Lauszus, TKJ Electronics)
*
* 运行时动态计算的 dt。
*
* 输出帧格式：
* [0]=0xAA [1]=0x55 [2]=LEN [3]=SEQ [4]=FLAGS
* payload: ax, ay, az, gx, gy, gz, temp, pitch, roll, yaw (各 int16 小端)
* checksum: 对 [LEN... 最后一个 payload 字节] 做 XOR
*
* 定点缩放：
* ax/ay/az/gx/gy/gz × 10000
* temp/pitch/roll/yaw × 100
*/
#include
 
  
#include
  
    #include"Kalman.h"// 需与本.ino 放在同一目录（或库目录）下 #defineMPU_ADDR 0x68 #defin
   eSDA_PIN 8 #defineSCL_PIN 9 // MPU6050 registe
   rs #defineREG_SMPLRT_DIV 0x19 #defineREG_CONFIG 0x1A #defineREG_GYRO_CONFIG 0x1B #defineREG_
   ACCEL_CONFIG 0x1C #defineREG_PWR_MGMT_1 0x6B #defineREG_WHO_AM_I 0x75 #defineREG_ACCEL_XOUT_H 0x3B // 量程换算系数： // 加速度 ±2g => 16384 LSB/g // 陀螺 ±250 => 131 LSB/(deg/s) staticconstfloatACCEL_LSB_PER_G =16384.0f; staticconstfloatGYRO_LSB_PER_DPS =131.0f; // 零偏（标定得到） floatax_offset =0, ay_offset =0, az_offset =0; floatgx_offset =0, gy_offset =0, gz_offset =0; // ---------------- 卡尔曼滤波器实例 ---------------- // 与原始示例一致：X 轴滤波器负责 roll，Y 轴滤波器负责 pitch Kalman kalmanX; Kalman kalmanY; // 卡尔曼滤波输出角度 floatpitch_kf =0.0f; floatroll_kf =0.0f; // yaw 无绝对参考，仅陀螺积分 floatyaw_kf =0.0f; boolcalibrated =false; uint8_tseq_no =0; // 动态 dt 相关 unsignedlonglastMic
   ros =0; floatloop_dt =1.0f/125.0f; // 初始默认值，仅用于第一次循环兜底 // ---------------- I2C 基础读写 ---------------- voidwriteByte(uint8_treg,uint8_tdata){ Wire.beginTransmission(MPU_ADDR); Wire.write(reg); Wire.write(data); Wire.endTransmission(true); } uint8_treadByte(uint8_treg){ Wire.beginTransmission(MPU_ADDR); Wire.write(reg); Wire.endTransmission(false); Wire.reques
   tF
   rom(MPU_ADDR, (uint8_t)1); returnWire.av
   ailable() ? Wire.read() :0; } voidreadBytes(uint8_treg,uint8_tcount,uint8_t*dest){ Wire.beginTransmission(MPU_ADDR); Wire.write(reg); Wire.endTransmission(false); Wire.requestFrom(MPU_ADDR, count); for(uint8_ti =0; i < count && Wire.available(); i++) {     dest[i] = Wire.read();   } } int16_t combineBytes(uint8_t high, uint8_t low) {   return (int16_t)((high << 8) | low); } // ---------------- MPU6050 初始化 ---------------- void mpuInit() {   // 唤醒设备，时钟源选 PLL with X-axis gyro reference（比内部振荡器稳定）   writeByte(REG_PWR_MGMT_1, 0x01);   // 采样率 = 1kHz / (1 + 7) = 125 Hz   writeByte(REG_SMPLRT_DIV, 7);   // DLPF_CFG = 3：陀螺与加速度计均开启中等低通滤波   writeByte(REG_CONFIG, 0x03);   // 陀螺满量程 ±250 dps   writeByte(REG_GYRO_CONFIG, 0x00);   // 加速度满量程 ±2g   writeByte(REG_ACCEL_CONFIG, 0x00);   delay(100); } // ---------------- 零偏标定 ---------------- void calibrateMPU6050(int samples = 500) {   long ax_sum = 0, ay_sum = 0, az_sum = 0;   long gx_sum = 0, gy_sum = 0, gz_sum = 0;   Serial.println("Calibration: keep the sensor still and flat...");   for (int i = 0; i < samples; i++) {     uint8_t raw[14];     readBytes(REG_ACCEL_XOUT_H, 14, raw);     int16_t ax = combineBytes(raw[0], raw[1]);     int16_t ay = combineBytes(raw[2], raw[3]);     int16_t az = combineBytes(raw[4], raw[5]);     int16_t gx = combineBytes(raw[8], raw[9]);     int16_t gy = combineBytes(raw[10], raw[11]);     int16_t gz = combineBytes(raw[12], raw[13]);     ax_sum += ax;     ay_sum += ay;     az_sum += az;     gx_sum += gx;     gy_sum += gy;     gz_sum += gz;     delay(3);   }   ax_offset = (float)ax_sum / samples;   ay_offset = (float)ay_sum / samples;   // 板子水平放置、Z 轴朝上时，静止应输出 1g；   // 减去 1g 后，静止时 az ≈ 0   az_offset = (float)az_sum / samples - ACCEL_LSB_PER_G;   gx_offset = (float)gx_sum / samples;   gy_offset = (float)gy_sum / samples;   gz_offset = (float)gz_sum / samples;   calibrated = true;   Serial.println("Calibration done."); } // ---------------- 读取并换算传感器数据 ---------------- void readSensor(float &ax_g, float &ay_g, float &az_g,                 float &gx_dps, float &gy_dps, float &gz_dps,                 float &temp_c,                 int16_t &ax_raw, int16_t &ay_raw, int16_t &az_raw) {   uint8_t raw[14];   readBytes(REG_ACCEL_XOUT_H, 14, raw);   ax_raw = combineBytes(raw[0], raw[1]);   ay_raw = combineBytes(raw[2], raw[3]);   az_raw = combineBytes(raw[4], raw[5]);   int16_t temp_raw = combineBytes(raw[6], raw[7]);   int16_t gx_raw = combineBytes(raw[8], raw[9]);   int16_t gy_raw = combineBytes(raw[10], raw[11]);   int16_t gz_raw = combineBytes(raw[12], raw[13]);   float ax_corr = (float)ax_raw - ax_offset;   float ay_corr = (float)ay_raw - ay_offset;   float az_corr = (float)az_raw - az_offset;   float gx_corr = (float)gx_raw - gx_offset;   float gy_corr = (float)gy_raw - gy_offset;   float gz_corr = (float)gz_raw - gz_offset;   ax_g = ax_corr / ACCEL_LSB_PER_G;   ay_g = ay_corr / ACCEL_LSB_PER_G;   az_g = az_corr / ACCEL_LSB_PER_G;   gx_dps = gx_corr / GYRO_LSB_PER_DPS;   gy_dps = gy_corr / GYRO_LSB_PER_DPS;   gz_dps = gz_corr / GYRO_LSB_PER_DPS;   temp_c = (temp_raw / 340.0f) + 36.53f; } // ---------------- 由加速度计算观测角度 ---------------- void computeAccelAngles(float ax, float ay, float az,                         float &pitch_acc, float &roll_acc) {   roll_acc = atan2(ay, az) * 180.0f / PI;   pitch_acc = atan(-ax / sqrt(ay * ay + az * az)) * 180.0f / PI; } // ---------------- 静止判定（与原程序一致） ---------------- bool isStill(float ax, float ay, float az, float gx, float gy, float gz) {   float acc_mag = sqrt(ax * ax + ay * ay + az * az);   float gyro_mag = sqrt(gx * gx + gy * gy + gz * gz);   const float ACC_TOL = 0.05f;  // 与 1g 偏差阈值   const float GYRO_TOL = 1.5f;  // 陀螺幅值阈值 dps   return (fabs(acc_mag - 1.0f) < ACC_TOL) && (gyro_mag < GYRO_TOL); } // ---------------- 校验与帧打包（与原程序一致） ---------------- uint8_t xorChecksum(const uint8_t *data, uint16_t len) {   uint8_t cs = 0;   for (uint16_t i = 0; i < len; i++) cs ^= data[i];   return cs; } template
   
     voidappendBytes(uint8_t*buf,uint16_t&idx, T value){ uint8_t*p = (uint8_t*)&value; for(size_ti =0; i < sizeof(T); i++) {     buf[idx++] = p[i];   } } // 帧格式与原程序完全一致，未做改动 void sendBinaryFrame(float ax, float ay, float az,                      float gx, float gy, float gz,                      float temp_c,                      float pitch, float roll, float yaw,                      bool still) {   int16_t ax_i = (int16_t)(ax * 10000.0f);   int16_t ay_i = (int16_t)(ay * 10000.0f);   int16_t az_i = (int16_t)(az * 10000.0f);   int16_t gx_i = (int16_t)(gx * 10000.0f);   int16_t gy_i = (int16_t)(gy * 10000.0f);   int16_t gz_i = (int16_t)(gz * 10000.0f);   int16_t temp_i = (int16_t)(temp_c * 100.0f);   int16_t pitch_i = (int16_t)(pitch * 100.0f);   int16_t roll_i = (int16_t)(roll * 100.0f);   int16_t yaw_i = (int16_t)(yaw * 100.0f);   uint8_t flags = 0;   if (still) flags |= 0x01;   if (calibrated) flags |= 0x02;   flags |= 0x04;  // data valid   uint8_t buf[64];   uint16_t idx = 0;   buf[idx++] = 0xAA;   buf[idx++] = 0x55;   buf[idx++] = 0;  // LEN 占位   buf[idx++] = seq_no++;   buf[idx++] = flags;   appendBytes(buf, idx, ax_i);   appendBytes(buf, idx, ay_i);   appendBytes(buf, idx, az_i);   appendBytes(buf, idx, gx_i);   appendBytes(buf, idx, gy_i);   appendBytes(buf, idx, gz_i);   appendBytes(buf, idx, temp_i);   appendBytes(buf, idx, pitch_i);   appendBytes(buf, idx, roll_i);   appendBytes(buf, idx, yaw_i);   // LEN = LEN 字段之后到校验和之前的字节数   buf[2] = idx - 3;   uint8_t cs = xorChecksum(&buf[2], idx - 2);   buf[idx++] = cs;   Serial.write(buf, idx); } // ---------------- setup ---------------- void setup() {   Serial.begin(115200);   delay(1000);   Wire.begin(SDA_PIN, SCL_PIN);   Wire.setClock(400000);   Serial.println("Init MPU6050 (Kalman filter, dynamic dt)...");   uint8_t who = readByte(REG_WHO_AM_I);   Serial.print("WHO_AM_I = 0x");   Serial.println(who, HEX);   mpuInit();   calibrateMPU6050(500);   // 用第一帧加速度数据初始化卡尔曼滤波器的起始角度，   // 避免上电后滤波器从 0 开始缓慢收敛   {     float ax, ay, az, gx, gy, gz, t;     int16_t rax, ray, raz;     readSensor(ax, ay, az, gx, gy, gz, t, rax, ray, raz);     float pitch_acc, roll_acc;     computeAccelAngles(ax, ay, az, pitch_acc, roll_acc);     kalmanX.setAngle(roll_acc);     kalmanY.setAngle(pitch_acc);     roll_kf = roll_acc;     pitch_kf = pitch_acc;     yaw_kf = 0.0f;   }   // 初始化时间基准   lastMicros = micros();   Serial.println("MPU6050 ready."); } // ---------------- loop ---------------- void loop() {   unsigned long now = micros();   if (lastMicros == 0) {     lastMicros = now;   }   float dt = (now - lastMicros) / 1000000.0f;   lastMicros = now;   // dt 限幅，避免偶发卡顿造成过大的积分步长   if (dt < 0.001f) dt = 0.001f;   if (dt >0.050f) dt =0.050f; floatax, ay, az, gx, gy, gz, temp_c; int16_tax_raw, ay_raw, az_raw; readSensor(ax, ay, az, gx, gy, gz, temp_c, ax_raw, ay_raw, az_raw); // 加速度观测角度 floatpitch_acc, roll_acc; computeAccelAngles(ax, ay, az, pitch_acc, roll_acc); // ---- 卡尔曼滤波（与
    算法提供者示例一致的 ±90° 跳变处理） ---- if((roll_acc < -90.0f && roll_kf >90.0f) || (roll_acc >90.0f&& roll_kf < -90.0f)) {     kalmanX.setAngle(roll_acc);     roll_kf = roll_acc;   } else {     roll_kf = kalmanX.getAngle(roll_acc, gx, dt);   }   // Y 滤波器：pitch（观测 = pitch_acc，预测 = gyroY）   float gyroYrate = gy;   if (fabs(roll_kf) >90.0f) { gyroYrate = -gyroYrate; } pitch_kf = kalmanY.getAngle(pitch_acc, gyroYrate, dt); // yaw：无绝对参考，仅陀螺积分（会漂移） yaw_kf += gz * dt; if(yaw_kf >180.0f) yaw_kf -=360.0f; if(yaw_kf < -180.0f) yaw_kf += 360.0f;   bool still = isStill(ax, ay, az, gx, gy, gz);   // 输出帧格式与原程序完全一致：pitch / roll / yaw 依次填入   sendBinaryFrame(ax, ay, az, gx, gy, gz, temp_c,                   pitch_kf, roll_kf, yaw_kf, still);   delay(5); }
   
  
 

上位机飞机建模和接收输出

1. 先用Blender(一个免费的3D建模工具)编辑一个飞机造型

2. 在Blender中导出为obj文件

其中后缀名中带1的表示的是旧版本。各位如果需要这个文件，可以留言小编。 3. 编写一个python脚本来接收ESP32信号，并读取obj文件后输出更新

上位机接收到来自ESP32S3模块串口发送的数据，根据这些数据对pyVista控件构建的3D飞行器更新其俯仰/滚转/偏航角度。这个界面中的物体的角度、远近和大小可以通过键盘鼠标进行调整(请查阅参考脚本中的操作注释)。

小编试过用3轴加速度获取位移，反正下图中的3D模型一会儿就“跑”到界面边缘去了。

importos
importmath
importtime
importstruct
importserial
importnumpyasnp
importpyvistaaspv
importvtk
# =========================================================
# 串口参数
# =========================================================
PORT ="COM9"
BAUD =115200
# =========================================================
# 协议参数
# =========================================================
FRAME_HEAD =b'xAAx55'
PAYLOAD_FMT ='
 
  int:
  cs =0
 forbindata:
    cs ^= b
 returncs
defread_exact(ser, n):
  buf =bytearray()
 whilelen(buf) < n:
        chunk = ser.read(n - len(buf))
        if chunk:
            buf.extend(chunk)
    return bytes(buf)
def find_header(ser):
    while True:
        b = ser.read(1)
        if not b:
            continue
        if b[0] == 0xAA:
            b2 = ser.read(1)
            if b2 and b2[0] == 0x55:
                return True
def parse_frame(frame: bytes):
    if len(frame) < 6:
        raise ValueError("Frame too short")
    if frame[0:2] != FRAME_HEAD:
        raise ValueError("Bad frame header")
    length = frame[2]
    seq = frame[3]
    flags = frame[4]
    payload = frame[5:-1]
    chk = frame[-1]
    calc = xor_checksum(frame[2:-1])
    if calc != chk:
        raise ValueError(f"Checksum mismatch: calc={calc:02X}, got={chk:02X}")
    if len(payload) != PAYLOAD_SIZE:
        raise ValueError(
            f"Payload size mismatch: expected {PAYLOAD_SIZE}, got {len(payload)}"
        )
    (ax_i, ay_i, az_i,
     gx_i, gy_i, gz_i,
     temp_i, pitch_i, roll_i, yaw_i) = struct.unpack(PAYLOAD_FMT, payload)
    return {
        "seq": seq,
        "flags": flags,
        "still": bool(flags & 0x01),
        "calibrated": bool(flags & 0x02),
        "valid": bool(flags & 0x04),
        "ax": ax_i / 10000.0,
        "ay": ay_i / 10000.0,
        "az": az_i / 10000.0,
        "gx": gx_i / 10000.0,
        "gy": gy_i / 10000.0,
        "gz": gz_i / 10000.0,
        "temp": temp_i / 100.0,
        "pitch": pitch_i / 100.0,
        "roll": roll_i / 100.0,
        "yaw": yaw_i / 100.0,
    }
def smooth_value(prev, new, alpha):
    return prev * (1.0 - alpha) + new * alpha
def find_obj_file(folder, filename=None):
    if filename:
        path = os.path.join(folder, filename)
        if not os.path.isfile(path):
            raise FileNotFoundError(f"OBJ not found: {path}")
        return path
    objs = [
        os.path.join(folder, f)
        for f in os.listdir(folder)
        if f.lower().endswith(".obj")
    ]
    if not objs:
        raise FileNotFoundError(f"No OBJ files found in folder: {folder}")
    return objs[0]
def rotation_matrix(pitch_deg, roll_deg, yaw_deg):
    """
    调整后的旋转逻辑：
    - yaw 绕 Z 轴
    - pitch 绕 Y 轴
    - roll 绕 X 轴
    """
    pitch = math.radians(pitch_deg)
    roll = math.radians(roll_deg)
    yaw = math.radians(yaw_deg)
    Ry = np.array([
        [math.cos(pitch), 0, math.sin(pitch)],
        [0, 1, 0],
        [-math.sin(pitch), 0, math.cos(pitch)]
    ], dtype=float)
    Rz = np.array([
        [math.cos(yaw), -math.sin(yaw), 0],
        [math.sin(yaw), math.cos(yaw), 0],
        [0, 0, 1]
    ], dtype=float)
    Rx = np.array([
        [1, 0, 0],
        [0, math.cos(roll), -math.sin(roll)],
        [0, math.sin(roll), math.cos(roll)]
    ], dtype=float)
    return Ry @ Rz @ Rx
def prepare_mesh(mesh: pv.PolyData):
    """
    居中并标准化大小，方便显示。
    """
    mesh = mesh.copy(deep=True)
    mesh.points = mesh.points - mesh.center
    bounds = mesh.bounds
    scale = max(
        bounds[1] - bounds[0],
        bounds[3] - bounds[2],
        bounds[5] - bounds[4],
    )
    if scale >0:
    mesh.points = mesh.points / scale
 returnmesh
defsetup_custom_interaction(plotter):
 """
  交互方式：
  - 左键拖动: 旋转
  - Shift + 左键拖动: 平移
  - 中键拖动: 平移
  - 右键拖动: 缩放
  - 滚轮: 缩放
  - 方向键: 平移微调
  """
  interactor = plotter.iren.interactor
  style = vtk.vtkInteractorStyleTrackballCamera()
  interactor.SetInteractorStyle(style)
 defon_key_press(obj, event):
    key = obj.GetKeySym()
    camera = plotter.camera
    pos = np.array(camera.GetPosition(), dtype=float)
    focal = np.array(camera.GetFocalPoint(), dtype=float)
    view_up = np.array(camera.GetViewUp(), dtype=float)
    direction = focal - pos
    direction = direction / (np.linalg.norm(direction) +1e-12)
    right = np.cross(direction, view_up)
    right = right / (np.linalg.norm(right) +1e-12)
    up = np.cross(right, direction)
    up = up / (np.linalg.norm(up) +1e-12)
    step =0.1
   ifkey =="Left":
      delta = -step * right
   elifkey =="Right":
      delta = step * right
   elifkey =="Up":
      delta = step * up
   elifkey =="Down":
      delta = -step * up
   else:
     return
    camera.SetPosition(*(pos + delta))
    camera.SetFocalPoint(*(focal + delta))
    plotter.render()
  interactor.AddObserver("KeyPressEvent", on_key_press)
# =========================================================
# 主程序
# =========================================================
defmain():
  obj_path = find_obj_file(MODEL_DIR, OBJ_FILENAME)
 print("Lo
  ading OBJ:", obj_path)
  mesh = pv.read(obj_path)
 ifnotisinstance(mesh, pv.PolyData):
    mesh = mesh.extract_surface()
  mesh = prepare_mesh(mesh)
  base_points = mesh.points.copy()
  current_mesh = mesh.copy(deep=True)
 try:
    ser = serial.Serial(PORT, BAUD, timeout=0.01)
    time.sleep(2)
    ser.reset_input_buffer()
   print(f"Serial opened:{PORT}@{BAUD}")
 exceptExceptio
  nase:
   print(f"Fai
  led to open serial{PORT}:{e}")
   return
  state = {"pitch":0.0,"roll":0.0,"yaw":0.0}
  last_frame_info = {"text":"Waiting for IMU data..."}
  pl = pv.Plotter(window_size=(1280,900))
  pl.set_background("black")
  pl.add_axes()
  floor = pv.Plane(
    center=(0,0, -0.6),
    direction=(0,0,1),
    i_size=8,
    j_size=8,
    i_resolution=1,
    j_resolution=1
  )
  pl.add_mesh(floor, color="gray", opacity=0.25)
  x_line = pv.Line((0,0,0), (2.5,0,0))
  y_line = pv.Line((0,0,0), (0,2.5,0))
  z_line = pv.Line((0,0,0), (0,0,2.5))
  pl.add_mesh(x_line, color="red", line_width=4)
  pl.add_mesh(y_line, color="green", line_width=4)
  pl.add_mesh(z_line, color="blue", line_width=4)
  actor = pl.add_mesh(
    current_mesh,
    color="orange",
    smooth_shading=True,
    name="airplane",
    copy_mesh=False
  )
  text_actor = pl.add_text(
   "Waiting for IMU data...",
    name="info",
    font_size=12,
    color="yellow"
  )
 defset_text(text):
   try:
      text_actor.SetText(2, text)
   exceptException:
     try:
        pl.remove_actor("info")
     exceptException:
       pass
      pl.add_text(text, name="info", font_size=12)
 defread_latest_frame(ser):
    last_data =None
   for_inrange(300):
     ifser.in_waiting < 6:
                break
            b = ser.read(1)
            if not b or b[0] != 0xAA:
                continue
            b2 = ser.read(1)
            if not b2 or b2[0] != 0x55:
                continue
            length_b = ser.read(1)
            if not length_b:
                break
            length = length_b[0]
            rest = ser.read(length + 1)
            if len(rest) < length + 1:
                break
            frame = FRAME_HEAD + length_b + rest
            try:
                data = parse_frame(frame)
                last_data = data
            except Exception:
                continue
        return last_data
    def update_state():
        data = read_latest_frame(ser)
        if data is None:
            return False
        target_pitch = PITCH_SIGN * data["pitch"]
        target_roll = ROLL_SIGN * data["roll"]
        target_yaw = YAW_SIGN * data["yaw"]
        cur_pitch = smooth_value(state["pitch"], target_pitch, ALPHA)
        cur_roll = smooth_value(state["roll"], target_roll, ALPHA)
        cur_yaw = smooth_value(state["yaw"], target_yaw, ALPHA)
        state["pitch"] = cur_pitch
        state["roll"] = cur_roll
        state["yaw"] = cur_yaw
        R = rotation_matrix(cur_pitch, cur_roll, cur_yaw)
        new_points = base_points @ R.T
        actor.mapper.dataset.points = new_points
        try:
            actor.mapper.dataset.Modified()
        except Exception:
            pass
        last_frame_info["text"] = (
            f"SEQ={data['seq']} {'STILL' if data['still'] else 'MOVE'} "
            f"Pitch={cur_pitch:+.2f} Roll={cur_roll:+.2f} Yaw={cur_yaw:+.2f}
"
            f"ACC=({data['ax']:+.3f}, {data['ay']:+.3f}, {data['az']:+.3f}) g
"
            f"GYR=({data['gx']:+.3f}, {data['gy']:+.3f}, {data['gz']:+.3f}) dps
"
            f"TEMP={data['temp']:.2f} C"
        )
        set_text(last_frame_info["text"])
        return True
    setup_custom_interaction(pl)
    pl.show(auto_close=False, interactive_update=True)
    pl.reset_camera()
    pl.camera.zoom(1.5)
    pl.render()
    frame_count = 0
    render_every = 2
    loop_sleep = 0.002
    print(">>> Entering interactive loop. 按 Ctrl+C 退出。")
 try:
   whileTrue:
      updated = update_state()
      f
  rame_count +=1
     ifframe_count % render_every ==0:
        pl.update()
     ifupdated:
        time.sleep(loop_sleep)
     else:
        time.sleep(0.001)
 exceptKeyboardInterrupt:
   print("Interrupted by user.")
 finally:
    ser.close()
    pl.close()
if__name__ =="__main__":
  main()
 

在windows中，VTK控件可能需要另外下载一个wheel文件再安装。

看效果：

按板上Reset键可以复位初始角度为0，上位机界面中的飞机也就可以复位到初始位置。

类似传感器其它的应用呢?

这类传感器的应用从来不限于无人机。

安费诺Piher传感器系统TS系列陀螺仪补偿式倾角仪就是其中的一款。TSDA倾角传感器是可靠且精确的传感器，非常适合需要快速响应和高精度加速度、倾斜、冲击与振动测量的应用。该双轴惯性测量单元(IMU)基于无机械结构的陀螺仪补偿MEMS技术，可在恶劣环境条件下精确测量倾角、倾斜和角度。

其能够在整个温度范围内以<0.5°的精度测量高达360°的角度，非常适合重载应用，例如车辆载荷监测、调平、臂架角度监测、滑移装载机、移动式起重机、平台调平以及装载机铲斗位置，而且在一些负载运输型的无人机上也是适用的。其先进的传感原理确保输出始终精确，而坚固的结构则足以应对最严苛的环境。而且很显然，TSDA这款产品似乎已经不需要额外的温度补偿了。

小结

让咱们再回到无人机的应用。要不要再聊一下振动、噪声的测量?还是电池包、电机的温度测量?大电流、开关信号也可以。涉及东西太多了，这次就这样。诸君如需文中飞机的3D模型文件，请跟小编吱一声。