diff --git a/conf/flight_plans/fixedwing_demo_takeoff_land.xml b/conf/flight_plans/fixedwing_demo_takeoff_land.xml
index 73809db45b..7975f460a0 100644
--- a/conf/flight_plans/fixedwing_demo_takeoff_land.xml
+++ b/conf/flight_plans/fixedwing_demo_takeoff_land.xml
@@ -36,7 +36,7 @@
     </block>
     <block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
       <set value="0" var="autopilot.flight_time"/>
-      <call fun="nav_takeoff_from_here()"/>
+      <call fun="nav_takeoff_from_here(-1.)"/>
       <deroute block="Standby"/>
     </block>
     <block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
diff --git a/conf/flight_plans/rotorcraft_demo_takeoff_land.xml b/conf/flight_plans/rotorcraft_demo_takeoff_land.xml
index 88c9b30764..b9214b35dc 100644
--- a/conf/flight_plans/rotorcraft_demo_takeoff_land.xml
+++ b/conf/flight_plans/rotorcraft_demo_takeoff_land.xml
@@ -35,7 +35,7 @@
       <deroute block="Standby"/>
     </block>
     <block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
-      <call fun="nav_takeoff_from_here()"/>
+      <call fun="nav_takeoff_from_here(-1.)"/>
       <deroute block="Standby"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
diff --git a/sw/airborne/modules/nav/nav_takeoff_and_landing.h b/sw/airborne/modules/nav/nav_takeoff_and_landing.h
index eeb2069731..8e6249054b 100644
--- a/sw/airborne/modules/nav/nav_takeoff_and_landing.h
+++ b/sw/airborne/modules/nav/nav_takeoff_and_landing.h
@@ -86,9 +86,10 @@ extern void nav_takeoff_and_landing_periodic(void);
  * - rotorcraft: set the wp at current location and climb verticaly
  *
  * @param[in] wp_id waypoint ID
+ * @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
  * @return true until procedure is completed
  */
-extern bool nav_takeoff_from_wp(uint8_t wp_id);
+extern bool nav_takeoff_from_wp(uint8_t wp_id, float height);
 
 /** Takeoff from lat long location
  *
@@ -97,18 +98,20 @@ extern bool nav_takeoff_from_wp(uint8_t wp_id);
  *
  * @param[in] lat takeoff latitude (deg)
  * @param[in] lon takeoff longitude (deg)
+ * @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
  * @return true until procedure is completed
  */
-extern bool nav_takeoff_from_loc(float lat, float lon);
+extern bool nav_takeoff_from_loc(float lat, float lon, float height);
 
 /** Takeoff from current location
  *
  * - fixedwing: climb direction is specified with setting
  * - rotorcraft: start from current location and climb verticaly
  *
+ * @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
  * @return true until procedure is completed
  */
-extern bool nav_takeoff_from_here(void);
+extern bool nav_takeoff_from_here(float height);
 
 /** Land at waypoint location
  *
diff --git a/sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c b/sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c
index 4f95a784be..02697df4e7 100644
--- a/sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c
+++ b/sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c
@@ -76,9 +76,10 @@ float nav_takeoff_direction;
 
 static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
 {
-  if (flag == MissionInit) {
+  if (flag == MissionInit && nb == 1) {
+    float height = params[0];
     takeoff.status = NAV_TAKEOFF_INIT;
-    return nav_takeoff_from_here();
+    return nav_takeoff_from_here(height);
   }
   else if (flag == MissionRun) {
     return nav_takeoff_run();
@@ -175,7 +176,7 @@ static bool nav_takeoff_run(void) {
       NavVerticalAutoThrottleMode(RadOfDeg(NAV_TAKEOFF_PITCH));
       NavVerticalThrottleMode(TRIM_UPPRZ(MAX_PPRZ*NAV_TAKEOFF_THROTTLE));
       if (nav_approaching_xy(takeoff.climb_pos.x, takeoff.climb_pos.y, takeoff.start_pos.x, takeoff.start_pos.y, CARROT)
-          || (stateGetPositionEnu_f()->z > takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT)) {
+          || (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z)) {
         // end when climb point or target alt is reached
         takeoff.status = NAV_TAKEOFF_DONE;
       }
@@ -188,39 +189,53 @@ static bool nav_takeoff_run(void) {
   return true;
 }
 
-bool nav_takeoff_from_wp(uint8_t wp_id)
+bool nav_takeoff_from_wp(uint8_t wp_id, float height)
 {
   if (takeoff.status == NAV_TAKEOFF_INIT) {
+    takeoff.start_pos = *stateGetPositionEnu_f();
     takeoff.climb_id = wp_id;
     takeoff.climb_pos.x = WaypointX(wp_id);
     takeoff.climb_pos.y = WaypointY(wp_id);
-    takeoff.climb_pos.z = WaypointAlt(wp_id) - GetAltRef();
-    takeoff.start_pos = *stateGetPositionEnu_f();
+    if (height < 0.f) {
+      takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
+    } else {
+      takeoff.climb_pos.z = takeoff.start_pos.z + height;
+    }
   }
 
   return nav_takeoff_run();
 }
 
-bool nav_takeoff_from_loc(float lat, float lon)
+bool nav_takeoff_from_loc(float lat, float lon, float height)
 {
   if (takeoff.status == NAV_TAKEOFF_INIT) {
-    struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), stateGetPositionLla_f()->alt + NAV_TAKEOFF_HEIGHT };
+    takeoff.start_pos = *stateGetPositionEnu_f();
+    float alt = stateGetPositionLla_f()->alt;
+    if (height < 0.f) {
+      alt += NAV_TAKEOFF_HEIGHT;
+    } else {
+      alt += height;
+    }
+    struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), alt };
     struct UtmCoor_f utm;
     utm_of_lla_f(&utm, &lla);
     ENU_OF_UTM_DIFF(takeoff.climb_pos, utm, *stateGetUtmOrigin_f());
   }
-  return nav_takeoff_from_here();
+  return nav_takeoff_run();
 }
 
-bool nav_takeoff_from_here(void)
+bool nav_takeoff_from_here(float height)
 {
   if (takeoff.status == NAV_TAKEOFF_INIT) {
     takeoff.climb_id = 0;
     takeoff.start_pos = *stateGetPositionEnu_f();
-    takeoff.climb_pos = takeoff.start_pos;
     takeoff.climb_pos.x = takeoff.start_pos.x + NAV_TAKEOFF_DIST * sinf(RadOfDeg(nav_takeoff_direction));
     takeoff.climb_pos.y = takeoff.start_pos.y + NAV_TAKEOFF_DIST * cosf(RadOfDeg(nav_takeoff_direction));
-    takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
+    if (height < 0.f) {
+      takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
+    } else {
+      takeoff.climb_pos.z = takeoff.start_pos.z + height;
+    }
   }
 
   return nav_takeoff_run();
diff --git a/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c b/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c
index 6376088f22..292a6f865e 100644
--- a/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c
+++ b/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c
@@ -64,9 +64,10 @@ float nav_takeoff_direction;
 
 static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
 {
-  if (flag == MissionInit) {
+  if (flag == MissionInit && nb == 1) {
+    float height = params[0];
     takeoff.status = NAV_TAKEOFF_INIT;
-    return nav_takeoff_from_here();
+    return nav_takeoff_from_here(height);
   }
   else if (flag == MissionRun) {
     return nav_takeoff_run();
@@ -157,7 +158,7 @@ static bool nav_takeoff_run(void) {
       autopilot_set_in_flight(true);
       NavGotoWaypoint(takeoff.climb_id);
       NavVerticalClimbMode(NAV_TAKEOFF_CLIMB_SPEED);
-      if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > NAV_TAKEOFF_HEIGHT) {
+      if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z) {
         // end when takeoff height is reached
         takeoff.status = NAV_TAKEOFF_DONE;
       }
@@ -171,29 +172,39 @@ static bool nav_takeoff_run(void) {
   return true;
 }
 
-bool nav_takeoff_from_wp(uint8_t wp_id)
+bool nav_takeoff_from_wp(uint8_t wp_id, float height)
 {
   if (takeoff.status == NAV_TAKEOFF_INIT) {
     takeoff.climb_id = wp_id;
     waypoint_set_here_2d(wp_id);
     takeoff.climb_pos = *waypoint_get_enu_f(wp_id);
     takeoff.start_pos = *stateGetPositionEnu_f();
+    if (height < 0.f) {
+      takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT; // default value
+    } else {
+      takeoff.climb_pos.z = takeoff.start_pos.z + height;
+    }
   }
 
   return nav_takeoff_run();
 }
 
-bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED)
+bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED, float height)
 {
-  return nav_takeoff_from_here();
+  return nav_takeoff_from_here(height);
 }
 
-bool nav_takeoff_from_here(void)
+bool nav_takeoff_from_here(float height)
 {
   if (takeoff.status == NAV_TAKEOFF_INIT) {
     takeoff.climb_id = 0; // use dummy hidden WP
     takeoff.start_pos = *stateGetPositionEnu_f();
     takeoff.climb_pos = takeoff.start_pos;
+    if (height < 0.f) {
+      takeoff.climb_pos.z += NAV_TAKEOFF_HEIGHT; // default value
+    } else {
+      takeoff.climb_pos.z += height;
+    }
     waypoint_set_enu(0, &takeoff.climb_pos);
   }