Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
ATPMQTT_project/data-process.py
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
127 lines (98 sloc)
4.42 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |