165 lines
5 KiB
Python
165 lines
5 KiB
Python
# V005.py (VERSION: QT-ORDER-FIX-1)
|
||
print("=== V005 VERSION: QT-ORDER-FIX-1 ===")
|
||
|
||
import sys
|
||
|
||
# ✅ MUST create QApplication BEFORE any QWidget exists
|
||
from PyQt5 import QtWidgets, QtCore
|
||
app = QtWidgets.QApplication.instance()
|
||
if app is None:
|
||
app = QtWidgets.QApplication(sys.argv)
|
||
|
||
# Now it is safe to import pyqtgraph / OpenGL stuff
|
||
import asyncio
|
||
import math
|
||
import time
|
||
import threading
|
||
import numpy as np
|
||
|
||
import pyqtgraph.opengl as gl
|
||
from bleak import BleakClient
|
||
|
||
# ====== HARD-CODED DEVICE ======
|
||
ADDRESS = "10:51:DB:1B:E7:1E"
|
||
CHAR_UUID = "7e2a1002-1111-2222-3333-444455556666"
|
||
|
||
latest = {"ax": 0.0, "ay": 0.0, "az": 1.0, "ts": 0.0, "count": 0, "raw": ""}
|
||
|
||
def parse_csv_triplet(s: str):
|
||
s = s.strip().strip('"')
|
||
parts = s.split(",")
|
||
if len(parts) != 3:
|
||
return None
|
||
try:
|
||
return float(parts[0]), float(parts[1]), float(parts[2])
|
||
except ValueError:
|
||
return None
|
||
|
||
def notify_handler(_sender: int, data: bytearray):
|
||
s = data.decode(errors="ignore").strip()
|
||
latest["raw"] = s
|
||
v = parse_csv_triplet(s)
|
||
if v is None:
|
||
return
|
||
latest["ax"], latest["ay"], latest["az"] = v
|
||
latest["ts"] = time.time()
|
||
latest["count"] += 1
|
||
|
||
def roll_pitch_from_accel(ax, ay, az):
|
||
roll = math.degrees(math.atan2(ay, az))
|
||
pitch = math.degrees(math.atan2(-ax, math.sqrt(ay * ay + az * az)))
|
||
return roll, pitch
|
||
|
||
async def ble_loop():
|
||
while True:
|
||
try:
|
||
print(f"Connecting to {ADDRESS} ...")
|
||
async with BleakClient(ADDRESS, timeout=15.0) as client:
|
||
if not client.is_connected:
|
||
raise RuntimeError("Connect failed.")
|
||
print("Connected. Enabling notifications...")
|
||
await client.start_notify(CHAR_UUID, notify_handler)
|
||
print("Notify enabled. Streaming...\n")
|
||
while True:
|
||
await asyncio.sleep(1.0)
|
||
except Exception as e:
|
||
print(f"[BLE] Error/disconnect: {e}")
|
||
print("Reconnecting in 2 seconds...\n")
|
||
await asyncio.sleep(2.0)
|
||
|
||
def start_ble_thread():
|
||
t = threading.Thread(target=lambda: asyncio.run(ble_loop()), daemon=True)
|
||
t.start()
|
||
|
||
def make_cuboid(size=(2.0, 1.2, 0.1)):
|
||
L, W, T = size
|
||
x, y, z = L/2, W/2, T/2
|
||
verts = np.array([
|
||
[-x,-y,-z], [ x,-y,-z], [ x, y,-z], [-x, y,-z],
|
||
[-x,-y, z], [ x,-y, z], [ x, y, z], [-x, y, z],
|
||
], dtype=float)
|
||
faces = np.array([
|
||
[0,1,2],[0,2,3],
|
||
[4,6,5],[4,7,6],
|
||
[0,4,5],[0,5,1],
|
||
[1,5,6],[1,6,2],
|
||
[2,6,7],[2,7,3],
|
||
[3,7,4],[3,4,0],
|
||
], dtype=int)
|
||
return verts, faces
|
||
|
||
class Viewer(QtWidgets.QMainWindow):
|
||
def __init__(self):
|
||
super().__init__()
|
||
self.setWindowTitle("ADXL345 – 3D PCB Tilt (BLE)")
|
||
|
||
self.view = gl.GLViewWidget()
|
||
self.view.setCameraPosition(distance=6, elevation=25, azimuth=40)
|
||
self.setCentralWidget(self.view)
|
||
|
||
grid = gl.GLGridItem()
|
||
grid.setSize(10, 10)
|
||
grid.setSpacing(1, 1)
|
||
self.view.addItem(grid)
|
||
|
||
axis = gl.GLAxisItem()
|
||
axis.setSize(2, 2, 2)
|
||
self.view.addItem(axis)
|
||
|
||
verts, faces = make_cuboid()
|
||
colors = np.ones((faces.shape[0], 4), dtype=float)
|
||
colors[:,0]=0.25; colors[:,1]=0.7; colors[:,2]=0.35; colors[:,3]=0.9
|
||
|
||
meshdata = gl.MeshData(vertexes=verts, faces=faces)
|
||
self.mesh = gl.GLMeshItem(meshdata=meshdata, faceColors=colors, drawEdges=True, edgeColor=(0,0,0,1))
|
||
self.view.addItem(self.mesh)
|
||
|
||
self.vec = gl.GLLinePlotItem(pos=np.array([[0,0,0],[0,0,1]]), width=3, antialias=True)
|
||
self.view.addItem(self.vec)
|
||
|
||
self.label = QtWidgets.QLabel(self)
|
||
self.label.setStyleSheet("color:white; background:rgba(0,0,0,140); padding:6px;")
|
||
self.label.move(10, 10)
|
||
self.label.resize(440, 90)
|
||
|
||
self.roll = 0.0
|
||
self.pitch = 0.0
|
||
|
||
self.timer = QtCore.QTimer()
|
||
self.timer.timeout.connect(self.update_scene)
|
||
self.timer.start(20)
|
||
|
||
def update_scene(self):
|
||
axg, ayg, azg = latest["ax"], latest["ay"], latest["az"]
|
||
roll, pitch = roll_pitch_from_accel(axg, ayg, azg)
|
||
|
||
alpha = 0.15
|
||
self.roll = (1-alpha)*self.roll + alpha*roll
|
||
self.pitch = (1-alpha)*self.pitch + alpha*pitch
|
||
|
||
self.mesh.resetTransform()
|
||
self.mesh.rotate(self.pitch, 0, 1, 0)
|
||
self.mesh.rotate(self.roll, 1, 0, 0)
|
||
|
||
self.vec.setData(pos=np.array([[0,0,0],[axg, ayg, azg]], dtype=float))
|
||
|
||
age = (time.time()-latest["ts"])*1000 if latest["ts"] else 0
|
||
self.label.setText(
|
||
f"Packets: {latest['count']} Age: {age:.0f} ms\n"
|
||
f"ax={axg:+.3f}g ay={ayg:+.3f}g az={azg:+.3f}g\n"
|
||
f"roll={self.roll:+.1f}° pitch={self.pitch:+.1f}°\n"
|
||
f"raw: {latest['raw']}"
|
||
)
|
||
|
||
def main():
|
||
print("IMPORTANT: Disconnect nRF Connect (or any other BLE client) first.")
|
||
start_ble_thread()
|
||
|
||
win = Viewer()
|
||
win.resize(900, 650)
|
||
win.show()
|
||
|
||
sys.exit(app.exec_())
|
||
|
||
if __name__ == "__main__":
|
||
main()
|