diff --git a/src/mrs_trajectory_generation.cpp b/src/mrs_trajectory_generation.cpp
index 5662bb7..de06766 100644
--- a/src/mrs_trajectory_generation.cpp
+++ b/src/mrs_trajectory_generation.cpp
@@ -518,6 +518,13 @@ std::tuple<std::optional<mrs_msgs::TrackerCommand>, bool, int> MrsTrajectoryGene
 
     initial_condition.position = uav_state->pose.position;
 
+    try {
+      initial_condition.heading = mrs_lib::AttitudeConverter(uav_state->pose.orientation).getHeading();
+    }
+    catch (...) {
+      ROS_WARN_THROTTLE(1.0, "[MrsTrajectoryGeneration]: could not obtain heading from the UAV State");
+    }
+
     initial_condition.position.z += _takeoff_height_;
 
     initial_condition.header = uav_state->header;
diff --git a/test/get_path_before_takeoff/config/custom_config.yaml b/test/get_path_before_takeoff/config/custom_config.yaml
new file mode 100644
index 0000000..eb09ff4
--- /dev/null
+++ b/test/get_path_before_takeoff/config/custom_config.yaml
@@ -0,0 +1,7 @@
+mrs_uav_managers:
+
+  uav_manager:
+
+    takeoff:
+
+      takeoff_height: 3.0
diff --git a/test/get_path_before_takeoff/get_path_before_takeoff.test b/test/get_path_before_takeoff/get_path_before_takeoff.test
index f95d40d..e3fbaf2 100644
--- a/test/get_path_before_takeoff/get_path_before_takeoff.test
+++ b/test/get_path_before_takeoff/get_path_before_takeoff.test
@@ -25,6 +25,7 @@
   <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
     <arg name="automatic_start" default="true" />
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
   </include>
 
diff --git a/test/get_path_before_takeoff/test.cpp b/test/get_path_before_takeoff/test.cpp
index c42645f..92e8d40 100644
--- a/test/get_path_before_takeoff/test.cpp
+++ b/test/get_path_before_takeoff/test.cpp
@@ -82,6 +82,22 @@ bool Tester::test() {
     }
   }
 
+  // | ---------- add the first waypoint after takeoff ---------- |
+
+  {
+    auto uav_pos = uh_->sh_uav_state_.getMsg()->pose.position;
+
+    double heading = mrs_lib::AttitudeConverter(uh_->sh_uav_state_.getMsg()->pose.orientation).getHeading();
+
+    mrs_msgs::Reference reference;
+    reference.position.x = uav_pos.x;
+    reference.position.y = uav_pos.y;
+    reference.position.z = uav_pos.z + 3.0;
+    reference.heading    = heading;
+
+    path.points.insert(path.points.begin(), reference);
+  }
+
   // | ------------------ check the trajectory ------------------ |
 
   {
diff --git a/test/path_before_takeoff/config/custom_config.yaml b/test/path_before_takeoff/config/custom_config.yaml
new file mode 100644
index 0000000..eb09ff4
--- /dev/null
+++ b/test/path_before_takeoff/config/custom_config.yaml
@@ -0,0 +1,7 @@
+mrs_uav_managers:
+
+  uav_manager:
+
+    takeoff:
+
+      takeoff_height: 3.0
diff --git a/test/path_before_takeoff/path_before_takeoff.test b/test/path_before_takeoff/path_before_takeoff.test
index f95d40d..e3fbaf2 100644
--- a/test/path_before_takeoff/path_before_takeoff.test
+++ b/test/path_before_takeoff/path_before_takeoff.test
@@ -25,6 +25,7 @@
   <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
     <arg name="automatic_start" default="true" />
     <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
     <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
   </include>
 
diff --git a/test/path_before_takeoff/test.cpp b/test/path_before_takeoff/test.cpp
index ba5211a..012d45a 100644
--- a/test/path_before_takeoff/test.cpp
+++ b/test/path_before_takeoff/test.cpp
@@ -81,6 +81,22 @@ bool Tester::test() {
     }
   }
 
+  // | ---------- add the first waypoint after takeoff ---------- |
+
+  {
+    auto uav_pos = uh_->sh_uav_state_.getMsg()->pose.position;
+
+    double heading = mrs_lib::AttitudeConverter(uh_->sh_uav_state_.getMsg()->pose.orientation).getHeading();
+
+    mrs_msgs::Reference reference;
+    reference.position.x = uav_pos.x;
+    reference.position.y = uav_pos.y;
+    reference.position.z = uav_pos.z + 3.0;
+    reference.heading    = heading;
+
+    path.points.insert(path.points.begin(), reference);
+  }
+
   // | ------------------------- takeoff ------------------------ |
 
   {