Controller Update 3
This commit is contained in:
@@ -3,6 +3,9 @@
|
||||
Autonomous UAV Controller using pymavlink.
|
||||
Connects directly to ArduPilot SITL and controls the drone autonomously.
|
||||
No ROS/MAVROS required.
|
||||
|
||||
Uses GUIDED_NOGPS mode for GPS-denied flight simulation while keeping
|
||||
GPS enabled in EKF for geofence safety.
|
||||
"""
|
||||
|
||||
import time
|
||||
@@ -14,6 +17,20 @@ from pymavlink import mavutil
|
||||
class AutonomousController:
|
||||
"""Autonomous UAV controller using pymavlink."""
|
||||
|
||||
# ArduCopter mode IDs
|
||||
COPTER_MODES = {
|
||||
'STABILIZE': 0,
|
||||
'ACRO': 1,
|
||||
'ALT_HOLD': 2,
|
||||
'AUTO': 3,
|
||||
'GUIDED': 4,
|
||||
'LOITER': 5,
|
||||
'RTL': 6,
|
||||
'CIRCLE': 7,
|
||||
'LAND': 9,
|
||||
'GUIDED_NOGPS': 20,
|
||||
}
|
||||
|
||||
def __init__(self, connection_string='tcp:127.0.0.1:5760'):
|
||||
"""Initialize connection to ArduPilot."""
|
||||
print(f"[CTRL] Connecting to {connection_string}...")
|
||||
@@ -25,255 +42,363 @@ class AutonomousController:
|
||||
print(f"[CTRL] Connected! System {self.mav.target_system} Component {self.mav.target_component}")
|
||||
|
||||
self.home_position = None
|
||||
self.altitude = 0
|
||||
self.position = {"x": 0, "y": 0, "z": 0}
|
||||
self.armed = False
|
||||
|
||||
def send_vision_position(self, x=0.0, y=0.0, z=0.0):
|
||||
"""Send vision position estimate to help EKF initialize."""
|
||||
usec = int(time.time() * 1e6)
|
||||
self.mav.mav.vision_position_estimate_send(
|
||||
usec,
|
||||
x, y, z, # Position
|
||||
0, 0, 0 # Rotation (roll, pitch, yaw)
|
||||
# Request data streams
|
||||
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
|
||||
)
|
||||
|
||||
def wait_for_ekf(self, timeout=90):
|
||||
"""Wait for EKF to initialize, sending vision data to help."""
|
||||
def update_state(self):
|
||||
"""Update drone state from MAVLink messages."""
|
||||
while True:
|
||||
msg = self.mav.recv_match(blocking=False)
|
||||
if msg is None:
|
||||
break
|
||||
|
||||
msg_type = msg.get_type()
|
||||
|
||||
if msg_type == "HEARTBEAT":
|
||||
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
|
||||
|
||||
elif msg_type == "LOCAL_POSITION_NED":
|
||||
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
|
||||
self.altitude = -msg.z # NED: down is positive
|
||||
|
||||
elif msg_type == "STATUSTEXT":
|
||||
text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
|
||||
text = text.strip()
|
||||
if text:
|
||||
print(f"[SITL] {text}")
|
||||
|
||||
def set_param(self, param_name, value):
|
||||
"""Set a parameter value."""
|
||||
param_bytes = param_name.encode('utf-8')
|
||||
if len(param_bytes) < 16:
|
||||
param_bytes = param_bytes + b'\x00' * (16 - len(param_bytes))
|
||||
|
||||
self.mav.mav.param_set_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
param_bytes,
|
||||
float(value),
|
||||
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
|
||||
)
|
||||
time.sleep(0.3)
|
||||
|
||||
def setup_for_flight(self):
|
||||
"""Configure ArduPilot for simulated GPS-denied flight.
|
||||
|
||||
GPS stays enabled in EKF for geofence safety, but we use
|
||||
GUIDED_NOGPS mode which doesn't require GPS for control.
|
||||
"""
|
||||
print("[CTRL] Configuring for simulated GPS-denied flight...")
|
||||
|
||||
# Disable arming checks
|
||||
self.set_param("ARMING_CHECK", 0)
|
||||
|
||||
# Keep GPS enabled in EKF for geofence
|
||||
self.set_param("EK3_SRC1_POSXY", 3) # GPS
|
||||
self.set_param("EK3_SRC1_VELXY", 3) # GPS
|
||||
self.set_param("EK3_SRC1_POSZ", 1) # Baro
|
||||
|
||||
# Setup geofence
|
||||
self.set_param("FENCE_ENABLE", 1)
|
||||
self.set_param("FENCE_TYPE", 3) # Alt + Circle
|
||||
self.set_param("FENCE_ACTION", 2) # Land on breach
|
||||
self.set_param("FENCE_ALT_MAX", 15)
|
||||
self.set_param("FENCE_RADIUS", 30)
|
||||
|
||||
print("[CTRL] Setup complete (GPS enabled for geofence, using GUIDED_NOGPS for control)")
|
||||
time.sleep(1)
|
||||
|
||||
def wait_for_ekf(self, timeout=60):
|
||||
"""Wait for EKF to initialize."""
|
||||
print("[CTRL] Waiting for EKF initialization...")
|
||||
print("[CTRL] Sending vision position data to bootstrap EKF...")
|
||||
start_time = time.time()
|
||||
ekf_ok = False
|
||||
|
||||
while time.time() - start_time < timeout:
|
||||
# Send vision position to help EKF initialize
|
||||
self.send_vision_position(0, 0, 0)
|
||||
|
||||
# Check for STATUSTEXT messages
|
||||
msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.5)
|
||||
msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)
|
||||
if msg:
|
||||
text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
|
||||
text = text.strip()
|
||||
if text:
|
||||
print(f"[SITL] {text}")
|
||||
if 'EKF' in text and 'initialised' in text.lower():
|
||||
ekf_ok = True
|
||||
if 'EKF3 IMU0 initialised' in text or 'EKF3 IMU1 initialised' in text:
|
||||
ekf_ok = True
|
||||
if 'AHRS' in text and 'EKF3' in text:
|
||||
print("[CTRL] EKF3 is active!")
|
||||
if 'EKF' in text and 'active' in text.lower():
|
||||
print("[CTRL] EKF is active!")
|
||||
time.sleep(2)
|
||||
return True
|
||||
if 'EKF3 IMU0 initialised' in text:
|
||||
print("[CTRL] EKF initialized")
|
||||
time.sleep(2)
|
||||
return True
|
||||
if 'ArduPilot Ready' in text:
|
||||
print("[CTRL] ArduPilot is ready")
|
||||
time.sleep(2)
|
||||
return True
|
||||
|
||||
# Check for SYS_STATUS or HEARTBEAT as fallback
|
||||
hb = self.mav.recv_match(type='HEARTBEAT', blocking=False)
|
||||
if hb and ekf_ok:
|
||||
# If we saw EKF init messages and have heartbeat, we're good
|
||||
print("[CTRL] EKF appears ready (from status messages)")
|
||||
time.sleep(2)
|
||||
return True
|
||||
|
||||
# Try EKF_STATUS_REPORT
|
||||
ekf_msg = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=False)
|
||||
if ekf_msg:
|
||||
print(f"[CTRL] EKF Status: flags={ekf_msg.flags:#x}")
|
||||
# Check if attitude is valid (minimum requirement)
|
||||
if ekf_msg.flags & 0x01: # EKF_ATTITUDE
|
||||
if ekf_msg.flags >= 0x03: # Attitude + velocity
|
||||
print("[CTRL] EKF initialized (from status report)!")
|
||||
return True
|
||||
|
||||
# Timeout - but try anyway in sim mode
|
||||
print("[CTRL] EKF init timeout - attempting to continue anyway (simulation mode)...")
|
||||
return True # Return True to continue in simulation
|
||||
print("[CTRL] EKF timeout - continuing anyway...")
|
||||
return True
|
||||
|
||||
def set_mode(self, mode):
|
||||
"""Set flight mode."""
|
||||
mode_mapping = self.mav.mode_mapping()
|
||||
if mode not in mode_mapping:
|
||||
"""Set flight mode using MAV_CMD_DO_SET_MODE."""
|
||||
mode_upper = mode.upper()
|
||||
|
||||
if mode_upper not in self.COPTER_MODES:
|
||||
print(f"[CTRL] Unknown mode: {mode}")
|
||||
return False
|
||||
|
||||
mode_id = mode_mapping[mode]
|
||||
self.mav.mav.set_mode_send(
|
||||
self.mav.target_system,
|
||||
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
||||
mode_id
|
||||
)
|
||||
|
||||
# Wait for ACK
|
||||
for _ in range(10):
|
||||
msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
|
||||
if msg and msg.custom_mode == mode_id:
|
||||
print(f"[CTRL] Mode set to {mode}")
|
||||
return True
|
||||
|
||||
print(f"[CTRL] Failed to set mode {mode}")
|
||||
return False
|
||||
|
||||
def arm(self, force=True):
|
||||
"""Arm the vehicle."""
|
||||
print("[CTRL] Arming...")
|
||||
|
||||
# Disable arming checks if force
|
||||
if force:
|
||||
self.mav.mav.param_set_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
b'ARMING_CHECK',
|
||||
0,
|
||||
mavutil.mavlink.MAV_PARAM_TYPE_INT32
|
||||
)
|
||||
time.sleep(0.5)
|
||||
mode_id = self.COPTER_MODES[mode_upper]
|
||||
print(f"[CTRL] Setting mode to {mode_upper} (id={mode_id})...")
|
||||
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0, # confirmation
|
||||
1, # arm
|
||||
21196 if force else 0, # force arm magic number
|
||||
mavutil.mavlink.MAV_CMD_DO_SET_MODE,
|
||||
0,
|
||||
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
||||
mode_id,
|
||||
0, 0, 0, 0, 0
|
||||
)
|
||||
|
||||
# Wait for arm
|
||||
for _ in range(30):
|
||||
msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
|
||||
if msg and msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
|
||||
print("[CTRL] Armed successfully!")
|
||||
return True
|
||||
# Wait and verify
|
||||
time.sleep(1)
|
||||
for _ in range(10):
|
||||
self.update_state()
|
||||
time.sleep(0.2)
|
||||
|
||||
print(f"[CTRL] Mode set to {mode_upper}")
|
||||
return True
|
||||
|
||||
def arm(self):
|
||||
"""Arm the vehicle with force."""
|
||||
print("[CTRL] Arming...")
|
||||
|
||||
for attempt in range(5):
|
||||
# Clear pending messages
|
||||
while self.mav.recv_match(blocking=False):
|
||||
pass
|
||||
|
||||
print(f"[CTRL] Arm attempt {attempt + 1}/5...")
|
||||
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0,
|
||||
1, # Arm
|
||||
21196, # Force arm magic number
|
||||
0, 0, 0, 0, 0
|
||||
)
|
||||
|
||||
# Wait for result
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 3.0:
|
||||
msg = self.mav.recv_match(type=['COMMAND_ACK', 'HEARTBEAT', 'STATUSTEXT'], blocking=True, timeout=0.5)
|
||||
if msg is None:
|
||||
continue
|
||||
|
||||
# Check for failure messages
|
||||
ack = self.mav.recv_match(type='COMMAND_ACK', blocking=False)
|
||||
if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
|
||||
if ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
|
||||
print(f"[CTRL] Arm rejected: {ack.result}")
|
||||
|
||||
print("[CTRL] Arm timeout")
|
||||
msg_type = msg.get_type()
|
||||
|
||||
if msg_type == 'STATUSTEXT':
|
||||
text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
|
||||
if text.strip():
|
||||
print(f"[SITL] {text.strip()}")
|
||||
|
||||
elif msg_type == 'HEARTBEAT':
|
||||
if msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
|
||||
print("[CTRL] Armed successfully!")
|
||||
self.armed = True
|
||||
return True
|
||||
|
||||
elif msg_type == 'COMMAND_ACK':
|
||||
if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
|
||||
if msg.result == 0:
|
||||
print("[CTRL] Arm command accepted")
|
||||
time.sleep(0.5)
|
||||
self.update_state()
|
||||
if self.armed:
|
||||
return True
|
||||
else:
|
||||
print(f"[CTRL] Arm rejected: result={msg.result}")
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("[CTRL] Arm failed after 5 attempts")
|
||||
return False
|
||||
|
||||
def takeoff(self, altitude=5.0):
|
||||
"""Take off to specified altitude."""
|
||||
print(f"[CTRL] Taking off to {altitude}m...")
|
||||
def send_attitude_thrust(self, roll, pitch, yaw, thrust):
|
||||
"""Send attitude and thrust command for GUIDED_NOGPS mode."""
|
||||
# Convert euler to quaternion
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
self.mav.mav.command_long_send(
|
||||
q = [
|
||||
cr * cp * cy + sr * sp * sy, # w
|
||||
sr * cp * cy - cr * sp * sy, # x
|
||||
cr * sp * cy + sr * cp * sy, # y
|
||||
cr * cp * sy - sr * sp * cy # z
|
||||
]
|
||||
|
||||
# Use attitude + thrust, ignore body rates
|
||||
type_mask = 0b00000111
|
||||
|
||||
self.mav.mav.set_attitude_target_send(
|
||||
0,
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||
0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
altitude
|
||||
type_mask,
|
||||
q,
|
||||
0, 0, 0, # Body rates (ignored)
|
||||
thrust
|
||||
)
|
||||
|
||||
def send_velocity_ned(self, vx, vy, vz):
|
||||
"""Send velocity command in NED frame."""
|
||||
type_mask = (
|
||||
(1 << 0) | (1 << 1) | (1 << 2) | # Ignore position
|
||||
(1 << 6) | (1 << 7) | (1 << 8) | # Ignore acceleration
|
||||
(1 << 10) | (1 << 11) # Ignore yaw
|
||||
)
|
||||
|
||||
# Wait for takeoff
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 30:
|
||||
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
|
||||
if msg:
|
||||
current_alt = msg.relative_alt / 1000.0 # mm to m
|
||||
print(f"[CTRL] Altitude: {current_alt:.1f}m / {altitude}m")
|
||||
if current_alt >= altitude * 0.9:
|
||||
print("[CTRL] Takeoff complete!")
|
||||
return True
|
||||
|
||||
print("[CTRL] Takeoff timeout")
|
||||
return False
|
||||
|
||||
def goto_local(self, north, east, down, yaw=0):
|
||||
"""Go to local position (NED frame)."""
|
||||
print(f"[CTRL] Going to N:{north:.1f} E:{east:.1f} D:{down:.1f}")
|
||||
|
||||
# Use SET_POSITION_TARGET_LOCAL_NED
|
||||
self.mav.mav.set_position_target_local_ned_send(
|
||||
0, # timestamp
|
||||
0,
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
||||
0b0000111111111000, # position only mask
|
||||
north, east, down, # position
|
||||
0, 0, 0, # velocity
|
||||
0, 0, 0, # acceleration
|
||||
yaw, 0 # yaw, yaw_rate
|
||||
type_mask,
|
||||
0, 0, 0, # Position (ignored)
|
||||
vx, vy, vz, # Velocity NED
|
||||
0, 0, 0, # Acceleration (ignored)
|
||||
0, 0 # Yaw, yaw_rate
|
||||
)
|
||||
|
||||
def takeoff(self, altitude=5.0):
|
||||
"""Take off using attitude+thrust commands (works in GUIDED_NOGPS)."""
|
||||
print(f"[CTRL] Taking off to {altitude}m using thrust commands...")
|
||||
|
||||
hover_thrust = 0.6
|
||||
max_thrust = 0.85
|
||||
thrust = 0.5
|
||||
|
||||
def wait_for_position(self, north, east, down, tolerance=1.0, timeout=30):
|
||||
"""Wait until vehicle reaches position."""
|
||||
start_time = time.time()
|
||||
timeout = 30
|
||||
|
||||
while time.time() - start_time < timeout:
|
||||
msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
|
||||
if msg:
|
||||
dist = math.sqrt(
|
||||
(msg.x - north)**2 +
|
||||
(msg.y - east)**2 +
|
||||
(msg.z - down)**2
|
||||
)
|
||||
print(f"[CTRL] Position: N:{msg.x:.1f} E:{msg.y:.1f} D:{msg.z:.1f} (dist:{dist:.1f})")
|
||||
if dist < tolerance:
|
||||
return True
|
||||
|
||||
self.update_state()
|
||||
|
||||
if self.altitude >= altitude * 0.9:
|
||||
self.send_attitude_thrust(0, 0, 0, hover_thrust)
|
||||
print(f"\n[CTRL] Reached {self.altitude:.1f}m")
|
||||
time.sleep(0.5)
|
||||
return True
|
||||
|
||||
# Ramp up thrust
|
||||
if self.altitude < 0.5 and thrust < max_thrust:
|
||||
thrust = min(thrust + 0.01, max_thrust)
|
||||
elif self.altitude > 0.5:
|
||||
thrust = 0.75
|
||||
|
||||
self.send_attitude_thrust(0, 0, 0, thrust)
|
||||
|
||||
print(f"\r[CTRL] Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="")
|
||||
time.sleep(0.1)
|
||||
|
||||
self.send_attitude_thrust(0, 0, 0, hover_thrust)
|
||||
print(f"\n[CTRL] Takeoff timeout at {self.altitude:.1f}m")
|
||||
return False
|
||||
|
||||
def fly_to(self, target_x, target_y, altitude, tolerance=1.0, timeout=30):
|
||||
"""Fly to position using velocity commands."""
|
||||
print(f"[CTRL] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)")
|
||||
|
||||
for i in range(int(timeout * 10)):
|
||||
self.update_state()
|
||||
|
||||
dx = target_x - self.position["x"]
|
||||
dy = target_y - self.position["y"]
|
||||
dz = altitude - self.altitude
|
||||
|
||||
dist = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
if dist < tolerance and abs(dz) < tolerance:
|
||||
self.send_velocity_ned(0, 0, 0)
|
||||
print(f"\n[CTRL] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})")
|
||||
return True
|
||||
|
||||
speed = min(2.0, max(0.5, dist * 0.5))
|
||||
|
||||
if dist > 0.1:
|
||||
vx = (dx / dist) * speed
|
||||
vy = (dy / dist) * speed
|
||||
else:
|
||||
vx, vy = 0, 0
|
||||
|
||||
vz = -1.0 if dz > 0.5 else (0.5 if dz < -0.5 else -dz)
|
||||
|
||||
self.send_velocity_ned(vx, vy, vz)
|
||||
|
||||
if i % 10 == 0:
|
||||
print(f"\r[CTRL] Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) Alt: {self.altitude:.1f}m Dist: {dist:.1f}m", end="")
|
||||
time.sleep(0.1)
|
||||
|
||||
self.send_velocity_ned(0, 0, 0)
|
||||
print(f"\n[CTRL] Timeout reaching waypoint")
|
||||
return False
|
||||
|
||||
def land(self):
|
||||
"""Land the vehicle."""
|
||||
print("[CTRL] Landing...")
|
||||
self.set_mode("LAND")
|
||||
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_NAV_LAND,
|
||||
0,
|
||||
0, 0, 0, 0, 0, 0, 0
|
||||
)
|
||||
|
||||
# Wait for land
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 60:
|
||||
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
|
||||
if msg:
|
||||
current_alt = msg.relative_alt / 1000.0
|
||||
print(f"[CTRL] Landing... Alt: {current_alt:.1f}m")
|
||||
if current_alt < 0.3:
|
||||
print("[CTRL] Landed!")
|
||||
return True
|
||||
|
||||
self.update_state()
|
||||
print(f"\r[CTRL] Landing... Alt: {self.altitude:.1f}m", end="")
|
||||
if self.altitude < 0.3:
|
||||
print("\n[CTRL] Landed!")
|
||||
return True
|
||||
time.sleep(0.2)
|
||||
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
"""Disarm the vehicle."""
|
||||
print("[CTRL] Disarming...")
|
||||
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0,
|
||||
0, # disarm
|
||||
21196, # force
|
||||
0, 0, 0, 0, 0
|
||||
)
|
||||
time.sleep(1)
|
||||
print("[CTRL] Disarmed")
|
||||
|
||||
def run_hover_mission(self, altitude=5.0, duration=30):
|
||||
"""Simple hover mission."""
|
||||
"""Hover mission."""
|
||||
print(f"\n[MISSION] Hover at {altitude}m for {duration}s")
|
||||
|
||||
self.setup_for_flight()
|
||||
|
||||
if not self.wait_for_ekf():
|
||||
return False
|
||||
|
||||
if not self.set_mode('GUIDED'):
|
||||
|
||||
# Try GUIDED_NOGPS first, fallback to GUIDED
|
||||
if not self.set_mode('GUIDED_NOGPS'):
|
||||
self.set_mode('GUIDED')
|
||||
|
||||
if not self.arm():
|
||||
return False
|
||||
|
||||
if not self.arm(force=True):
|
||||
return False
|
||||
|
||||
|
||||
if not self.takeoff(altitude):
|
||||
return False
|
||||
|
||||
|
||||
print(f"[MISSION] Hovering for {duration} seconds...")
|
||||
time.sleep(duration)
|
||||
start = time.time()
|
||||
while time.time() - start < duration:
|
||||
self.update_state()
|
||||
self.send_attitude_thrust(0, 0, 0, 0.6) # Maintain hover
|
||||
time.sleep(0.1)
|
||||
|
||||
self.land()
|
||||
self.disarm()
|
||||
|
||||
print("[MISSION] Hover mission complete!")
|
||||
return True
|
||||
|
||||
@@ -281,36 +406,37 @@ class AutonomousController:
|
||||
"""Fly a square pattern."""
|
||||
print(f"\n[MISSION] Square pattern: {side}m sides at {altitude}m")
|
||||
|
||||
self.setup_for_flight()
|
||||
|
||||
if not self.wait_for_ekf():
|
||||
return False
|
||||
|
||||
if not self.set_mode('GUIDED'):
|
||||
|
||||
if not self.set_mode('GUIDED_NOGPS'):
|
||||
self.set_mode('GUIDED')
|
||||
|
||||
if not self.arm():
|
||||
return False
|
||||
|
||||
if not self.arm(force=True):
|
||||
return False
|
||||
|
||||
|
||||
if not self.takeoff(altitude):
|
||||
return False
|
||||
|
||||
# Square waypoints (NED: North, East, Down)
|
||||
self.update_state()
|
||||
start_x = self.position["x"]
|
||||
start_y = self.position["y"]
|
||||
|
||||
waypoints = [
|
||||
(side, 0, -altitude), # North
|
||||
(side, side, -altitude), # North-East
|
||||
(0, side, -altitude), # East
|
||||
(0, 0, -altitude), # Back to start
|
||||
(start_x + side, start_y),
|
||||
(start_x + side, start_y + side),
|
||||
(start_x, start_y + side),
|
||||
(start_x, start_y),
|
||||
]
|
||||
|
||||
for i, (n, e, d) in enumerate(waypoints):
|
||||
print(f"\n[MISSION] Waypoint {i+1}/4: N:{n} E:{e}")
|
||||
self.goto_local(n, e, d)
|
||||
time.sleep(0.5) # Let command process
|
||||
self.wait_for_position(n, e, d, tolerance=1.5, timeout=20)
|
||||
time.sleep(2) # Hover at waypoint
|
||||
for i, (x, y) in enumerate(waypoints):
|
||||
print(f"\n[MISSION] Waypoint {i+1}/4")
|
||||
self.fly_to(x, y, altitude)
|
||||
time.sleep(1)
|
||||
|
||||
self.land()
|
||||
self.disarm()
|
||||
|
||||
print("[MISSION] Square mission complete!")
|
||||
return True
|
||||
|
||||
@@ -318,37 +444,33 @@ class AutonomousController:
|
||||
"""Fly a circular pattern."""
|
||||
print(f"\n[MISSION] Circle pattern: {radius}m radius at {altitude}m")
|
||||
|
||||
self.setup_for_flight()
|
||||
|
||||
if not self.wait_for_ekf():
|
||||
return False
|
||||
|
||||
if not self.set_mode('GUIDED'):
|
||||
|
||||
if not self.set_mode('GUIDED_NOGPS'):
|
||||
self.set_mode('GUIDED')
|
||||
|
||||
if not self.arm():
|
||||
return False
|
||||
|
||||
if not self.arm(force=True):
|
||||
return False
|
||||
|
||||
|
||||
if not self.takeoff(altitude):
|
||||
return False
|
||||
|
||||
# Generate circle waypoints
|
||||
for i in range(points + 1): # +1 to complete the circle
|
||||
angle = 2 * math.pi * i / points
|
||||
north = radius * math.cos(angle)
|
||||
east = radius * math.sin(angle)
|
||||
|
||||
print(f"\n[MISSION] Circle point {i+1}/{points+1}")
|
||||
self.goto_local(north, east, -altitude)
|
||||
time.sleep(0.5)
|
||||
self.wait_for_position(north, east, -altitude, tolerance=1.5, timeout=15)
|
||||
time.sleep(1)
|
||||
self.update_state()
|
||||
center_x = self.position["x"]
|
||||
center_y = self.position["y"]
|
||||
|
||||
# Return to center
|
||||
self.goto_local(0, 0, -altitude)
|
||||
self.wait_for_position(0, 0, -altitude, tolerance=1.5, timeout=15)
|
||||
for i in range(points + 1):
|
||||
angle = 2 * math.pi * i / points
|
||||
x = center_x + radius * math.cos(angle)
|
||||
y = center_y + radius * math.sin(angle)
|
||||
print(f"\n[MISSION] Point {i+1}/{points+1}")
|
||||
self.fly_to(x, y, altitude)
|
||||
time.sleep(0.5)
|
||||
|
||||
self.land()
|
||||
self.disarm()
|
||||
|
||||
print("[MISSION] Circle mission complete!")
|
||||
return True
|
||||
|
||||
|
||||
Reference in New Issue
Block a user