constructelligence's picture
Update code: real-machine G-code adapter, commissioning gate, machine_job runner, pump watchdog firmware, twin completion gate, realism and masking modules (card and weights unchanged)
4eecd03 verified
Raw History Blame Contribute Delete
10.3 kB
"""Flight logs in, wall-frame strokes and measured flight quality out.
A log is a CSV with one row per telemetry sample (what QGroundControl / Mission Planner / PX4 Flight Review export, or a
logger you write). Column names are matched loosely:
time t, time, time_s, timestamp, time_boot_ms (milliseconds)
position lat/lon/alt (degrees, metres) OR e/n/u, east/north/up (metres from home) OR x/y/z
pump pump, spray, servo, pwm, pump_pwm (>= 1500 or 1 = spraying; the pump servo's PWM works as it is)
``strokes_from_log`` turns the spraying runs into polylines in the twin's pixel frame for a facade
(x along the wall, z up from the ground, the photo's bottom row = the ground), and ``flight_quality`` measures what the
drone really did against the planned passes: cross-track error, standoff, speed. Those measurements replace the
planning assumptions (``calibrate``): the margin the plan keeps off glass, and how well the twin knows where paint landed.
Assumption stated, not hidden: the photo is taken square-on with the wall filling the frame from the ground up, so a point at
height z above the ground sits z / mm_per_px pixels above the bottom row.
"""
from __future__ import annotations
import csv
import io
import math
import numpy as np
EARTH_R = 6378137.0
ALIASES = {
"t": ("t", "time", "time_s", "timestamp", "seconds"),
"t_ms": ("time_boot_ms", "time_ms", "t_ms"),
"lat": ("lat", "latitude"), "lon": ("lon", "lng", "longitude"), "alt": ("alt", "altitude", "amsl"),
"relalt": ("relative_alt", "rel_alt", "height", "alt_rel", "alt_agl"),
"e": ("e", "east", "x", "local_x"), "n": ("n", "north", "y", "local_y"), "u": ("u", "up", "z", "local_z"),
"pump": ("pump", "spray", "servo", "pwm", "pump_pwm", "servo9", "trigger"),
}
def _col(header, key):
low = {h.strip().lower(): h for h in header}
for name in ALIASES[key]:
if name in low:
return low[name]
return None
def read_log(text, home=None):
"""Parse CSV text -> dict of float arrays ``t`` (s), ``e``, ``n``, ``u`` (m from home), ``pump`` (bool). Raises ValueError
naming what is missing."""
rows = list(csv.DictReader(io.StringIO(text)))
if len(rows) < 5:
raise ValueError("the log has fewer than 5 samples")
header = list(rows[0].keys())
c = {k: _col(header, k) for k in ALIASES}
if not c["pump"]:
raise ValueError("no pump column (looked for: " + ", ".join(ALIASES["pump"]) + "): the log must say when the drone was spraying")
num = lambda col: np.array([float(r[col]) if r[col] not in ("", None) else np.nan for r in rows], float)
if c["t_ms"]:
t = num(c["t_ms"]) / 1000.0
elif c["t"]:
t = num(c["t"])
else:
raise ValueError("no time column (looked for: " + ", ".join(ALIASES["t"] + ALIASES["t_ms"]) + ")")
if c["lat"] and c["lon"]:
lat, lon = num(c["lat"]), num(c["lon"])
if not home:
raise ValueError("a lat/lon log needs the home position (lat, lon[, alt]): the launch point the facade's east/north "
"coordinates are measured from. Guessing it from the first sample would shift the whole job.")
h0 = home
e = np.radians(lon - h0[1]) * EARTH_R * math.cos(math.radians(h0[0]))
n = np.radians(lat - h0[0]) * EARTH_R
# Heights: a column named relative_alt / rel_alt / height / alt_agl is already above the launch point; a plain alt /
# altitude / amsl column is above sea level, so the home altitude is subtracted (and must then be given).
if c["relalt"]:
u = num(c["relalt"])
elif c["alt"]:
if len(home) < 3:
raise ValueError("the log's altitude is above sea level (column %r): give the home altitude as the third home value, "
"or log a relative_alt column" % c["alt"])
u = num(c["alt"]) - home[2]
else:
u = np.zeros_like(e)
elif c["e"] and c["n"]:
e, n = num(c["e"]), num(c["n"])
u = num(c["u"]) if c["u"] else np.zeros_like(e)
else:
raise ValueError("no position columns (lat/lon[/alt] or e/n[/u])")
p = num(c["pump"])
pump = (p >= 1500) if np.nanmax(p) > 5 else (p >= 0.5)
ok = np.isfinite(t) & np.isfinite(e) & np.isfinite(n) & np.isfinite(u)
order = np.argsort(t[ok])
return {"t": t[ok][order], "e": e[ok][order], "n": n[ok][order], "u": u[ok][order], "pump": pump[ok][order]}
def to_wall_frame(log, a_en, b_en):
"""x along the facade from A (m), y distance out from the wall plane (m, positive = away from the wall), z height (m)."""
de, dn = b_en[0] - a_en[0], b_en[1] - a_en[1]
length = math.hypot(de, dn)
if length < 0.5:
raise ValueError("facade points are closer than 0.5 m")
along = (de / length, dn / length)
out = (along[1], -along[0])
rx, ry = log["e"] - a_en[0], log["n"] - a_en[1]
return {"x": rx * along[0] + ry * along[1], "y": rx * out[0] + ry * out[1], "z": log["u"], "t": log["t"], "pump": log["pump"], "length_m": length}
def strokes_from_log(log, a_en, b_en, mm_per_px, height_px, ground_alt_m=0.0, min_len_m=0.3, max_gap_s=1.5):
"""Spraying runs as polylines [[col, row], ...] in the twin's pixel frame. A run ends when the pump goes off or the
samples have a gap longer than ``max_gap_s``; runs shorter than ``min_len_m`` are dropped (a pump blip)."""
w = to_wall_frame(log, a_en, b_en)
strokes, cur = [], []
def close():
nonlocal cur
if len(cur) >= 2 and sum(math.dist(p, q) for p, q in zip(cur[:-1], cur[1:])) >= min_len_m:
strokes.append([[x * 1000.0 / mm_per_px, height_px - (z - ground_alt_m) * 1000.0 / mm_per_px] for x, z in cur])
cur = []
last_t = None
for x, z, t, on in zip(w["x"], w["z"], w["t"], w["pump"]):
if on and (last_t is None or t - last_t <= max_gap_s or not cur):
cur.append((float(x), float(z)))
else:
close()
if on:
cur.append((float(x), float(z)))
last_t = t
close()
return strokes
def flight_quality(log, a_en, b_en, planned_rows_m, target_standoff_m, boom_m=0.5, ground_alt_m=0.0):
"""What the drone really did while spraying.
``planned_rows_m`` = heights (m above the ground) of the planned horizontal passes. Cross-track error is the distance of
each spraying sample from the nearest planned pass; standoff error is the nozzle distance from the wall
(airframe distance - boom) against the target; speed from consecutive samples."""
w = to_wall_frame(log, a_en, b_en)
on = w["pump"]
if on.sum() < 5:
raise ValueError("the log has fewer than 5 spraying samples")
rows = np.asarray(sorted(planned_rows_m), float)
z = w["z"][on] - ground_alt_m
cross = np.min(np.abs(z[:, None] - rows[None, :]), axis=1)
standoff = w["y"][on] - boom_m
dt = np.diff(w["t"])
dist = np.hypot(np.diff(w["x"]), np.diff(w["z"]))
moving = on[1:] & on[:-1] & (dt > 0)
speed = dist[moving] / dt[moving] if moving.any() else np.array([0.0])
return {"spraying_samples": int(on.sum()), "spray_seconds": round(float(dt[on[1:]].sum()), 1) if on[1:].any() else 0.0,
"cross_track_rms_m": round(float(np.sqrt(np.mean(cross ** 2))), 4), "cross_track_p95_m": round(float(np.percentile(cross, 95)), 4),
"standoff_mean_m": round(float(standoff.mean()), 3), "standoff_error_rms_m": round(float(np.sqrt(np.mean((standoff - target_standoff_m) ** 2))), 4),
"speed_mean_ms": round(float(speed.mean()), 3), "speed_p95_ms": round(float(np.percentile(speed, 95)), 3)}
def calibrate(quality, floor_positioning_m=0.01):
"""Turn measured flight quality into the numbers the plan and the twin use instead of assumptions."""
pos = max(quality["cross_track_rms_m"], floor_positioning_m)
return {"positioning_m": round(pos, 4), # the margin model's sigma (drone_areas.spray_margin_mm)
"telemetry_sigma_m": round(max(0.5 * pos, 0.005), 4), # how well the twin should believe the logged path
"standoff_error_rms_m": quality["standoff_error_rms_m"], "speed_ms": quality["speed_mean_ms"],
"source": "flight log", "samples": quality["spraying_samples"]}
def synth_log(strokes_px, mm_per_px, height_px, a_en, b_en, speed_ms=0.4, standoff_m=0.3, boom_m=0.5, rate_hz=10, seed=0, wobble_m=0.02, home=(37.7749, -122.4194)):
"""A synthetic log for tests and demos: the drone flies the strokes with its own wobble. CLEARLY SYNTHETIC, not data."""
rng = np.random.default_rng(seed)
de, dn = b_en[0] - a_en[0], b_en[1] - a_en[1]
length = math.hypot(de, dn)
along = (de / length, dn / length)
out = (along[1], -along[0])
t, rows = 0.0, []
for s in strokes_px:
err = rng.normal(0, wobble_m)
pts = np.array(s, float)
seg = np.hypot(*np.diff(pts, axis=0).T) * mm_per_px / 1000.0
total = seg.sum()
n = max(2, int(total / speed_ms * rate_hz))
cum = np.concatenate([[0], np.cumsum(seg)])
for k in range(n + 1):
d = total * k / n
i = min(np.searchsorted(cum, d, side="right") - 1, len(seg) - 1)
f = (d - cum[i]) / max(seg[i], 1e-9)
px, py = pts[i] + (pts[i + 1] - pts[i]) * f
x_m, z_m = px * mm_per_px / 1000.0, (height_px - py) * mm_per_px / 1000.0 + err + rng.normal(0, 0.004)
y_m = standoff_m + boom_m + rng.normal(0, 0.01)
e = a_en[0] + along[0] * x_m + out[0] * y_m
n_ = a_en[1] + along[1] * x_m + out[1] * y_m
lat = home[0] + math.degrees(n_ / EARTH_R)
lon = home[1] + math.degrees(e / (EARTH_R * math.cos(math.radians(home[0]))))
rows.append((round(t, 2), lat, lon, z_m, 1900))
t += 1.0 / rate_hz
rows.append((round(t, 2), rows[-1][1], rows[-1][2], rows[-1][3], 1100)) # pump off at the end of the pass
t += 3.0 # line up the next pass
head = "time_s,lat,lon,relative_alt,pump_pwm\n"
return head + "\n".join(f"{a},{b:.8f},{c:.8f},{d:.3f},{e}" for a, b, c, d, e in rows)