SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Remove ROS2 dependency, enhance arming with retries, and improve takeoff sequence with better altitude monitoring and arming failure handling.

Browse Source
This commit is contained in:
default
2026-01-07 20:29:11 +00:00
parent 1eb0d23fb9
commit 760293d896
1 changed files with 46 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
scripts/run_ardupilot.py
View File

@@ -31,14 +31,6 @@ except ImportError:
print("Run: pip install pymavlink") print("Run: pip install pymavlink")
sys.exit(1) sys.exit(1)
try:
import rclpy
from rclpy.node import Node
ROS2_AVAILABLE = True
except ImportError:
ROS2_AVAILABLE = False
print("[WARN] ROS 2 (rclpy) not available - using standalone controller")
from config import ARDUPILOT, CONTROLLER, MAVLINK from config import ARDUPILOT, CONTROLLER, MAVLINK
@@ -83,10 +75,11 @@ class ArduPilotInterface:
if msg_type == "HEARTBEAT": if msg_type == "HEARTBEAT":
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
# Get mode name # Get mode name using mavutil helper
if hasattr(mavutil.mavlink, 'enums'): try:
mode_map = mavutil.mavlink.enums.get('COPTER_MODE', {}) self.mode = mavutil.mode_string_v10(msg)
self.mode = mode_map.get(msg.custom_mode, {}).get('name', str(msg.custom_mode)) except Exception:
self.mode = str(msg.custom_mode)
elif msg_type == "LOCAL_POSITION_NED": elif msg_type == "LOCAL_POSITION_NED":
self.position = {"x": msg.x, "y": msg.y, "z": msg.z} self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
@@ -135,6 +128,9 @@ class ArduPilotInterface:
def arm(self, force=True): def arm(self, force=True):
"""Arm motors.""" """Arm motors."""
print("[INFO] Arming...")
for attempt in range(3):
if force: if force:
# Force arm (bypass checks) # Force arm (bypass checks)
self.mav.mav.command_long_send( self.mav.mav.command_long_send(
@@ -146,11 +142,18 @@ class ArduPilotInterface:
else: else:
self.mav.arducopter_arm() self.mav.arducopter_arm()
# Wait and check if armed
time.sleep(1) time.sleep(1)
self.update_telemetry() self.update_telemetry()
if self.armed: if self.armed:
print("[OK] Armed") print("[OK] Armed")
return self.armed return True
else:
print(f"[WARN] Arm attempt {attempt + 1}/3 failed, retrying...")
print("[ERROR] Failed to arm after 3 attempts")
return False
def disarm(self): def disarm(self):
"""Disarm motors.""" """Disarm motors."""
@@ -279,22 +282,30 @@ def main():
# Set GUIDED mode and arm # Set GUIDED mode and arm
print("[INFO] Setting up for takeoff...") print("[INFO] Setting up for takeoff...")
ardupilot.set_mode("GUIDED") ardupilot.set_mode("GUIDED")
time.sleep(1) time.sleep(2) # Give mode time to change
ardupilot.arm(force=True) # Update telemetry to get current mode
time.sleep(1) ardupilot.update_telemetry()
print(f"[INFO] Current mode: {ardupilot.mode}")
if ardupilot.armed: if not ardupilot.arm(force=True):
print("[ERROR] Could not arm - continuing in manual mode")
print("[INFO] Use MAVProxy to arm: arm throttle force")
else:
ardupilot.takeoff(args.takeoff_alt) ardupilot.takeoff(args.takeoff_alt)
print(f"[INFO] Taking off to {args.takeoff_alt}m...") print(f"[INFO] Taking off to {args.takeoff_alt}m...")
# Wait for takeoff # Wait for takeoff (up to 15 seconds)
for _ in range(50): # 5 seconds for i in range(150):
telemetry = ardupilot.get_telemetry() telemetry = ardupilot.get_telemetry()
if telemetry["altimeter"]["altitude"] > args.takeoff_alt * 0.9: alt = telemetry["altimeter"]["altitude"]
print("[OK] Reached target altitude") if alt > args.takeoff_alt * 0.9:
print(f"\n[OK] Reached target altitude ({alt:.1f}m)")
break break
if i % 10 == 0:
print(f"\r[INFO] Climbing... {alt:.1f}m / {args.takeoff_alt}m", end="")
time.sleep(0.1) time.sleep(0.1)
print("")
print("\n[INFO] Starting controller loop...") print("\n[INFO] Starting controller loop...")
print("[INFO] Press Ctrl+C to stop\n") print("[INFO] Press Ctrl+C to stop\n")