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
14.4 kB
#!/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()