eSlider
3deaa382e1
One process per drone: seeded patrol kinematics, sensor streams at realistic rates, detection events, hot current/ blocks sealed into Hive-partitioned Parquet, and the compact UDP state broadcast with peer RSSI derivation. Scales via docker compose --scale.
76 lines
2.1 KiB
Python
76 lines
2.1 KiB
Python
"""Kinematics: a seeded patrol route and the drone's motion along it."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import random
|
|
from dataclasses import dataclass, replace
|
|
|
|
from .config import Config
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Pose:
|
|
ts_ns: int
|
|
x: float
|
|
y: float
|
|
z: float
|
|
roll: float
|
|
pitch: float
|
|
yaw: float
|
|
vx: float
|
|
vy: float
|
|
vz: float
|
|
|
|
|
|
def make_waypoints(cfg: Config, rng: random.Random) -> list[tuple[float, float]]:
|
|
"""Perimeter patrol with per-drone jitter so drones do not overlap."""
|
|
s = cfg.area_size_m
|
|
jitter = rng.uniform(0.05, 0.20) * s
|
|
corners = [(jitter, jitter), (s - jitter, jitter), (s - jitter, s - jitter), (jitter, s - jitter)]
|
|
start = rng.randrange(4)
|
|
return corners[start:] + corners[:start]
|
|
|
|
|
|
def initial_pose(cfg: Config, rng: random.Random, ts_ns: int) -> Pose:
|
|
wps = make_waypoints(cfg, rng)
|
|
x, y = wps[0]
|
|
return Pose(ts_ns=ts_ns, x=x, y=y, z=cfg.cruise_alt_m, roll=0.0, pitch=0.0, yaw=0.0, vx=0.0, vy=0.0, vz=0.0)
|
|
|
|
|
|
def step(pose: Pose, target: tuple[float, float], cfg: Config, rng: random.Random, dt: float, ts_ns: int) -> tuple[Pose, bool]:
|
|
"""Advance one tick toward the target waypoint. Returns (pose, reached)."""
|
|
dx, dy = target[0] - pose.x, target[1] - pose.y
|
|
dist = math.hypot(dx, dy)
|
|
reached = dist < cfg.cruise_speed_ms * dt * 2
|
|
|
|
speed = cfg.cruise_speed_ms * rng.uniform(0.9, 1.1)
|
|
if dist > 1e-6:
|
|
vx, vy = dx / dist * speed, dy / dist * speed
|
|
else:
|
|
vx, vy = 0.0, 0.0
|
|
|
|
yaw = math.degrees(math.atan2(vy, vx))
|
|
# Small attitude noise around the commanded values
|
|
roll = rng.gauss(0.0, 1.5)
|
|
pitch = rng.gauss(2.0, 1.0)
|
|
z = cfg.cruise_alt_m + rng.gauss(0.0, 0.5)
|
|
vz = (z - pose.z) / dt if dt > 0 else 0.0
|
|
|
|
return (
|
|
replace(
|
|
pose,
|
|
ts_ns=ts_ns,
|
|
x=pose.x + vx * dt,
|
|
y=pose.y + vy * dt,
|
|
z=z,
|
|
roll=roll,
|
|
pitch=pitch,
|
|
yaw=yaw,
|
|
vx=vx,
|
|
vy=vy,
|
|
vz=vz,
|
|
),
|
|
reached,
|
|
)
|