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

drone controller update

Browse Source
This commit is contained in:
default
2026-01-09 20:10:09 +00:00
parent fcceabd47e
commit d4c8bb572b
1 changed files with 39 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/drone_controller.py
View File

@@ -46,24 +46,47 @@ class DroneController:
try:
self.mav = mavutil.mavlink_connection(self.connection_string)
msg = self.mav.wait_heartbeat(timeout=timeout)
print(f"[OK] Connected to system {self.mav.target_system}")
# Wait for heartbeat with retries
print("[INFO] Waiting for heartbeat...")
msg = None
for attempt in range(3):
msg = self.mav.wait_heartbeat(timeout=timeout)
if msg and self.mav.target_system > 0:
break
print(f"[WARN] Heartbeat attempt {attempt + 1}/3 - retrying...")
if not msg or self.mav.target_system == 0:
print("[ERROR] Failed to receive valid heartbeat")
print("[TIP] Make sure SITL is running:")
print(" sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console")
return False
print(f"[OK] Connected to system {self.mav.target_system}, component {self.mav.target_component}")
# Detect vehicle type from heartbeat
if msg:
mav_type = msg.type
if mav_type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
self.vehicle_type = "copter"
print(f"[INFO] Vehicle: Quadrotor (ArduCopter)")
elif mav_type == mavutil.mavlink.MAV_TYPE_HELICOPTER:
self.vehicle_type = "copter"
print(f"[INFO] Vehicle: Helicopter")
elif mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
self.vehicle_type = "plane"
print(f"[INFO] Vehicle: Fixed Wing (ArduPlane)")
else:
self.vehicle_type = "copter" # Default to copter
print(f"[INFO] Vehicle type: {mav_type}")
mav_type = msg.type
if mav_type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
self.vehicle_type = "copter"
print(f"[INFO] Vehicle: Quadrotor (ArduCopter)")
elif mav_type == mavutil.mavlink.MAV_TYPE_HELICOPTER:
self.vehicle_type = "copter"
print(f"[INFO] Vehicle: Helicopter")
elif mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
self.vehicle_type = "plane"
print(f"[INFO] Vehicle: Fixed Wing (ArduPlane)")
else:
self.vehicle_type = "copter" # Default to copter
print(f"[INFO] Vehicle type ID: {mav_type}")
# Request data streams for telemetry
self.mav.mav.request_data_stream_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL,
4, # 4 Hz
1 # Start
)
return True
except Exception as e: