FallenSigh
e939b16d2d
Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent) Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
103 lines
3.4 KiB
Python
103 lines
3.4 KiB
Python
#!/usr/bin/env python3
|
|
"""Generate fake drone telemetry data for testing pipeview Lua decoder."""
|
|
|
|
import argparse
|
|
import math
|
|
import random
|
|
import socket
|
|
import time
|
|
|
|
|
|
def generate_line(t: float) -> str:
|
|
"""Generate one telemetry line with simulated sensor values."""
|
|
# Quaternion (w, x, y, z) — gently rotating
|
|
angle = t * 0.5
|
|
qw = math.cos(angle)
|
|
qx = math.sin(angle) * 0.1
|
|
qy = math.sin(angle) * 0.05
|
|
qz = math.sin(angle) * 0.02
|
|
|
|
# Yaw/Pitch/Roll — sine wave simulation
|
|
yaw = math.degrees(math.sin(t * 0.3)) * 30
|
|
pitch = math.degrees(math.sin(t * 0.5)) * 15
|
|
roll = math.degrees(math.sin(t * 0.7)) * 10
|
|
|
|
# Gyro (deg/s)
|
|
gz = math.sin(t * 0.3) * 50 + random.uniform(-2, 2)
|
|
gy = math.sin(t * 0.5) * 30 + random.uniform(-2, 2)
|
|
gx = math.sin(t * 0.7) * 20 + random.uniform(-2, 2)
|
|
|
|
# RC channels (1000-2000 us)
|
|
rc_r = int(1500 + math.sin(t * 0.3) * 200)
|
|
rc_p = int(1500 + math.sin(t * 0.5) * 150)
|
|
rc_t = int(1000 + (math.sin(t * 0.1) + 1) * 500) # throttle 1000-2000
|
|
rc_y = int(1500 + math.sin(t * 0.3) * 100)
|
|
|
|
# Motors (1000-2000)
|
|
base = 1200 + int((math.sin(t * 0.1) + 1) * 400)
|
|
m1 = base + random.randint(-20, 20)
|
|
m2 = base + random.randint(-20, 20)
|
|
m3 = base + random.randint(-20, 20)
|
|
m4 = base + random.randint(-20, 20)
|
|
|
|
return (
|
|
f"AHRS q:{qw:.4f},{qx:.4f},{qy:.4f},{qz:.4f}|"
|
|
f"YPR:{yaw:.2f},{pitch:.2f},{roll:.2f}|"
|
|
f"Gyro:{gz:.2f},{gy:.2f},{gx:.2f}|"
|
|
f"RC:{rc_r},{rc_p},{rc_t},{rc_y}|"
|
|
f"M:{m1},{m2},{m3},{m4}|"
|
|
f"L:0 F:1 C:0\n"
|
|
)
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description="Drone telemetry test data generator")
|
|
parser.add_argument("--host", default="127.0.0.1", help="TCP host")
|
|
parser.add_argument("--port", type=int, default=8092, help="TCP port")
|
|
parser.add_argument("--rate", type=float, default=10, help="Lines per second")
|
|
parser.add_argument("--duration", type=float, default=0, help="Seconds to run (0 = forever)")
|
|
args = parser.parse_args()
|
|
|
|
interval = 1.0 / args.rate
|
|
print(f"Sending drone telemetry to {args.host}:{args.port} at {args.rate} Hz")
|
|
|
|
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server:
|
|
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
server.bind((args.host, args.port))
|
|
server.listen(1)
|
|
print(f"Listening on {args.host}:{args.port}, waiting for connections...")
|
|
|
|
while True:
|
|
sock, addr = server.accept()
|
|
print(f"Client connected from {addr}")
|
|
|
|
t0 = time.time()
|
|
seq = 0
|
|
|
|
try:
|
|
while True:
|
|
t = time.time() - t0
|
|
line = generate_line(t)
|
|
try:
|
|
sock.sendall(line.encode())
|
|
except (BrokenPipeError, ConnectionResetError):
|
|
break
|
|
seq += 1
|
|
|
|
if args.duration > 0 and t >= args.duration:
|
|
break
|
|
|
|
elapsed = time.time() - t0
|
|
target = (seq + 1) * interval
|
|
sleep_time = target - elapsed
|
|
if sleep_time > 0:
|
|
time.sleep(sleep_time)
|
|
except (BrokenPipeError, ConnectionResetError):
|
|
pass
|
|
|
|
print(f"Client disconnected. Sent {seq} lines. Waiting for new connection...")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|