"""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)