Initial commit — WireClaw SITL fonctionnel

.env.example

@@ -0,0 +1,3 @@
+GEMINI_API_KEY=your_gemini_api_key_here
+TELEGRAM_TOKEN=
+TELEGRAM_CHAT_ID=

.gitignore

@@ -0,0 +1,5 @@
+.env
+__pycache__/
+*.pyc
+mav.tlog
+mav.parm

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

sitl_params.parm

@@ -0,0 +1,4 @@
+BATT_MONITOR 0
+SIM_BATT_VOLTAGE 14.0
+BATT_LOW_VOLT 0
+BATT_CRT_VOLT 0

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

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": <float metres, defaut 15>,
+  "lat": <float, pour goto>,
+  "lon": <float, pour goto>,
+  "radius": <float metres, pour orbit, defaut 10>,
+  "speed": <float m/s, pour orbit, defaut 2>,
+  "message": "<confirmation courte>"
+}}
+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())