diff --git a/main/env_sens.cpp b/main/env_sens.cpp
index 700fdc0..c12532f 100644
--- a/main/env_sens.cpp
+++ b/main/env_sens.cpp
@@ -5,7 +5,7 @@
 #include <Wire.h>
 
 #include "freertos/idf_additions.h"
-#include "nav.h"
+#include "sens_fus.h"
 
 #define SEALEVELPRESSURE_HPA (1030)
 
@@ -90,18 +90,19 @@ void baro_poll_task(void *_) {
 
       float v_z = (filtered_alt - last_alt) / dt;
 
-      if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) {
+      if (sens_fus_mutex &&
+          xSemaphoreTake(sens_fus_mutex, (TickType_t)20) == pdTRUE) {
 
-        Eigen::Vector3f baro_pos = nav_filter.position;
+        Eigen::Vector3f baro_pos = sens_fus.position;
         baro_pos.z() = filtered_alt;
 
-        Eigen::Vector3f baro_vel = nav_filter.velocity;
+        Eigen::Vector3f baro_vel = sens_fus.velocity;
         baro_vel.z() = v_z;
 
         // Update the filter with Baro data
-        nav_filter.measure_baro(dt, baro_pos, baro_vel);
+        sens_fus.measure_baro(dt, baro_pos, baro_vel);
 
-        xSemaphoreGive(nav_mutex);
+        xSemaphoreGive(sens_fus_mutex);
       }
 
       last_alt = filtered_alt;
diff --git a/main/gps.cpp b/main/gps.cpp
index a0d8238..cec8c0d 100644
--- a/main/gps.cpp
+++ b/main/gps.cpp
@@ -1,6 +1,6 @@
 #include "gps.h"
 #include "esp_log.h"
-#include "nav.h"
+#include "sens_fus.h"
 
 static const char *TAG = "GPS_TASK";
 
@@ -17,15 +17,15 @@ void gps_poll_task(void *_) {
 
       // ESP_LOGI(TAG, "Polling gps.");
       gps->poll();
-      if (gps->gps_avaliable() && nav_mutex &&
-          xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
+      if (gps->gps_avaliable() && sens_fus_mutex &&
+          xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
         auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
         auto coords = gps->get_coordinates();
         if (coords.has_value()) {
-          nav_filter.measure_gps(1.0, gps->get_coordinates().value(),
-                                 Eigen::Vector3f(vel.x(), vel.y(), 0.0));
+          sens_fus.measure_gps(1.0, gps->get_coordinates().value(),
+                               Eigen::Vector3f(vel.x(), vel.y(), 0.0));
         }
-        xSemaphoreGive(nav_mutex);
+        xSemaphoreGive(sens_fus_mutex);
       }
       xSemaphoreGive(gps_mutex);
     } else {
diff --git a/main/imu.cpp b/main/imu.cpp
index e475b66..04d210e 100644
--- a/main/imu.cpp
+++ b/main/imu.cpp
@@ -3,7 +3,7 @@
 #include "esp_timer.h"
 #include "freertos/idf_additions.h"
 #include "hal/spi_types.h"
-#include "nav.h"
+#include "sens_fus.h"
 
 #ifdef PS
 #undef PS
@@ -50,10 +50,10 @@ BNO08x *setup_imu(imu_state *state) {
         float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
         Vec3C accel_global = apply_rot(&state->accel, &state->rot);
 
-        if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
-          nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
-                                                 accel_global.z));
-          xSemaphoreGive(nav_mutex);
+        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
+          sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
+                                               accel_global.z));
+          xSemaphoreGive(sens_fus_mutex);
         } else {
           ESP_LOGE(TAG, "Failed to get mutex.");
         }
diff --git a/main/main.cpp b/main/main.cpp
index 504b360..d7d1b44 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -13,18 +13,19 @@
 
 #include "env_sens.h"
 #include "gps.h"
-#include "nav.h"
-#include "radio.h"
-
 #include "imu.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "radio.h"
+#include "sens_fus.h"
 
 static const char *TAG = "MAIN";
 
-void handle_packet(uint8_t *packet_addr);
-
 extern "C" void app_main(void) {
 
+  sens_fus_mutex = xSemaphoreCreateMutex();
   nav_mutex = xSemaphoreCreateMutex();
+
   initArduino();
   gpio_install_isr_service(0);
   Serial.begin(115200);
@@ -38,7 +39,7 @@ extern "C" void app_main(void) {
                           NULL,         // Parameters
                           5,            // Priority (higher = more urgent)
                           NULL,         // Task handle
-                          1             // Core ID
+                          0             // Core ID
   );
 
   xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
@@ -73,11 +74,12 @@ extern "C" void app_main(void) {
     }
     env_sens::dbg_sens();
 
-    if (nav_mutex && xSemaphoreTake(nav_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
-      local_pos = nav_filter.position;
-      local_vel = nav_filter.velocity;
+    if (sens_fus_mutex &&
+        xSemaphoreTake(sens_fus_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
+      local_pos = sens_fus.position;
+      local_vel = sens_fus.velocity;
       nav_data_ready = true;
-      xSemaphoreGive(nav_mutex); // RELEASE IMMEDIATELY
+      xSemaphoreGive(sens_fus_mutex);
     }
 
     if (nav_data_ready) {
@@ -90,42 +92,3 @@ extern "C" void app_main(void) {
     vTaskDelay(pdMS_TO_TICKS(1000));
   }
 }
-
-void handle_packet(uint8_t *packet_addr) {
-  PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
-
-  if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
-
-    packet_command_request *packet =
-        (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
-    PACKET_TYPE requested_type = packet->packet_requested;
-    std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
-
-    if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
-      resp_packet = create_packet_pooled(
-          PACKET_TYPE::INFO_DRONE_POSITION,
-          packet_info_drone_position{
-              {nav_filter.position.x(), nav_filter.position.y(),
-               nav_filter.position.z()},
-              {nav_filter.velocity.x(), nav_filter.velocity.y(),
-               nav_filter.velocity.z()},
-              {0.0, 0.0, 0.0}});
-    }
-
-    if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
-
-      if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
-
-        resp_packet = create_packet_pooled(
-            PACKET_TYPE::INFO_DRONE_STATUS,
-            packet_info_drone_status{
-                {gps->origin_long, gps->origin_lat}, millis(), 0});
-        xSemaphoreGive(gps_mutex);
-      }
-    }
-
-    if (resp_packet.first != nullptr) {
-      xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
-    }
-  }
-}
diff --git a/main/nav.cpp b/main/nav.cpp
new file mode 100644
index 0000000..775c1e7
--- /dev/null
+++ b/main/nav.cpp
@@ -0,0 +1 @@
+#include "nav.h"
diff --git a/main/nav.h b/main/nav.h
index b5b9a18..f69aad2 100644
--- a/main/nav.h
+++ b/main/nav.h
@@ -1,6 +1,8 @@
 #pragma once
+
+#include "Eigen/Core"
 #include "freertos/idf_additions.h"
-#include <cmath>
+#include <cstdint>
 
 #ifdef PS
 #undef PS
@@ -12,87 +14,33 @@
 
 #include <Eigen/Dense>
 
-inline float getYawDifference(const Eigen::Vector3f &v_gps,
-                              const Eigen::Vector3f &v_imu) {
-  float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
-  float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
+#define WAYPOINT_COUNT 8
 
-  float delta_yaw = yaw_gps - yaw_imu;
+struct waypoint {
+  Eigen::Vector3f coords; // long, lat, alt
+  bool active;            // active or to be skipped
+};
 
-  return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
-}
+struct drone_nav {
+  waypoint waypoints[WAYPOINT_COUNT];
+  uint8_t current_waypoint;
 
-struct nav_compl {
-  Eigen::Vector3f position = Eigen::Vector3f::Zero();
-  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
-  float yaw_offset = 0.0f;
-
-  // Time Constants per axis (X, Y, Z)
-  // Lower = faster tracking of GPS; Higher = smoother/more IMU trust
-  Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
-  Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
-
-  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
-  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
-
-  float tau_yaw = 2.0f; // Yaw remains a scalar
-
-  void predict(float dt, Eigen::Vector3f accel) {
-    // Rotate body-frame accel to world-frame
-    Eigen::Vector3f accel_rotated =
-        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
-
-    Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
-
-    // Trapezoidal integration for position
-    this->position += (this->velocity + next_velocity) * 0.5f * dt;
-    this->velocity = next_velocity;
-  }
-
-  void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
-    // Calculate Alpha vectors using element-wise operations
-    // Formula: alpha = dt / (tau + dt)
-    Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
-    Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
-    float alpha_yaw = dt / (tau_yaw + dt);
-
-    // 1. Position Update (Element-wise LPF)
-    // res = (1 - alpha) * state + alpha * measurement
-    this->position =
-        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
-        alpha_pos.array() * gps_pos.array();
-
-    // 2. Yaw Correction (only if moving > 1.0 m/s)
-    if (gps_vel.norm() > 1.0f) {
-      float yaw_delta = getYawDifference(gps_vel, this->velocity);
-      this->yaw_offset += yaw_delta * alpha_yaw;
-
-      this->yaw_offset =
-          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
+  void set_active_mask(uint8_t mask) {
+    for (int i = 0; i < WAYPOINT_COUNT; i++) {
+      this->waypoints[i].active = (mask & (1 << i)) != 0;
     }
-
-    // 3. Velocity Update (Element-wise LPF)
-    this->velocity =
-        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
-        alpha_vel.array() * gps_vel.array();
   }
 
-  void measure_baro(float dt, Eigen::Vector3f baro_pos,
-                    Eigen::Vector3f baro_vel) {
-    // Calculate Alpha vectors using element-wise operations
-    // Formula: alpha = dt / (tau + dt)
-    Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
-    Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
-
-    this->position =
-        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
-        alpha_pos.array() * baro_pos.array();
-
-    this->velocity =
-        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
-        alpha_vel.array() * baro_vel.array();
+  uint8_t get_active_mask() {
+    uint8_t mask = 0;
+    for (int i = 0; i < WAYPOINT_COUNT; i++) {
+      if (waypoints[i].active) {
+        mask |= (1 << i);
+      }
+    }
+    return mask;
   }
 };
 
 inline SemaphoreHandle_t nav_mutex = NULL;
-inline nav_compl nav_filter;
+inline drone_nav nav_man;
diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp
new file mode 100644
index 0000000..fd95e92
--- /dev/null
+++ b/main/packet_handler.cpp
@@ -0,0 +1,134 @@
+#include "packet_handler.h"
+
+#include "Eigen/Core"
+#include "drone_comms.h"
+#include "freertos/idf_additions.h"
+#include "gps.h"
+#include "nav.h"
+#include "portmacro.h"
+#include "radio.h"
+#include "sens_fus.h"
+#include <cstdint>
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+void handle_request(uint8_t *packet_addr);
+
+void handle_waypoint_update(uint8_t *packet_addr, uint8_t index);
+void handle_nav_update(uint8_t *packet_addr);
+
+void handle_packet(uint8_t *packet_addr) {
+  PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
+  if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
+    handle_request(packet_addr);
+  }
+
+  // NAV SETTERS
+  if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
+      packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
+    uint8_t index = packet_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
+    handle_waypoint_update(packet_addr, index);
+  }
+  if (packet_type == PACKET_TYPE::DRONE_NAV) {
+    handle_nav_update(packet_addr);
+  }
+}
+
+void handle_waypoint_update(uint8_t *packet_addr, uint8_t index) {
+  packet_drone_nav_waypoint *packet =
+      (packet_drone_nav_waypoint *)(packet_addr + sizeof(PACKET_TYPE));
+  float lon, lat, alt;
+  lon = packet->coord[0];
+  lat = packet->coord[1];
+  alt = packet->coord[2];
+  Eigen::Vector3f coords(lon, lat, alt);
+
+  if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+    nav_man.waypoints[index].coords = coords;
+    xSemaphoreGive(nav_mutex);
+  }
+}
+
+void handle_nav_update(uint8_t *packet_addr) {
+  packet_drone_nav *packet =
+      (packet_drone_nav *)(packet_addr + sizeof(PACKET_TYPE));
+
+  if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+    nav_man.set_active_mask(packet->active_mask);
+    nav_man.current_waypoint = packet->current_waypoint;
+  }
+}
+
+void handle_request(uint8_t *packet_addr) {
+
+  packet_command_request *packet =
+      (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
+  PACKET_TYPE requested_type = packet->packet_requested;
+  std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
+
+  if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
+    // TODO: send pitch, roll, yaw
+    resp_packet =
+        create_packet_pooled(PACKET_TYPE::INFO_DRONE_POSITION,
+                             packet_info_drone_position{
+                                 {sens_fus.position.x(), sens_fus.position.y(),
+                                  sens_fus.position.z()},
+                                 {sens_fus.velocity.x(), sens_fus.velocity.y(),
+                                  sens_fus.velocity.z()},
+                                 {0.0, 0.0, 0.0}});
+  }
+
+  if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
+
+    if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY) == pdTRUE) {
+
+      // TODO: Absolute time from GPS instead of 0
+      resp_packet = create_packet_pooled(
+          PACKET_TYPE::INFO_DRONE_STATUS,
+          packet_info_drone_status{
+              {gps->origin_long, gps->origin_lat}, millis(), 0});
+      xSemaphoreGive(gps_mutex);
+    }
+  }
+
+  // Navigation
+  if (requested_type == PACKET_TYPE::DRONE_NAV) {
+
+    uint8_t active_mask, current;
+    if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+      active_mask = nav_man.get_active_mask();
+      current = nav_man.current_waypoint;
+      xSemaphoreGive(nav_mutex);
+
+      resp_packet = create_packet_pooled(
+          PACKET_TYPE::DRONE_NAV, packet_drone_nav{active_mask, current});
+    }
+  }
+
+  if (requested_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
+      requested_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
+    uint8_t index = requested_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
+
+    Eigen::Vector3f coords;
+    if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+      coords = nav_man.waypoints[index].coords;
+      xSemaphoreGive(nav_mutex);
+    }
+
+    resp_packet = create_packet_pooled(
+        requested_type,
+        packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}});
+  }
+
+  if (resp_packet.first != nullptr) {
+    xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
+  }
+}
diff --git a/main/packet_handler.h b/main/packet_handler.h
new file mode 100644
index 0000000..e22d46a
--- /dev/null
+++ b/main/packet_handler.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include <cstdint>
+
+void handle_packet(uint8_t *packet_addr);
diff --git a/main/sens_fus.h b/main/sens_fus.h
new file mode 100644
index 0000000..6cefe22
--- /dev/null
+++ b/main/sens_fus.h
@@ -0,0 +1,97 @@
+#pragma once
+#include "freertos/idf_additions.h"
+#include <cmath>
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+inline float getYawDifference(const Eigen::Vector3f &v_gps,
+                              const Eigen::Vector3f &v_imu) {
+  float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
+  float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
+
+  float delta_yaw = yaw_gps - yaw_imu;
+  return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
+}
+
+struct sens_fus_compl {
+  Eigen::Vector3f position = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+  float yaw_offset = 0.0f;
+
+  // Time Constants per axis (X, Y, Z)
+  // Lower = faster tracking of GPS; Higher = smoother/more IMU trust
+  Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
+  Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
+
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
+
+  float tau_yaw = 2.0f; // Yaw remains a scalar
+
+  void predict(float dt, Eigen::Vector3f accel) {
+    // Rotate body-frame accel to world-frame
+    Eigen::Vector3f accel_rotated =
+        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
+
+    Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
+
+    // Trapezoidal integration for position
+    this->position += (this->velocity + next_velocity) * 0.5f * dt;
+    this->velocity = next_velocity;
+  }
+
+  void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
+    // Calculate Alpha vectors using element-wise operations
+    // Formula: alpha = dt / (tau + dt)
+    Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
+    Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
+    float alpha_yaw = dt / (tau_yaw + dt);
+
+    // 1. Position Update (Element-wise LPF)
+    // res = (1 - alpha) * state + alpha * measurement
+    this->position =
+        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
+        alpha_pos.array() * gps_pos.array();
+
+    // 2. Yaw Correction (only if moving > 1.0 m/s)
+    if (gps_vel.norm() > 1.0f) {
+      float yaw_delta = getYawDifference(gps_vel, this->velocity);
+      this->yaw_offset += yaw_delta * alpha_yaw;
+
+      this->yaw_offset =
+          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
+    }
+
+    // 3. Velocity Update (Element-wise LPF)
+    this->velocity =
+        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
+        alpha_vel.array() * gps_vel.array();
+  }
+
+  void measure_baro(float dt, Eigen::Vector3f baro_pos,
+                    Eigen::Vector3f baro_vel) {
+    // Calculate Alpha vectors using element-wise operations
+    // Formula: alpha = dt / (tau + dt)
+    Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
+    Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
+
+    this->position =
+        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
+        alpha_pos.array() * baro_pos.array();
+
+    this->velocity =
+        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
+        alpha_vel.array() * baro_vel.array();
+  }
+};
+
+inline SemaphoreHandle_t sens_fus_mutex = NULL;
+inline sens_fus_compl sens_fus;