#!/usr/bin/env python3 """Spray-drone flight plans for facades: passes at a held standoff, sorties, go/no-go gates, and a QGC mission. A drone paints a facade the way a sprayer on a lift does — horizontal passes, gun on over wall, off over windows — but three things change, and this module makes each one explicit: 1. **Where the wall is.** The facade is a plane: two ground points at its base (local east/north metres from home, or from a depth/mono_depth plane fit) give its line and outward normal; heights come from the wall-plane plan. Waypoint = base + x·along + (standoff + boom)·out + z·up, yaw facing the wall: the nozzle sits ``standoff`` from the wall on a boom, so the airframe keeps its own minimum distance. 2. **Holding the film.** Fan width — and so film thickness — scales with standoff. The plan states the standoff tolerance that keeps film within ±``film_tolerance``, and the lateral accuracy that keeps the pass overlap (half the overlap band). Standard GNSS (~1.5 m) cannot hold either: the gate requires RTK or a wall-relative sensor (forward rangefinder / depth — ``mono_depth.py`` + a range reading is the camera-only fallback). 3. **Endurance.** Paint and battery run out; the job splits into sorties that each end with ``battery_reserve`` left, and the drone returns to refill/swap between them. Gates (all must pass, or the plan is refused with reasons): wind at or below the limit, positioning accuracy within the overlap budget, standoff above the minimum safe distance, altitude within the ceiling (120 m default — check the rules where you fly; spraying from a drone needs operator authorisation in most jurisdictions). ``inspection_plan`` gives camera positions (footprint from FOV and standoff, 70% overlap) for the re-scan that closes the loop: photograph -> predict -> robot_planner --observe -> touch-up sortie. python3 drone_plan.py --plan plan.json --wall-a 0,0 --wall-b 12,0 --home 37.7749,-122.4194,10 \\ --tip 517 --dft-um 38 --vs 0.38 --positioning-m 0.02 --wind-ms 4 --qgc mission.plan """ from __future__ import annotations import argparse import json import math import application import hardware EARTH_R = 6378137.0 def facade_frame(a_en, b_en): """Unit vectors (east, north) along the facade A->B and outward (to the right of A->B, i.e. towards the drone).""" 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]) return along, out, length def to_world(x_mm, z_mm, a_en, along, out, standoff_m, ground_alt_m=0.0): e = a_en[0] + along[0] * x_mm / 1000 + out[0] * standoff_m n = a_en[1] + along[1] * x_mm / 1000 + out[1] * standoff_m return e, n, ground_alt_m + z_mm / 1000 def yaw_facing(out): """Heading (deg, clockwise from north) pointing back at the wall, i.e. along -out.""" return (math.degrees(math.atan2(-out[0], -out[1])) + 360.0) % 360.0 def gates(spray, standoff_m, positioning_m, wind_ms, max_alt_m, top_m, wind_limit_ms=8.0, min_standoff_m=0.6, film_tolerance=0.15, ceiling_m=120.0, boom_m=0.0, reaction_n=0.0, hold_force_n=20.0, sag_mm=0.0): reasons = [] overlap_band_m = (spray["fan_width_mm"] - spray["pass_spacing_mm"]) / 1000 lateral_budget = overlap_band_m / 2 standoff_tol = standoff_m * film_tolerance # film ~ 1/fan width ~ 1/standoff if wind_ms > wind_limit_ms: reasons.append(f"wind {wind_ms} m/s above the {wind_limit_ms} m/s spray limit (drift and position hold)") if positioning_m > lateral_budget: reasons.append(f"positioning {positioning_m * 100:.0f} cm cannot hold the {lateral_budget * 100:.1f} cm overlap budget: " "use RTK or a wall-relative sensor") if positioning_m > standoff_tol: reasons.append(f"standoff must hold ±{standoff_tol * 100:.1f} cm for ±{film_tolerance:.0%} film; positioning is " f"{positioning_m * 100:.0f} cm: add a forward rangefinder") if reaction_n > 0.25 * hold_force_n: reasons.append(f"the spray pushes back with {reaction_n:.1f} N; the hold is assumed to have {hold_force_n:.0f} N of lateral " "authority: use a smaller tip, less pressure, or a stiffer hold") if sag_mm / 1000.0 > standoff_tol: reasons.append(f"the reaction moves the nozzle {sag_mm:.1f} mm off its {standoff_m * 100:.0f} cm standoff, past the " f"±{standoff_tol * 100:.1f} cm budget for ±{film_tolerance:.0%} film: stiffen the hold or reduce the flow") if standoff_m + boom_m < min_standoff_m: reasons.append(f"the airframe would be {standoff_m + boom_m:.2f} m from the wall (nozzle standoff {standoff_m} m + " f"boom {boom_m} m), inside the {min_standoff_m} m minimum: fit a longer spray boom") if top_m > min(max_alt_m, ceiling_m): reasons.append(f"facade top {top_m:.1f} m above the {min(max_alt_m, ceiling_m):.0f} m ceiling") return {"go": not reasons, "reasons": reasons, "lateral_budget_m": round(lateral_budget, 4), "standoff_tolerance_m": round(standoff_tol, 4), "reaction_n": round(float(reaction_n), 2), "sag_mm": round(float(sag_mm), 1), "hold_force_n": round(float(hold_force_n), 1)} def sorties(strokes_mm, spray, speed_ms, flight_min, battery_reserve, tank_l, transit_s): """Split strokes (in order) into sorties by usable flight time and paint, each ending with the reserve unused.""" usable_s = flight_min * 60 * (1 - battery_reserve) - 2 * transit_s flow_lps = spray["flow_l_min"] / 60 if usable_s <= 0: raise ValueError("the transit to the facade and back uses the whole usable flight time") out, current, used_s, used_l = [], [], 0.0, 0.0 for stroke in strokes_mm: length_m = sum(math.dist(p, q) for p, q in zip(stroke[:-1], stroke[1:])) / 1000 dt, dl = length_m / speed_ms + 4.0, length_m / speed_ms * flow_lps # +4 s to line up each pass if current and (used_s + dt > usable_s or used_l + dl > tank_l): out.append({"strokes": current, "spray_s": round(used_s, 1), "paint_l": round(used_l, 2)}) current, used_s, used_l = [], 0.0, 0.0 current.append(stroke) used_s += dt used_l += dl if current: out.append({"strokes": current, "spray_s": round(used_s, 1), "paint_l": round(used_l, 2)}) return out def local_to_geo(e, n, home): lat0, lon0 = math.radians(home[0]), math.radians(home[1]) return (math.degrees(lat0 + n / EARTH_R), math.degrees(lon0 + e / (EARTH_R * math.cos(lat0)))) def qgc_mission(plan_sorties, home, a_en, along, out, standoff_m, speed_ms, pump_servo=9, pwm_on=1900, pwm_off=1100): """QGroundControl .plan (MAVLink mission items) for one sortie each: takeoff, speed, passes with pump on/off, RTL.""" missions = [] yaw = yaw_facing(out) for s in plan_sorties: items, seq = [], 1 def item(command, params, alt=0.0): nonlocal seq items.append({"type": "SimpleItem", "autoContinue": True, "command": command, "doJumpId": seq, "frame": 3, "params": params, "Altitude": alt, "AltitudeMode": 1, "AMSLAltAboveTerrain": None}) seq += 1 first = s["strokes"][0][0] e, n, alt = to_world(first[0], first[1], a_en, along, out, standoff_m) item(22, [0, 0, 0, yaw, *local_to_geo(e, n, home), alt], alt) # NAV_TAKEOFF item(178, [1, speed_ms, -1, 0, 0, 0, 0]) # DO_CHANGE_SPEED for stroke in s["strokes"]: for i, (x, z) in enumerate(stroke): e, n, alt = to_world(x, z, a_en, along, out, standoff_m) item(16, [0, 0.10, 0, yaw, *local_to_geo(e, n, home), alt], alt) # NAV_WAYPOINT, 10 cm accept if i == 0: item(183, [pump_servo, pwm_on, 0, 0, 0, 0, 0]) # DO_SET_SERVO: pump on item(183, [pump_servo, pwm_off, 0, 0, 0, 0, 0]) # pump off at the pass end item(20, [0, 0, 0, 0, 0, 0, 0]) # NAV_RETURN_TO_LAUNCH missions.append({"fileType": "Plan", "version": 1, "groundStation": "QGroundControl", "mission": {"version": 2, "firmwareType": 12, "vehicleType": 2, "cruiseSpeed": speed_ms, "hoverSpeed": speed_ms, "plannedHomePosition": list(home), "items": items}, "geoFence": {"circles": [], "polygons": [], "version": 2}, "rallyPoints": {"points": [], "version": 2}}) return missions def inspection_plan(wall_width_mm, wall_height_mm, standoff_m, hfov_deg=70.0, aspect=0.75, overlap=0.7): """Camera stations for the re-scan: footprint from FOV at the standoff, ``overlap`` between neighbours.""" fw = 2 * standoff_m * math.tan(math.radians(hfov_deg) / 2) fh = fw * aspect step_x, step_z = fw * (1 - overlap), fh * (1 - overlap) xs = [min(fw / 2 + i * step_x, max(fw / 2, wall_width_mm / 1000 - fw / 2)) for i in range(max(1, math.ceil((wall_width_mm / 1000 - fw) / step_x) + 1))] zs = [min(fh / 2 + j * step_z, max(fh / 2, wall_height_mm / 1000 - fh / 2)) for j in range(max(1, math.ceil((wall_height_mm / 1000 - fh) / step_z) + 1))] return {"footprint_m": [round(fw, 2), round(fh, 2)], "stations": [[round(x * 1000), round(z * 1000)] for z in zs for x in xs], "photos": len(xs) * len(zs), "mm_per_px_at_4000px": round(fw * 1000 / 4000, 2)} def build(strokes_mm, wall_width_mm, wall_height_mm, a_en, b_en, home, spray, standoff_m=0.30, positioning_m=0.02, wind_ms=3.0, flight_min=18.0, battery_reserve=0.25, tank_l=10.0, max_alt_m=120.0, boom_m=0.5, pressure_psi=2000.0, hold_force_n=20.0, hold_stiffness_n_per_mm=0.5): along, out, length = facade_frame(a_en, b_en) if wall_width_mm / 1000 > length + 0.5: raise ValueError(f"the plan is {wall_width_mm / 1000:.1f} m wide but the facade line is {length:.1f} m") speed_ms = spray["traverse_speed_mm_s"] / 1000 transit_s = math.hypot(a_en[0], a_en[1]) / 3.0 + 20.0 reaction = application.spray_reaction(spray["flow_l_min"] / 60.0, pressure_psi) sag_mm = reaction["reaction_n"] / max(float(hold_stiffness_n_per_mm), 1e-3) setpoint_m = max(0.05, standoff_m - sag_mm / 1000.0) # hold the airframe this much closer, so the nozzle lands on standoff verdict = gates(spray, standoff_m, positioning_m, wind_ms, max_alt_m, wall_height_mm / 1000, boom_m=boom_m, reaction_n=reaction["reaction_n"], hold_force_n=hold_force_n, sag_mm=sag_mm) parts = sorties(strokes_mm, spray, speed_ms, flight_min, battery_reserve, tank_l, transit_s) return {"gate": verdict, "standoff_m": standoff_m, "boom_m": boom_m, "airframe_distance_m": setpoint_m + boom_m, "standoff_setpoint_mm": round(setpoint_m * 1000.0, 1), "standoff_bias_mm": round(sag_mm, 1), "speed_ms": round(speed_ms, 3), "yaw_deg": round(yaw_facing(out), 1), "reaction": {**reaction, "sag_mm": round(sag_mm, 1), "setpoint_mm": round(setpoint_m * 1000.0, 1), "hold_force_n": hold_force_n, "hold_stiffness_n_per_mm": hold_stiffness_n_per_mm}, "sorties": [{"index": i + 1, "passes": len(s["strokes"]), "spray_s": s["spray_s"], "paint_l": s["paint_l"]} for i, s in enumerate(parts)], "paint_l_total": round(sum(s["paint_l"] for s in parts), 2), "inspection": inspection_plan(wall_width_mm, wall_height_mm, max(standoff_m * 6, 2.0)), "_sorties": parts, "_frame": (along, out)} def main(): ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) ap.add_argument("--plan", required=True, help="robot_planner.py plan JSON with a mm scale (facade plane)") ap.add_argument("--wall-a", required=True, help="facade base start, local east,north metres from home") ap.add_argument("--wall-b", required=True, help="facade base end") ap.add_argument("--home", required=True, help="lat,lon,alt of the launch point") ap.add_argument("--tip", default="517"); ap.add_argument("--pressure-psi", type=float, default=2000) ap.add_argument("--dft-um", type=float, required=True); ap.add_argument("--vs", type=float, required=True) ap.add_argument("--standoff-m", type=float, default=0.30, help="nozzle to wall") ap.add_argument("--boom-m", type=float, default=0.5, help="spray boom: the airframe flies standoff + boom from the wall") ap.add_argument("--positioning-m", type=float, default=1.5, help="1-sigma position hold (RTK ~0.02)") ap.add_argument("--wind-ms", type=float, required=True) ap.add_argument("--flight-min", type=float, default=18); ap.add_argument("--tank-l", type=float, default=10) ap.add_argument("--qgc", help="write QGroundControl .plan files (one per sortie: NAME-1.plan, ...)") a = ap.parse_args() plan = json.loads(open(a.plan).read()) strokes, height = hardware.strokes_from_plan(plan) width = plan["plane_size"]["width"] * plan["plane_size"]["mm_per_unit"] tip = application.SprayTip.parse(a.tip) spray = application.spray_settings(tip.flow_lps(a.pressure_psi), tip.fan_angle_deg, a.standoff_m * 1000, a.dft_um, a.vs) a_en, b_en = (tuple(float(v) for v in a.wall_a.split(",")), tuple(float(v) for v in a.wall_b.split(","))) home = tuple(float(v) for v in a.home.split(",")) result = build(strokes, width, height, a_en, b_en, home, spray, a.standoff_m, a.positioning_m, a.wind_ms, a.flight_min, tank_l=a.tank_l, boom_m=a.boom_m) if a.qgc and result["gate"]["go"]: along, out = result["_frame"] for i, m in enumerate(qgc_mission(result["_sorties"], home, a_en, along, out, result["standoff_setpoint_mm"] / 1000.0 + a.boom_m, result["speed_ms"])): open(a.qgc.replace(".plan", f"-{i + 1}.plan"), "w").write(json.dumps(m, indent=1)) print(json.dumps({k: v for k, v in result.items() if not k.startswith("_")}, indent=1)) if __name__ == "__main__": main()