navigator: takeoff and mission fixes

7 files changed, 166 insertions, 164 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index ee0aca69e..4f5e6d1a5 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1230,7 +1230,7 @@ FixedwingPositionControl::task_main()
 				}
 
 				/* XXX check if radius makes sense here */
-				float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius);
+				float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius);
 
 				/* lazily publish navigation capabilities */
 				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 2ff11e813..1adfeafde 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -143,7 +143,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 			mission_item->pitch_min = mavlink_mission_item->param2;
 			break;
 		default:
-			mission_item->radius = mavlink_mission_item->param2;
+			mission_item->acceptance_radius = mavlink_mission_item->param2;
 			break;
 	}
 
@@ -173,7 +173,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 			mavlink_mission_item->param2 = mission_item->pitch_min;
 			break;
 		default:
-			mavlink_mission_item->param2 = mission_item->radius;
+			mavlink_mission_item->param2 = mission_item->acceptance_radius;
 			break;
 	}
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 757eb4690..a6f33b5e7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -170,7 +170,8 @@ private:
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_force_takeoff;
+	bool		_need_takeoff;		/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
+	bool		_do_takeoff;		/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
 
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
@@ -178,16 +179,20 @@ private:
 
 	struct {
 		float min_altitude;
+		float acceptance_radius;
 		float loiter_radius;
 		int onboard_mission_enabled;
+		float takeoff_alt;
 		float land_alt;
 		float rtl_alt;
 	}		_parameters;			/**< local copies of parameters */
 
 	struct {
 		param_t min_altitude;
+		param_t acceptance_radius;
 		param_t loiter_radius;
 		param_t onboard_mission_enabled;
+		param_t takeoff_alt;
 		param_t land_alt;
 		param_t rtl_alt;
 	}		_parameter_handles;		/**< handles for parameters */
@@ -291,7 +296,12 @@ private:
 	/**
 	 * Check if current mission item has been reached.
 	 */
-	bool		mission_item_reached();
+	bool		check_mission_item_reached();
+
+	/**
+	 * Perform actions when current mission item reached.
+	 */
+	void		on_mission_item_reached();
 
 	/**
 	 * Move to next waypoint
@@ -379,13 +389,16 @@ Navigator::Navigator() :
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_set_nav_state_timestamp(0),
-	_force_takeoff(false)
+	_need_takeoff(true),
+	_do_takeoff(false)
 {
 	memset(&_fence, 0, sizeof(_fence));
 
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
+	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
+	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 	_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
 	_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
 
@@ -436,8 +449,10 @@ Navigator::parameters_update()
 	orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
 	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
+	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
 	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
 	param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
 	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
 
@@ -726,27 +741,8 @@ Navigator::task_main()
 			global_position_update();
 			/* only check if waypoint has been reached in MISSION */
 			if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
-				if (mission_item_reached()) {
-					if (myState == NAV_STATE_MISSION) {
-						if (_force_takeoff) {
-							/* forced takeoff completed */
-							_force_takeoff = false;
-							mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed");
-						} else {
-							/* advance by one mission item */
-							_mission.move_to_next();
-						}
-
-						/* if no more mission items available send this to state machine and start loiter at the last WP */
-						if (_mission.current_mission_available()) {
-							advance_mission();
-						} else {
-							dispatch(EVENT_MISSION_FINISHED);
-						}
-					} else {
-						/* RTL finished */
-						dispatch(EVENT_MISSION_FINISHED);
-					}
+				if (check_mission_item_reached()) {
+					on_mission_item_reached();
 				}
 			}
 		}
@@ -976,7 +972,7 @@ Navigator::start_none()
 
 	_reset_loiter_pos = true;
 	_rtl_done = false;
-	_force_takeoff = false;
+	_do_takeoff = false;
 
 	publish_mission_item_triplet();
 }
@@ -984,7 +980,7 @@ Navigator::start_none()
 void
 Navigator::start_loiter()
 {
-	_force_takeoff = false;
+	_do_takeoff = false;
 
 	/* set loiter position if needed */
 	if (_reset_loiter_pos || !_mission_item_triplet.current_valid) {
@@ -997,7 +993,7 @@ Navigator::start_loiter()
 		_mission_item_triplet.current.altitude_is_relative = false;
 		float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
 
-		/* Use current altitude if above min altitude set by parameter */
+		/* use current altitude if above min altitude set by parameter */
 		if (_global_pos.alt < min_alt_amsl) {
 			_mission_item_triplet.current.altitude = min_alt_amsl;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
@@ -1019,72 +1015,10 @@ Navigator::start_loiter()
 void
 Navigator::start_mission()
 {
-	int ret;
-	bool onboard;
-	unsigned index;
-
-	// TODO set prev item to current position?
-	_reset_loiter_pos = true;
 	_rtl_done = false;
-	_force_takeoff = false;
-
-	_mission_item_triplet.previous_valid = false;
-
-	ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
-
-	if (ret == OK) {
-		_mission_item_triplet.current_valid = true;
-		add_home_pos_to_rtl(&_mission_item_triplet.current);
-
-		if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) {
-			/* if landed, takeoff first, if this not defined in mission */
-			_force_takeoff = true;
-
-			/* move current mission item to next */
-			memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s));
-			_mission_item_triplet.next_valid = true;
+	_need_takeoff = true;
 
-			/* fill takeoff item */
-			get_takeoff_item(&_mission_item_triplet.current);
-			if (_vstatus.is_rotary_wing) {
-				/* for VTOL use current position */
-				_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
-				_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
-			} else {
-				/* for fixed wing use first waypoint position */
-				// TODO what if first mission item has no lat/lon?
-				_mission_item_triplet.current.lat = _mission_item_triplet.next.lat;
-				_mission_item_triplet.current.lon = _mission_item_triplet.next.lon;
-			}
-			_mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt;
-			_mission_item_triplet.current.altitude_is_relative = false;
-			mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude);
-		} else {
-			/* normal mission start: mission starts with takeoff item or vehicle is already in air */
-			ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
-
-			if (ret == OK) {
-				add_home_pos_to_rtl(&_mission_item_triplet.next);
-				_mission_item_triplet.next_valid = true;
-			} else {
-				/* this will fail for the last WP */
-				_mission_item_triplet.next_valid = false;
-			}
-
-			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index);
-			} else {
-				mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index);
-			}
-		}
-	} else {
-		/* mission is started without knowledge if there are more mission, indicates bug in navigator */
-		_mission_item_triplet.current_valid = false;
-		_mission_item_triplet.next_valid = false;
-		mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined");
-	}
-
-	publish_mission_item_triplet();
+	advance_mission();
 }
 
 void
@@ -1095,6 +1029,7 @@ Navigator::advance_mission()
 	_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
 	_reset_loiter_pos = true;
+	_do_takeoff = false;
 
 	int ret;
 	bool onboard;
@@ -1103,14 +1038,54 @@ Navigator::advance_mission()
 	ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
 
 	if (ret == OK) {
-
-		add_home_pos_to_rtl(&_mission_item_triplet.current);
 		_mission_item_triplet.current_valid = true;
+		add_home_pos_to_rtl(&_mission_item_triplet.current);
+
+		if (_vstatus.is_rotary_wing) {
+			if (_need_takeoff && (
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
+				)) {
+				/* do special TAKEOFF handling for VTOL */
+				_need_takeoff = false;
+
+				/* check if we really need takeoff */
+				float wp_alt_amsl = _mission_item_triplet.current.altitude;
+				if (_mission_item_triplet.current.altitude_is_relative)
+					wp_alt_amsl += _home_pos.altitude;
+				if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt ||
+						(_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) {
+					/* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */
+					_do_takeoff = true;
+
+					/* move current mission item to next */
+					memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s));
+					_mission_item_triplet.next_valid = true;
+
+					/* set current item to TAKEOFF */
+					get_takeoff_item(&_mission_item_triplet.current);
+
+					_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
+					_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
+				}
+			} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
+				/* will need takeoff after landing */
+				_need_takeoff = true;
+			}
+		}
 
-		if (onboard) {
-			mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+		if (_do_takeoff) {
+			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude);
 		} else {
-			mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+			if (onboard) {
+				mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+			} else {
+				mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+			}
 		}
 	} else {
 		/* since a mission is not advanced without WPs available, this is not supposed to happen */
@@ -1118,16 +1093,16 @@ Navigator::advance_mission()
 		warnx("ERROR: current WP can't be set");
 	}
 
-	ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
-
-	if (ret == OK) {
-
-		add_home_pos_to_rtl(&_mission_item_triplet.next);
+	if (!_do_takeoff) {
+		ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
 
-		_mission_item_triplet.next_valid = true;
-	} else {
-		/* this will fail for the last WP */
-		_mission_item_triplet.next_valid = false;
+		if (ret == OK) {
+			add_home_pos_to_rtl(&_mission_item_triplet.next);
+			_mission_item_triplet.next_valid = true;
+		} else {
+			/* this will fail for the last WP */
+			_mission_item_triplet.next_valid = false;
+		}
 	}
 
 	publish_mission_item_triplet();
@@ -1149,6 +1124,7 @@ Navigator::start_rtl()
 {
 	_reset_loiter_pos = true;
 	_rtl_done = false;
+	_do_takeoff = false;
 
 	/* discard all mission item and insert RTL item */
 	_mission_item_triplet.previous_valid = false;
@@ -1157,12 +1133,13 @@ Navigator::start_rtl()
 
 	_mission_item_triplet.current.lat = _home_pos.lat;
 	_mission_item_triplet.current.lon = _home_pos.lon;
-	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
+	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt;
+	_mission_item_triplet.current.altitude_is_relative = false;
 	_mission_item_triplet.current.yaw = 0.0f;	// TODO use heading to waypoint?
 	_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
 	_mission_item_triplet.current.loiter_direction = 1;
 	_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
+	_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
 	_mission_item_triplet.current.autocontinue = false;
 	_mission_item_triplet.current_valid = true;
 
@@ -1184,7 +1161,7 @@ Navigator::finish_rtl()
 }
 
 bool
-Navigator::mission_item_reached()
+Navigator::check_mission_item_reached()
 {
 	/* only check if there is actually a mission item to check */
 	if (!_mission_item_triplet.current_valid) {
@@ -1206,22 +1183,15 @@ Navigator::mission_item_reached()
 	}	
 
 	uint64_t now = hrt_absolute_time();
-	float orbit;
+	float acceptance_radius;
 
-	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) {
-
-		orbit = _mission_item_triplet.current.radius;
+	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) {
+		acceptance_radius = _mission_item_triplet.current.acceptance_radius;
 
 	} else {
-
-		// XXX set default orbit via param
-		orbit = 15.0f;
+		acceptance_radius = _parameters.acceptance_radius;
 	}
 
-	/* keep vertical orbit */
-	float vertical_switch_distance = orbit;
-
-
 	// TODO add frame
 	// int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
 	
@@ -1232,10 +1202,12 @@ Navigator::mission_item_reached()
 	// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
 
 	/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
-	float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt;
+	float wp_alt_amsl = _mission_item_triplet.current.altitude;
+	if (_mission_item_triplet.current.altitude_is_relative)
+		_mission_item_triplet.current.altitude += _home_pos.altitude;
 
-	dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude,
-		                              (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt,
+	dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl,
+		                              (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
 		                              &dist_xy, &dist_z);
 
 	// warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude);
@@ -1254,25 +1226,29 @@ Navigator::mission_item_reached()
 	// 	// XXX TODO
 	// }
 
-	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
-		/* require only altitude for takeoff */
-		if (current_alt >= _mission_item_triplet.current.altitude)
+	if (_do_takeoff) {
+		if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
+			/* require only altitude for takeoff */
 			_waypoint_position_reached = true;
+		}
 	} else {
-		if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
+		if (dist >= 0.0f && dist <= acceptance_radius) {
 			_waypoint_position_reached = true;
 		}
 	}
 
-	/* check if required yaw reached */
-	float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
-
-	if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+	if (_vstatus.is_rotary_wing && !_do_takeoff) {
+		/* check yow only for rotary wing except takeoff */
+		float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
+		if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			_waypoint_yaw_reached = true;
+		}
+	} else {
 		_waypoint_yaw_reached = true;
 	}
 
 	/* check if the current waypoint was reached */
-	if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 		
 		if (_time_first_inside_orbit == 0) {
 			/* XXX announcement? */
@@ -1294,19 +1270,44 @@ Navigator::mission_item_reached()
 }
 
 void
+Navigator::on_mission_item_reached()
+{
+	if (myState == NAV_STATE_MISSION) {
+		if (_do_takeoff) {
+			/* takeoff completed */
+			_do_takeoff = false;
+			mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+		} else {
+			/* advance by one mission item */
+			_mission.move_to_next();
+		}
+
+		if (_mission.current_mission_available()) {
+			advance_mission();
+		} else {
+			/* if no more mission items available then finish mission */
+			dispatch(EVENT_MISSION_FINISHED);
+		}
+	} else {
+		/* RTL finished */
+		dispatch(EVENT_MISSION_FINISHED);
+	}
+}
+
+void
 Navigator::get_loiter_item(struct mission_item_s *item)
 {
 	//item->altitude_is_relative
 	//item->lat
 	//item->lon
 	//item->altitude
-	item->yaw = NAN;
-	item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+	//item->yaw
+	item->loiter_radius = _parameters.loiter_radius;
 	item->loiter_direction = 1;
 	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-	item->radius = 50.0f; // TODO: get rid of magic number
-	//item->time_inside
-	//item->pitch_min
+	item->acceptance_radius = _parameters.acceptance_radius;
+	item->time_inside = 0.0f;
+	item->pitch_min = 0.0f;
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
 
@@ -1320,17 +1321,31 @@ Navigator::get_takeoff_item(mission_item_s *item)
 	//item->lon
 	//item->altitude
 	item->yaw = NAN;
-	item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+	item->loiter_radius = _parameters.loiter_radius;
 	item->loiter_direction = 1;
 	item->nav_cmd = NAV_CMD_TAKEOFF;
-	item->radius = 50.0f; // TODO: get rid of magic number
+	item->acceptance_radius = _parameters.acceptance_radius;
 	item->time_inside = 0.0f;
-	item->pitch_min = 0.3; // TODO: get rid of magic number
+	item->pitch_min = 0.0;
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
 }
 
 void
+Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
+{
+	if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		/* append the home position to RTL item */
+		new_mission_item->lat = _home_pos.lat;
+		new_mission_item->lon = _home_pos.lon;
+		new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->loiter_radius = _parameters.loiter_radius;
+		new_mission_item->acceptance_radius = _parameters.acceptance_radius;
+	}
+}
+
+void
 Navigator::publish_mission_item_triplet()
 {
 	/* lazily publish the mission triplet only once available */
@@ -1424,7 +1439,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
 	    fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
 	    a.loiter_direction == b.loiter_direction &&
 	    a.nav_cmd == b.nav_cmd &&
-	    fabsf(a.radius - b.radius) < FLT_EPSILON &&
+	    fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON &&
 	    fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
 	    a.autocontinue == b.autocontinue) {
 		return true;
@@ -1485,16 +1500,3 @@ int navigator_main(int argc, char *argv[])
 
 	return 0;
 }
-
-void
-Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
-{
-	if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		/* if it is a RTL waypoint, append the home position */
-		new_mission_item->lat = _home_pos.lat;
-		new_mission_item->lon = _home_pos.lon;
-		new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude;
-		new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
-		new_mission_item->radius = 50.0f; // TODO: get rid of magic number
-	}
-}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 63601ce6d..8df47fc3b 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -52,7 +52,9 @@
  */
 
 PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
+PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
 PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f);	// default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f);		// min altitude for going home in RTL mode
+PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
+PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
+PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 78322aff3..6833ec43f 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1161,9 +1161,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;				
 				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
 				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
-				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-				log_msg.body.log_GPSP.radius = buf.triplet.current.radius;
+				log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius;
 				log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
 				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index cb1393f1f..3afaaa2ad 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -217,7 +217,7 @@ struct log_GPSP_s {
 	uint8_t nav_cmd;
 	float loiter_radius;
 	int8_t loiter_direction;
-	float radius;
+	float acceptance_radius;
 	float time_inside;
 	float pitch_min;
 };
@@ -287,7 +287,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"),
+	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 95bceb8bb..194e2ed0c 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -87,7 +87,7 @@ struct mission_item_s
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
 	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
 	enum NAV_CMD nav_cmd;		/**< navigation command					*/
-	float radius;			/**< radius in which the mission is accepted as reached in meters */
+	float acceptance_radius;	/**< default radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */