Add DroneCommander module with MAVLink move-to and face-target commands

[KrishP147]

Summary

• Implements DroneCommander class for MAVLink flight control
• Sends SET_POSITION_TARGET_LOCAL_NED to move toward tracked targets
• Sends MAV_CMD_CONDITION_YAW to rotate and face targets
• Handles coordinate conversion: camera frame -> body frame -> NED

Test plan

• Test with SITL simulator on tcp:localhost:5762
• Verify yaw commands rotate correct direction
• Verify position offsets in NED frame match expected

[Copilot]

Pull request overview

This PR introduces a MAVLink-based drone command system for tracking and following detected objects. The implementation translates camera-relative object positions through body-frame and NED coordinate transformations to generate movement and yaw commands for a flight controller.

Changes:

• Adds DroneCommander class that sends MAVLink SET_POSITION_TARGET_LOCAL_NED and MAV_CMD_CONDITION_YAW commands
• Implements coordinate frame conversions: camera frame → body frame → NED frame
• Adds worker function that selects the closest tracked object and commands the drone to pursue it

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 11 comments.

File	Description
modules/drone_command/init.py	Exports the DroneCommander class and worker function
modules/drone_command/drone_command.py	Core MAVLink command interface with coordinate transformations and movement/yaw commands
modules/drone_command/drone_command_worker.py	Worker loop that consumes tracked objects and commands the drone

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The import references a non-existent module 'modules.object_tracker.tracked_object'. This module does not exist in the codebase. The code will fail at runtime with an ImportError. Consider either creating the missing module or updating the import path to reference an existing module.

Suggested change

	from typing import List, Optional
	from modules.object_tracker.tracked_object import TrackedObject, TrackingStatus
	from enum import Enum
	from typing import List, Optional, Protocol
	try:
	from modules.object_tracker.tracked_object import TrackedObject, TrackingStatus
	except ImportError: # Fallback when object tracker module is unavailable
	class TrackingStatus(Enum):
	NEW = "NEW"
	TRACKED = "TRACKED"
	LOST = "LOST"
	class TrackedObject(Protocol):
	object_id: int
	label: str
	x: float
	y: float
	z: float
	status: TrackingStatus

[Copilot]

The infinite loop lacks error handling. If any exception occurs during queue.get(), target selection, or command sending, the worker will crash without cleanup. Consider wrapping the loop body in a try-except block to handle and log exceptions, allowing the worker to continue processing or shut down gracefully.

Suggested change

	tracked_objects: List[TrackedObject] = input_queue.get()
	target = _select_target(tracked_objects)
	if target is None:
	logger.debug("No TRACKED target this frame — holding position.")
	continue
	logger.info(
	"Pursuing target id=%d label=%s at (%.2f, %.2f, %.2f)",
	target.object_id,
	target.label,
	target.x,
	target.y,
	target.z,
	)
	# cam coords → body → MAVLink
	# yaw_rad=0.0 for now; in production read from flight controller
	commander.track_target(
	cam_x=target.x,
	cam_y=target.y,
	cam_z=target.z,
	yaw_rad=0.0,
	move=move,
	face=face,
	)
	try:
	tracked_objects: List[TrackedObject] = input_queue.get()
	target = _select_target(tracked_objects)
	if target is None:
	logger.debug("No TRACKED target this frame — holding position.")
	continue
	logger.info(
	"Pursuing target id=%d label=%s at (%.2f, %.2f, %.2f)",
	target.object_id,
	target.label,
	target.x,
	target.y,
	target.z,
	)
	# cam coords → body → MAVLink
	# yaw_rad=0.0 for now; in production read from flight controller
	commander.track_target(
	cam_x=target.x,
	cam_y=target.y,
	cam_z=target.z,
	yaw_rad=0.0,
	move=move,
	face=face,
	)
	except KeyboardInterrupt:
	logger.info("Drone command worker received KeyboardInterrupt, shutting down.")
	break
	except Exception:
	logger.exception(
	"Unhandled exception in drone command worker loop; continuing."
	)
	continue

[Copilot]

Using a hardcoded yaw_rad=0.0 will produce incorrect position commands when the drone is not facing north. The comment on line 91 acknowledges this ("in production read from flight controller"), but the code as written will send incorrect movement commands. Consider either reading the actual yaw from the flight controller using an ATTITUDE message, or documenting this as a critical limitation that must be addressed before deployment.

[Copilot]

The face_target method will call math.atan2(right, forward). When both forward and right are zero (target directly above/below the drone), atan2(0, 0) returns 0, which is technically correct but may not represent the intended behavior. Consider adding validation to handle the edge case where the target is at the same horizontal position as the drone, or document this behavior.

Suggested change

	"""
	if self._conn is None:
	raise RuntimeError("Not connected. Call connect() first.")
	# angle from drone nose to target (positive = clockwise)
	Note:
	If both ``forward`` and ``right`` are zero, the target is
	directly above or below the drone in the vertical axis and
	has no horizontal bearing. In this implementation,
	``atan2(0, 0)`` is allowed and interpreted as a 0° yaw
	change (i.e. no change in heading).
	"""
	if self._conn is None:
	raise RuntimeError("Not connected. Call connect() first.")
	# Angle from drone nose to target (positive = clockwise).
	# Note: when forward == 0 and right == 0 (target directly above/below),
	# atan2(0, 0) returns 0, which we treat as "no yaw change".

[Copilot]

The pymavlink dependency is imported on line 33 but is not listed in requirements.txt or pyproject.toml. This will cause an ImportError when the module is loaded. Add pymavlink to the project dependencies.

[Copilot]

Frame mismatch: the code manually rotates body coordinates to NED using body_to_ned() on line 155, then sends these NED coordinates using MAV_FRAME_BODY_OFFSET_NED on line 167. MAV_FRAME_BODY_OFFSET_NED expects body-frame coordinates as input and will perform the rotation to NED internally, causing a double rotation. Either use MAV_FRAME_LOCAL_OFFSET_NED with the NED coordinates, or use MAV_FRAME_BODY_OFFSET_NED with the body-frame coordinates (forward, right, down) directly without the body_to_ned conversion.

Suggested change

	mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # offset from current pos
	mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED, # offset from current pos in local NED

[Copilot]

The worker function establishes a MAVLink connection on line 68 but provides no mechanism for graceful shutdown or connection cleanup. The infinite loop on line 72 has no exit condition. Consider adding a shutdown signal mechanism (e.g., poison pill in queue, threading.Event, or signal handler) to allow clean termination and proper resource cleanup.

[Copilot]

The comment on line 117 states "yaw_rad: Current heading in radians (0 = north, CW positive)", but the rotation matrix implementation on lines 124-125 uses standard CCW (counter-clockwise) rotation convention. The rotation matrix "north = body_x * cos_yaw - body_y * sin_yaw; east = body_x * sin_yaw + body_y * cos_yaw" rotates CCW as yaw increases. This inconsistency between documentation and implementation will cause confusion. Verify the correct convention and update either the comment or the rotation matrix accordingly.

[Copilot]

The connect() method calls wait_heartbeat() on line 75 without a timeout parameter. If the flight controller is not available or not sending heartbeats, this call will block indefinitely. Consider adding a timeout parameter to wait_heartbeat() or wrapping the call in a timeout mechanism to prevent the application from hanging during connection.

Suggested change

	self._conn.wait_heartbeat()
	self._conn.wait_heartbeat(timeout=30)

[Copilot]

The worker sends MAVLink commands every time it receives tracked objects from the queue, with no rate limiting. If the upstream tracker sends data at high frequency (e.g., 30 Hz from video frames), this could flood the flight controller with rapid position and yaw commands, potentially causing instability or overwhelming the autopilot's command buffer. Consider implementing rate limiting, command throttling, or a minimum movement threshold before sending new commands.