From 82f27884df81ad727b1ee84cb5c3bc612cd6786a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 23 Oct 2016 12:34:30 +0200
Subject: [PATCH] navigator: fix wrong altitude after takeoff

This change fixes a wrong behaviour when a takeoff command is sent.

An example:
- MIS_TAKEOFF_ALT set to 10 meters
- Takeoff command with alt setpoint of 2 meters

Old behaviour:
1. Climb to 10 meters -> takeoff WP reached
2. Go to setpoint at 2 meters

New behaviour:
1. Climb to 10 meters and stay there but notify that altitude was
   overridden.
---
 src/modules/navigator/mission_block.cpp | 16 +++--------
 src/modules/navigator/mission_block.h   |  2 +-
 src/modules/navigator/takeoff.cpp       | 38 +++++++++++++++++++++----
 3 files changed, 37 insertions(+), 19 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 50ff3c6717..b1aa4c61a9 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -618,25 +618,17 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
 }
 
 void
-MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
+MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
 {
 	item->nav_cmd = NAV_CMD_TAKEOFF;
 
-	/* use current position and use return altitude as clearance */
+	/* use current position */
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->altitude = _navigator->get_global_position()->alt;
-
-	if (min_clearance > 0.0f) {
-		item->altitude += min_clearance;
-
-		/* we must takeoff to a point further above ground than the acceptance radius */
-		if (_navigator->get_acceptance_radius() > min_clearance) {
-			item->altitude += _navigator->get_acceptance_radius();
-		}
-	}
 
+	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
+
 	item->yaw = NAN;
 	item->loiter_radius = _navigator->get_loiter_radius();
 	item->loiter_direction = 1;
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 54da136d52..c2b2412675 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -107,7 +107,7 @@ protected:
 	/**
 	 * Set a takeoff mission item
 	 */
-	void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f);
+	void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch = 0.0f);
 
 	/**
 	 * Set a land mission item
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index e075c248f1..f419705bdf 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -99,8 +99,39 @@ Takeoff::on_active()
 void
 Takeoff::set_takeoff_position()
 {
+	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
+
+	float abs_altitude = 0.0f;
+
+	const float min_abs_altitude = _navigator->get_home_position()->alt + _param_min_alt.get();
+
+	// Use altitude if it has been set.
+	if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
+		abs_altitude = rep->current.alt;
+
+		// If the altitude suggestion is lower than home + minimum clearance, raise it and complain.
+		if (abs_altitude < min_abs_altitude) {
+			abs_altitude = min_abs_altitude;
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+					     "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
+		}
+	} else {
+		// Use home + minimum clearance but only notify.
+		abs_altitude = min_abs_altitude;
+		mavlink_log_info(_navigator->get_mavlink_log_pub(),
+				 "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
+	}
+
+
+	if (abs_altitude < _navigator->get_global_position()->alt) {
+		// If the suggestion is lower than our current alt, let's not go down.
+		abs_altitude = _navigator->get_global_position()->alt;
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+				     "Already higher than takeoff altitude");
+	}
+
 	// set current mission item to takeoff
-	set_takeoff_item(&_mission_item, _param_min_alt.get());
+	set_takeoff_item(&_mission_item, abs_altitude);
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->set_mission_result_updated();
@@ -114,12 +145,7 @@ Takeoff::set_takeoff_position()
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
-	// check if a specific target altitude has been set
-	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
 	if (rep->current.valid) {
-		if (PX4_ISFINITE(rep->current.alt)) {
-			pos_sp_triplet->current.alt = rep->current.alt;
-		}
 
 		// Go on and check which changes had been requested
 		if (PX4_ISFINITE(rep->current.yaw)) {