diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py
index 51b4408..553ad1d 100644
--- a/aerpaw/client/uav_runner.py
+++ b/aerpaw/client/uav_runner.py
@@ -9,16 +9,14 @@ Or use the auto-detecting wrapper:
     ./run_uav.sh
 
 Binary Protocol:
-    Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
-    Client sends: ACK (1 byte)
-    Client (after moving): READY (1 byte)
-    Server sends: RTL (1 byte)
-    Client sends: ACK (1 byte)
-    Client (after returning home): READY (1 byte)
-    Server sends: LAND (1 byte)
-    Client sends: ACK (1 byte)
-    Client (after landing): READY (1 byte)
-    Server sends: READY (1 byte) - mission complete, disconnect
+    For each waypoint:
+        Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
+        Client sends: ACK (1 byte)
+        Client (after moving): READY (1 byte)
+    After all waypoints:
+        Server sends: RTL (1 byte) → Client: ACK, return home, READY
+        Server sends: LAND (1 byte) → Client: ACK, land, READY
+        Server sends: READY (1 byte) - mission complete, disconnect
 """
 from enum import IntEnum
 from pathlib import Path
@@ -43,7 +41,7 @@ class MessageType(IntEnum):
 
 
 AERPAW_DIR = Path(__file__).parent.parent
-CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml"
+CONFIG_FILE = AERPAW_DIR / "config" / "client.yaml"
 
 
 def load_config():
@@ -89,22 +87,6 @@ def send_message_type(sock: socket.socket, msg_type: MessageType):
     sock.sendall(bytes([msg_type]))
 
 
-def recv_target(sock: socket.socket) -> tuple[float, float, float]:
-    """Receive TARGET message (1 byte type + 3 doubles).
-
-    Returns (x, y, z) ENU coordinates.
-    """
-    # First byte is message type (already consumed or check it)
-    msg_type = recv_message_type(sock)
-    if msg_type != MessageType.TARGET:
-        raise ValueError(f"Expected TARGET, got {msg_type.name}")
-
-    # Next 24 bytes are 3 doubles (little-endian)
-    data = recv_exactly(sock, 24)
-    x, y, z = struct.unpack('<ddd', data)
-    return x, y, z
-
-
 class UAVRunner(BasicRunner):
     def initialize_args(self, extra_args):
         """Load configuration from YAML config file."""
@@ -126,6 +108,11 @@ class UAVRunner(BasicRunner):
     @entrypoint
     async def run_mission(self, drone: Drone):
         """Main mission entry point."""
+        # Enable built-in telemetry logging
+        drone._verbose_logging = True
+        drone._verbose_logging_file_prefix = "uav_telemetry"
+        drone._verbose_logging_delay = 1.0  # 1 Hz
+
         print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
 
         # Retry connection up to 10 times (~30 seconds total)
@@ -153,58 +140,59 @@ class UAVRunner(BasicRunner):
             # Takeoff to above AERPAW minimum altitude
             print("[UAV] Taking off...")
             await drone.takeoff(25)
-            print("[UAV] Takeoff complete, waiting for target...")
+            print("[UAV] Takeoff complete, waiting for commands...")
 
-            # Receive TARGET command (1 byte type + 24 bytes coords)
-            enu_x, enu_y, enu_z = recv_target(sock)
-            print(f"[UAV] Received TARGET: x={enu_x}, y={enu_y}, z={enu_z}")
-
-            # Convert ENU to lat/lon
-            # ENU: x=East, y=North, z=Up
-            # NED: north, east, down
-            target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
-            print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
-
-            # Acknowledge
-            send_message_type(sock, MessageType.ACK)
-            print(f"[UAV] Sent {MessageType.ACK.name}")
-
-            # Move to target
-            print("[UAV] Moving to target...")
-            await drone.goto_coordinates(target)
-            print("[UAV] Arrived at target")
-
-            # Signal ready
-            send_message_type(sock, MessageType.READY)
-            print(f"[UAV] Sent {MessageType.READY.name}, waiting for commands...")
-
-            # Command loop - handle RTL, LAND, READY (mission complete) from controller
+            # Command loop - handle TARGET, RTL, LAND, READY from controller
+            waypoint_num = 0
             while True:
                 msg_type = recv_message_type(sock)
                 print(f"[UAV] Received: {msg_type.name}")
 
-                if msg_type == MessageType.RTL:
+                if msg_type == MessageType.TARGET:
+                    # Read 24 bytes of coordinates (3 little-endian doubles)
+                    data = recv_exactly(sock, 24)
+                    enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
+                    waypoint_num += 1
+                    print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x}, y={enu_y}, z={enu_z}")
+
+                    # Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
+                    target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
+                    print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
+
                     send_message_type(sock, MessageType.ACK)
-                    print(f"[UAV] Sent {MessageType.ACK.name}")
+                    print(f"[UAV] Sent ACK")
+
+                    print(f"[UAV] Moving to waypoint {waypoint_num}...")
+                    await drone.goto_coordinates(target)
+                    print(f"[UAV] Arrived at waypoint {waypoint_num}")
+
+                    send_message_type(sock, MessageType.READY)
+                    print(f"[UAV] Sent READY")
+
+                elif msg_type == MessageType.RTL:
+                    send_message_type(sock, MessageType.ACK)
+                    print(f"[UAV] Sent ACK")
                     print("[UAV] Returning to home...")
-                    # Navigate to home lat/lon at safe altitude
                     home = drone.home_coords
-                    safe_alt = 25  # Set to 25m alt for RTL motion
+                    safe_alt = 25
                     rtl_target = Coordinate(home.lat, home.lon, safe_alt)
                     print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
                     await drone.goto_coordinates(rtl_target)
                     print("[UAV] Arrived at home position")
                     send_message_type(sock, MessageType.READY)
-                    print(f"[UAV] Sent {MessageType.READY.name}")
+                    print(f"[UAV] Sent READY")
 
                 elif msg_type == MessageType.LAND:
                     send_message_type(sock, MessageType.ACK)
-                    print(f"[UAV] Sent {MessageType.ACK.name}")
+                    print(f"[UAV] Sent ACK")
                     print("[UAV] Landing...")
                     await drone.land()
-                    print("[UAV] Landed and disarmed")
+                    # Switch out of LAND mode so the drone is re-armable
+                    from dronekit import VehicleMode
+                    drone._vehicle.mode = VehicleMode("ALT_HOLD")
+                    print("[UAV] Landed and disarmed (ALT_HOLD)")
                     send_message_type(sock, MessageType.READY)
-                    print(f"[UAV] Sent {MessageType.READY.name}")
+                    print(f"[UAV] Sent READY")
 
                 elif msg_type == MessageType.READY:
                     print("[UAV] Mission complete")
diff --git a/aerpaw/config/config.yaml b/aerpaw/config/client.yaml
similarity index 64%
rename from aerpaw/config/config.yaml
rename to aerpaw/config/client.yaml
index f73742f..eb3a7b0 100644
--- a/aerpaw/config/config.yaml
+++ b/aerpaw/config/client.yaml
@@ -1,18 +1,10 @@
-# AERPAW UAV Guidance System Configuration
-# This file contains all settings for both simulation and testbed environments
+# AERPAW UAV (Client) Configuration
 
 # ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
 origin:
   lat: 35.727436
   lon: -78.696471
   alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
-
-# Target positions in ENU coordinates (meters)
-# Each target is assigned to a client in order: client 1 gets targets[0], etc.
-targets:
-  - [10.5, 20.3, 45.0]   # Client 1: east, north, up
-  - [25.0, 30.0, 30.0]   # Client 2: east, north, up
-
 # Environment-specific settings
 environments:
   local:
@@ -33,4 +25,4 @@ environments:
     # Controller runs on host machine (192.168.122.1 from E-VM perspective)
     controller:
       ip: "192.168.122.1"
-      port: 5000
\ No newline at end of file
+      port: 5000
diff --git a/aerpaw/config/server.yaml b/aerpaw/config/server.yaml
new file mode 100644
index 0000000..a1a0fc9
--- /dev/null
+++ b/aerpaw/config/server.yaml
@@ -0,0 +1,24 @@
+# AERPAW Controller (Server) Configuration
+
+# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
+origin:
+  lat: 35.727436
+  lon: -78.696471
+  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
+
+# Target waypoints in ENU coordinates (meters: east, north, up)
+# Grouped by client: first N entries = Client 1, next N = Client 2, etc.
+# Each client must have the same number of waypoints.
+targets:
+  # Client 1 waypoints
+  - [10.5, 20.3, 45.0]
+  - [30.0, 40.0, 45.0]
+  - [50.0, 60.0, 45.0]
+  - [70.0, 80.0, 45.0]
+  - [90.0, 100.0, 45.0]
+  # Client 2 waypoints
+  - [25.0, 30.0, 30.0]
+  - [35.0, 40.0, 30.0]
+  - [45.0, 50.0, 30.0]
+  - [55.0, 60.0, 30.0]
+  - [65.0, 70.0, 30.0]
diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj
index 07dbbf4..192f8ba 100644
--- a/aerpaw/controller.coderprj
+++ b/aerpaw/controller.coderprj
@@ -58,6 +58,11 @@
                   <Size type="coderapp.internal.codertype.Dimension"/>
                   <Size type="coderapp.internal.codertype.Dimension"/>
                 </Types>
+                <Types id="12" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
               </Types>
             </coderapp.internal.interface.project.Interface>
           </MF0>
@@ -155,7 +160,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-02-01T15:39:22</Timestamp>
+                <Timestamp>2026-02-13T18:04:24</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>
diff --git a/aerpaw/controller.m b/aerpaw/controller.m
index c839ca7..922937f 100644
--- a/aerpaw/controller.m
+++ b/aerpaw/controller.m
@@ -5,26 +5,32 @@ end
 
 coder.extrinsic('disp', 'loadTargetsFromYaml');
 
-% Maximum clients supported
+% Maximum clients and waypoints supported
 MAX_CLIENTS = 4;
+MAX_WAYPOINTS = 10;
+MAX_TARGETS = MAX_CLIENTS * MAX_WAYPOINTS;
 
-% Allocate targets array (MAX_CLIENTS x 3)
-targets = zeros(MAX_CLIENTS, 3);
+% Allocate targets array (MAX_TARGETS x 3)
+targets = zeros(MAX_TARGETS, 3);
+numWaypoints = int32(0);
 
 % Load targets from YAML config file
 if coder.target('MATLAB')
-    disp('Loading targets from config.yaml (simulation)...');
-    targetsLoaded = loadTargetsFromYaml('aerpaw/config/config.yaml');
-    numTargets = min(size(targetsLoaded, 1), numClients);
-    targets(1:numTargets, :) = targetsLoaded(1:numTargets, :);
-    disp(['Loaded ', num2str(numTargets), ' targets']);
+    disp('Loading targets from server.yaml (simulation)...');
+    targetsLoaded = loadTargetsFromYaml('aerpaw/config/server.yaml');
+    totalLoaded = size(targetsLoaded, 1);
+    targets(1:totalLoaded, :) = targetsLoaded(1:totalLoaded, :);
+    numWaypoints = int32(totalLoaded / numClients);
+    disp(['Loaded ', num2str(numWaypoints), ' waypoints per client']);
 else
     coder.cinclude('controller_impl.h');
     % Define filename as null-terminated character array for C compatibility
-    filename = ['config/config.yaml', char(0)];
+    filename = ['config/server.yaml', char(0)];
     % loadTargets fills targets array (column-major for MATLAB compatibility)
-    coder.ceval('loadTargets', coder.ref(filename), ...
-                coder.ref(targets), int32(MAX_CLIENTS));
+    totalLoaded = int32(0);
+    totalLoaded = coder.ceval('loadTargets', coder.ref(filename), ...
+                coder.ref(targets), int32(MAX_TARGETS));
+    numWaypoints = totalLoaded / int32(numClients);
 end
 
 % Initialize server
@@ -43,35 +49,38 @@ for i = 1:numClients
     end
 end
 
-% Send target coordinates to each client
-for i = 1:numClients
-    % Get target for this client (1x3 array)
-    target = targets(i, :);
+% Waypoint loop: send each waypoint to all clients, wait for all to arrive
+for w = 1:numWaypoints
+    % Send TARGET for waypoint w to each client
+    for i = 1:numClients
+        % Targets are grouped by client: client i's waypoints are at rows
+        % (i-1)*numWaypoints+1 through i*numWaypoints
+        targetIdx = (i - 1) * numWaypoints + w;
+        target = targets(targetIdx, :);
 
-    if coder.target('MATLAB')
-        disp(['Sending ', char(MESSAGE_TYPE.TARGET), ' to client ', num2str(i), ': ', ...
-              num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
-    else
-        coder.ceval('sendTarget', int32(i), coder.ref(target));
+        if coder.target('MATLAB')
+            disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
+                  num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
+        else
+            coder.ceval('sendTarget', int32(i), coder.ref(target));
+        end
     end
-end
 
-% Wait for ACK from all clients
-if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']);
-    disp('All acknowledgments received.');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.ACK));
-end
+    % Wait for ACK from all clients
+    if coder.target('MATLAB')
+        disp('Waiting for ACK from all clients...');
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.ACK));
+    end
 
-% Wait for READY signals from all clients
-if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']);
-    disp('All UAVs at target positions.');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.READY));
+    % Wait for READY from all clients (all arrived at waypoint w)
+    if coder.target('MATLAB')
+        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.READY));
+    end
 end
 
 % Wait for user input before closing experiment
@@ -84,7 +93,7 @@ end
 % Send RTL command to all clients
 for i = 1:numClients
     if coder.target('MATLAB')
-        disp(['Sending ', char(MESSAGE_TYPE.RTL), ' to client ', num2str(i)]);
+        disp(['Sending RTL to client ', num2str(i)]);
     else
         coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL));
     end
@@ -92,7 +101,7 @@ end
 
 % Wait for ACK from all clients
 if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']);
+    disp('Waiting for ACK from all clients...');
 else
     coder.ceval('waitForAllMessageType', int32(numClients), ...
                 int32(MESSAGE_TYPE.ACK));
@@ -100,7 +109,6 @@ end
 
 % Wait for READY from all clients (returned to home)
 if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']);
     disp('All UAVs returned to home.');
 else
     coder.ceval('waitForAllMessageType', int32(numClients), ...
@@ -110,7 +118,7 @@ end
 % Send LAND command to all clients
 for i = 1:numClients
     if coder.target('MATLAB')
-        disp(['Sending ', char(MESSAGE_TYPE.LAND), ' to client ', num2str(i)]);
+        disp(['Sending LAND to client ', num2str(i)]);
     else
         coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND));
     end
@@ -118,7 +126,7 @@ end
 
 % Wait for ACK from all clients
 if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']);
+    disp('Waiting for ACK from all clients...');
 else
     coder.ceval('waitForAllMessageType', int32(numClients), ...
                 int32(MESSAGE_TYPE.ACK));
@@ -126,7 +134,6 @@ end
 
 % Wait for READY from all clients (landed and disarmed)
 if coder.target('MATLAB')
-    disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']);
     disp('All UAVs landed and disarmed.');
 else
     coder.ceval('waitForAllMessageType', int32(numClients), ...
@@ -136,7 +143,7 @@ end
 % Send READY to all clients to signal mission complete
 for i = 1:numClients
     if coder.target('MATLAB')
-        disp(['Sending ', char(MESSAGE_TYPE.READY), ' to client ', num2str(i)]);
+        disp(['Sending READY to client ', num2str(i)]);
     else
         coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY));
     end
diff --git a/aerpaw/run_uav.sh b/aerpaw/run_uav.sh
index c501282..62d2275 100755
--- a/aerpaw/run_uav.sh
+++ b/aerpaw/run_uav.sh
@@ -38,7 +38,7 @@ export AERPAW_ENV="$ENV"
 # Read MAVLink connection from config.yaml using Python
 CONN=$(python3 -c "
 import yaml
-with open('config/config.yaml') as f:
+with open('config/client.yaml') as f:
     cfg = yaml.safe_load(f)
 env = cfg['environments']['$ENV']['mavlink']
 print(f\"udp:{env['ip']}:{env['port']}\")