data-process.py

import paho.mqtt.client as mqtt
import json
import numpy as np
import time
import matplotlib.pyplot as plt

data = json

class SimplePoseEstimator:
    def __init__(self, alpha=0.98, beta=0.98):
        self.alpha = alpha  # 互补滤波器参数，用于加权平均
        self.beta = beta    # 同上，可根据实际情况调整
        
        # 初始化角度状态（以弧度为单位，便于后续计算）
        self.roll, self.pitch, self.yaw = 0, 0, 0
        
        # 陀螺仪积分角度（会有累积误差）
        self.gyro_roll, self.gyro_pitch, self.gyro_yaw = 0, 0, 0
        
        # 上一帧时间戳
        self.prev_time = time.time()

    def update(self, data):
        """根据加速度计和陀螺仪数据更新姿态"""
        curr_time = time.time()
        dt = curr_time - self.prev_time
        self.prev_time = curr_time
        
        # 解析数据
        ax, ay, az = data['accel_x'], data['accel_y'], data['accel_z']
        gx, gy, gz = data['gyro_x'], data['gyro_y'], data['gyro_z']
        
        # 陀螺仪数据积分得到角速度变化
        self.gyro_roll += gx * dt
        self.gyro_pitch += gy * dt
        self.gyro_yaw += gz * dt
        
        # 角速度到角度的转换需要积分，这里简化处理，直接使用累积值
        # 实际应用中应考虑如何减少累积误差，比如通过重置或使用其他传感器数据校正
        
        # 使用加速度计数据进行修正（简化处理，实际需要考虑重力向量的正确投影）
        norm = np.sqrt(ax**2 + ay**2 + az**2)
        if norm > 0.1:  # 防止初始未稳定或异常值影响
            self.roll = np.arctan2(ay, ax)  # 修正roll
            self.pitch = np.arcsin(az / norm)  # 修正pitch
            
            # 使用互补滤波融合加速度计和陀螺仪数据
            self.roll = self.alpha * (self.roll + self.gyro_roll*dt) + (1-self.alpha) * self.roll
            self.pitch = self.alpha * (self.pitch + self.gyro_pitch*dt) + (1-self.alpha) * self.pitch
        
        # yaw角（航向）一般需要磁力计数据来准确计算，这里简化处理，不涉及
        # self.yaw 的更新逻辑，实际应用中需要根据磁力计数据融合计算
        
        # 返回当前姿态角度（转换为度数）
        return np.degrees([self.roll, self.pitch, self.yaw])

# 连接成功时的回调函数
def on_connect(client, userdata, flags, rc):
    print("Connected to MQTT broker")
    # 订阅需要接收的主题
    client.subscribe("sensor/data")
    # print("Connection result code: ", rc)

# 收到消息时的回调函数
def on_message(client, userdata, msg):
    data = json.loads(msg.payload)
    # print(f"Received data: {data}")
    # 在这里处理接收到的消息
    processed_data = json.dumps({'temperature': data['temperature'], 'pressure': data['pressure']})
    # print(f"Processed data: {processed_data}")
    # 发布处理后的消息
    client.publish("dashboard/data", processed_data)
    estimator = SimplePoseEstimator()
    angles = estimator.update(data)
    print(f"Roll: {angles[0]:.2f}°, Pitch: {angles[1]:.2f}°, Yaw: {angles[2]:.2f}°")


    pitch, yaw = angles[0], angles[1]

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # 初始化模型（简单立方体示例）
    cube_verts = np.array([
        [-1, -1, -1],
        [1, -1, -1],
        [1, 1, -1],
        [-1, 1, -1],
        [-1, -1, 1],
        [1, -1, 1],
        [1, 1, 1],
        [-1, 1, 1]
    ])

    # 定义立方体边
    edges = [
        (0, 1), (1, 2), (2, 3), (3, 0),
        (4, 5), (5, 6), (6, 7), (7, 4),
        (0, 4), (1, 5), (2, 6), (3, 7)
    ]

    # 绘制立方体
    for edge in edges:
        ax.plot3D(*zip(cube_verts[edge[0]], cube_verts[edge[1]]), color='b')

    # 应用旋转（此处简化处理，实际应用中需根据俯仰、偏航、滚转计算旋转矩阵并应用）
    # 注意：此处仅为示意，实际旋转计算较为复杂，需考虑坐标系变换
    # 以下代码未直接实现基于欧拉角的旋转，仅为示意

    # 更新视角
    ax.view_init(elev=pitch, azim=yaw)
    


# 创建MQTT客户端
client = mqtt.Client()

# 设置连接成功和消息接收的回调函数
client.on_connect = on_connect
client.on_message = on_message

# 连接到MQTT broker
client.username_pw_set("admin", "public")
client.connect("localhost", 1883, 60)

# 开始循环处理MQTT消息
client.loop_forever()