commit 17c2ae5ce8a93dc5ffc70926542fc455ae0d0f63 Author: nicoboy Date: Sat May 30 10:35:15 2026 +0200 Initial commit — WireClaw SITL fonctionnel diff --git a/.env.example b/.env.example new file mode 100644 index 0000000..7921305 --- /dev/null +++ b/.env.example @@ -0,0 +1,3 @@ +GEMINI_API_KEY=your_gemini_api_key_here +TELEGRAM_TOKEN= +TELEGRAM_CHAT_ID= diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fc0ac9a --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.env +__pycache__/ +*.pyc +mav.tlog +mav.parm diff --git a/config.py b/config.py new file mode 100644 index 0000000..97166b6 --- /dev/null +++ b/config.py @@ -0,0 +1,14 @@ +from dotenv import load_dotenv +import os + +load_dotenv() + +GEMINI_API_KEY = os.getenv("GEMINI_API_KEY") +GEMINI_MODEL = "gemini-2.0-flash" +MAVLINK_ADDRESS = "udp://:14551" +MAVLINK_PYMAV = "udpin:0.0.0.0:14552" +NATS_SERVER = "nats://localhost:4222" +TELEGRAM_TOKEN = os.getenv("TELEGRAM_TOKEN", "") +TELEGRAM_CHAT_ID = os.getenv("TELEGRAM_CHAT_ID", "") +BASE_LAT = -35.363262 +BASE_LON = 149.165237 diff --git a/sitl_params.parm b/sitl_params.parm new file mode 100644 index 0000000..6175917 --- /dev/null +++ b/sitl_params.parm @@ -0,0 +1,4 @@ +BATT_MONITOR 0 +SIM_BATT_VOLTAGE 14.0 +BATT_LOW_VOLT 0 +BATT_CRT_VOLT 0 diff --git a/start.sh b/start.sh new file mode 100755 index 0000000..b8cfff7 --- /dev/null +++ b/start.sh @@ -0,0 +1,63 @@ +#!/bin/bash + +echo "=== WireClaw Startup ===" + +# 1. Nettoyage +echo "Nettoyage..." +pkill -f "hermes" 2>/dev/null +pkill -f "arducopter" 2>/dev/null +pkill -f "mavproxy" 2>/dev/null +pkill -f "wireclaw_core" 2>/dev/null +sleep 2 +sudo fuser -k 14550/udp 14551/udp 14552/udp 2>/dev/null +sleep 1 + +# 2. Lancer ArduCopter SITL directement +echo "Lancement ArduCopter..." +/home/nicoboy/ardupilot/build/sitl/bin/arducopter \ + --model + \ + --speedup 1 \ + --sim-address=127.0.0.1 \ + --defaults /home/nicoboy/wireclaw/sitl_params.parm \ + > /tmp/arducopter.log 2>&1 & +sleep 3 + +# 3. Lancer MAVProxy directement +echo "Lancement MAVProxy..." +/home/nicoboy/.local/bin/mavproxy.py \ + --master tcp:127.0.0.1:5760 \ + --sitl 127.0.0.1:5501 \ + --out udp:127.0.0.1:14551 \ + --out udp:127.0.0.1:14552 \ + --daemon \ + > /tmp/arducopter.log 2>&1 & +sleep 2 + +# 4. Attendre GPS fix +echo "Attente GPS fix..." +timeout=120 +while [ $timeout -gt 0 ]; do + if grep -q "IMU0 is using GPS" /tmp/arducopter.log 2>/dev/null; then + echo "" + echo "✅ SITL prêt !" + break + fi + sleep 2 + timeout=$((timeout-2)) + echo -n "." +done + +if [ $timeout -le 0 ]; then + echo "" + echo "❌ Timeout — logs :" + tail -10 /tmp/arducopter.log + tail -10 /tmp/arducopter.log + exit 1 +fi + +sleep 2 + +# 5. Lancer WireClaw +echo "Lancement WireClaw..." +cd ~/wireclaw +python3 wireclaw_core.py diff --git a/wireclaw_core.py b/wireclaw_core.py new file mode 100644 index 0000000..6768923 --- /dev/null +++ b/wireclaw_core.py @@ -0,0 +1,221 @@ +import asyncio +import json +import math +from google import genai +from google.genai import types +from mavsdk import System +from mavsdk.mission import MissionItem, MissionPlan +from pymavlink import mavutil +from config import (GEMINI_API_KEY, GEMINI_MODEL, + MAVLINK_ADDRESS, MAVLINK_PYMAV, + BASE_LAT, BASE_LON) + +client = genai.Client(api_key=GEMINI_API_KEY) + +SYSTEM_PROMPT = f"""Tu es WireClaw, cerveau d'un drone d'inspection autonome. +Reponds UNIQUEMENT avec un objet JSON valide, sans markdown. +Format : +{{ + "action": "takeoff|land|rtl|hover|goto|orbit|status", + "altitude": , + "lat": , + "lon": , + "radius": , + "speed": , + "message": "" +}} +Position de base : lat={BASE_LAT}, lon={BASE_LON}. +Pour goto, calcule des coordonnees proches (max 500m en test). +Nord = lat+, Sud = lat-, Est = lon+, Ouest = lon- +1 degre lat = 111320m, 1 degre lon = 111320*cos(lat)m +""" + +drone_armed = False +current_pos = {"lat": BASE_LAT, "lon": BASE_LON, "alt": 0, "abs_alt": 584} +mav = None + +def init_pymavlink(): + global mav + print("Connexion pymavlink sur 14552...") + mav = mavutil.mavlink_connection(MAVLINK_PYMAV) + mav.wait_heartbeat() + print("pymavlink connecte !") + +def send_orbit_cmd(radius, speed, lat, lon, abs_alt): + mav.mav.command_long_send( + mav.target_system, mav.target_component, + 34, 0, radius, speed, 0, float('nan'), lat, lon, abs_alt + ) + +async def refresh_position(drone): + async for pos in drone.telemetry.position(): + current_pos["lat"] = pos.latitude_deg + current_pos["lon"] = pos.longitude_deg + current_pos["alt"] = pos.relative_altitude_m + current_pos["abs_alt"] = pos.absolute_altitude_m + break + +async def monitor_armed(drone): + global drone_armed + async for is_armed in drone.telemetry.armed(): + drone_armed = is_armed + +async def interpret_command(text): + response = client.models.generate_content( + model=GEMINI_MODEL, + contents=text, + config=types.GenerateContentConfig( + system_instruction=SYSTEM_PROMPT, + temperature=0.1, + ) + ) + raw = response.text.strip() + if raw.startswith("```"): + raw = raw.split("```")[1] + if raw.startswith("json"): + raw = raw[4:] + return json.loads(raw.strip()) + +async def execute_command(drone, cmd): + global drone_armed + action = cmd.get("action") + alt = float(cmd.get("altitude", 15)) + msg = cmd.get("message", "OK") + + if action == "status": + await refresh_position(drone) + async for bat in drone.telemetry.battery(): + raw = bat.remaining_percent + pct = abs(raw) * 100 if abs(raw) <= 1 else abs(raw) + break + async for fm in drone.telemetry.flight_mode(): + mode = str(fm) + break + return (f"\nPosition : {current_pos['lat']:.6f}, {current_pos['lon']:.6f}\n" + f"Altitude : {current_pos['alt']:.1f}m (AGL)\n" + f"Abs alt : {current_pos['abs_alt']:.1f}m\n" + f"Batterie : {pct:.0f}%\n" + f"Mode : {mode}\n" + f"Arme : {drone_armed}") + + elif action == "takeoff": + if drone_armed: + return "Deja en vol" + await drone.action.set_takeoff_altitude(alt) + await drone.action.arm() + await drone.action.takeoff() + await asyncio.sleep(8) + await refresh_position(drone) + return f"OK {msg} — altitude {current_pos['alt']:.1f}m" + + elif action == "goto": + if current_pos["alt"] < 2.0: + await drone.action.set_takeoff_altitude(alt) + await drone.action.arm() + await drone.action.takeoff() + await asyncio.sleep(8) + lat = float(cmd.get("lat", current_pos["lat"])) + lon = float(cmd.get("lon", current_pos["lon"])) + items = [MissionItem( + lat, lon, alt, 5, True, + float('nan'), float('nan'), + MissionItem.CameraAction.NONE, + float('nan'), float('nan'), + float('nan'), float('nan'), + float('nan'), + MissionItem.VehicleAction.NONE + )] + await drone.mission.upload_mission(MissionPlan(items)) + await drone.mission.start_mission() + return f"OK {msg} → ({lat:.5f}, {lon:.5f}) alt {alt}m" + + elif action == "orbit": + if current_pos["alt"] < 2.0: + await drone.action.set_takeoff_altitude(alt) + await drone.action.arm() + await drone.action.takeoff() + await asyncio.sleep(8) + await refresh_position(drone) + await refresh_position(drone) + radius = float(cmd.get("radius", 10)) + speed = float(cmd.get("speed", 2)) + send_orbit_cmd(radius, speed, + current_pos["lat"], + current_pos["lon"], + current_pos["abs_alt"]) + ack = mav.recv_match(type='COMMAND_ACK', blocking=True, timeout=2) + if ack is None or ack.result != 0: + steps = 16 + items = [] + lat0 = current_pos["lat"] + lon0 = current_pos["lon"] + for i in range(steps + 1): + angle = 2 * math.pi * i / steps + dlat = (radius / 111320) * math.cos(angle) + dlon = (radius / (111320 * math.cos(math.radians(lat0)))) * math.sin(angle) + items.append(MissionItem( + lat0 + dlat, lon0 + dlon, alt, speed, True, + float('nan'), float('nan'), + MissionItem.CameraAction.NONE, + float('nan'), float('nan'), + float('nan'), float('nan'), + float('nan'), + MissionItem.VehicleAction.NONE + )) + await drone.mission.upload_mission(MissionPlan(items)) + await drone.mission.start_mission() + return (f"OK {msg} — orbite waypoints {radius}m " + f"a {speed}m/s ({steps} points)\n" + f"Centre : ({lat0:.6f}, {lon0:.6f})") + return f"OK {msg} — orbite native {radius}m a {speed}m/s" + + elif action == "hover": + await drone.action.hold() + return "OK " + msg + + elif action == "land": + await drone.action.land() + return "OK " + msg + + elif action == "rtl": + await drone.action.return_to_launch() + return "OK " + msg + + return "Action inconnue : " + action + +async def main(): + init_pymavlink() + drone = System() + await drone.connect(system_address=MAVLINK_ADDRESS) + print("Connexion MAVSDK...") + async for state in drone.core.connection_state(): + if state.is_connected: + print("SITL connecte !") + async for is_armed in drone.telemetry.armed(): + drone_armed = is_armed + print(f"Etat initial : {'arme' if drone_armed else 'desarme'}") + break + asyncio.ensure_future(monitor_armed(drone)) + break + + print("\nWireClaw pret") + print("Commandes : takeoff, goto, orbit, status, hover, rtl, land\n") + + while True: + try: + text = input(">>> ").strip() + if text.lower() in ("quit", "exit", "q"): + break + if not text: + continue + cmd = await interpret_command(text) + print(json.dumps(cmd, ensure_ascii=False)) + result = await execute_command(drone, cmd) + print(result + "\n") + except KeyboardInterrupt: + break + except Exception as e: + print(f"Erreur : {e}\n") + +if __name__ == "__main__": + asyncio.run(main()) \ No newline at end of file