navigator: spaces/tabs fixed, old commented code removed

2 files changed, 36 insertions, 358 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 493e86d4a..e22c4d72d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -446,329 +446,7 @@ Navigator::status()
 		warnx("Geofence not set");
 	}
 }
-#if 0
-bool
-Navigator::start_none_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_none_in_air()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_auto_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_loiter()
-{
-	/* if no existing item available, use current position */
-	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
-
-		_pos_sp_triplet.current.lat = _global_pos.lat;
-		_pos_sp_triplet.current.lon = _global_pos.lon;
-		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-		_pos_sp_triplet.current.alt = _global_pos.alt;
-	}
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
-	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_pos_sp_triplet.current.loiter_direction = 1;
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_mission()
-{
-	/* start fresh */
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	return set_mission_items();
-}
-
-bool
-Navigator::advance_mission()
-{
-	/* tell mission to move by one */
-	_mission.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	return set_mission_items();
-}
-
-bool
-Navigator::set_mission_items()
-{
-	if (_pos_sp_triplet.current.valid) {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-		_pos_sp_triplet.previous.valid = true;
-	}
-
-	bool onboard;
-	int index;
-
-	/* if we fail to set the current mission, continue to loiter */
-	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
-
-		return false;
-	}
-
-	/* if we got an RTL mission item, switch to RTL mode and give up */
-	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		return false;
-	}
-
-	_mission_item_valid = true;
-
-	/* convert the current mission item and set it valid */
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-	_pos_sp_triplet.current.valid = true;
-
-
-	mission_item_s next_mission_item;
-
-	bool last_wp = false;
-	/* now try to set the next mission item as well, if there is no more next
-	 * this means we're heading to the last waypoint */
-	if (_mission.get_next_mission_item(&next_mission_item)) {
-		/* convert the next mission item and set it valid */
-		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
-		_pos_sp_triplet.next.valid = true;
-	} else {
-		last_wp = true;
-	}
-
-	/* notify user about what happened */
-	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
-		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
-
-	_update_triplet = true;
-
-	reset_reached();
-
-	return true;
-}
-
-bool
-Navigator::start_rtl()
-{
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	/* if RTL doesn't work, fallback to loiter */
-	return false;
-}
-
-bool
-Navigator::advance_rtl()
-{
-	/* tell mission to move by one */
-	_rtl.move_to_next();
 
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	return false;
-}
-
-bool
-Navigator::start_land()
-{
-	/* TODO: verify/test */
-
-	reset_reached();
-
-	/* this state can be requested by commander even if no global position available,
-	 * in his case controller must perform landing without position control */
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-	_mission_item.lat = _global_pos.lat;
-	_mission_item.lon = _global_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _global_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	_mission_item_valid = true;
-
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-bool
-Navigator::check_mission_item_reached()
-{
-	/* only check if there is actually a mission item to check */
-	if (!_mission_item_valid) {
-		return false;
-	}
-
-	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _vstatus.condition_landed;
-	}
-
-	/* XXX TODO count turns */
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-	// 	return false;
-	// }
-
-	uint64_t now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-		float acceptance_radius;
-
-		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
-			acceptance_radius = _mission_item.acceptance_radius;
-
-		} else {
-			acceptance_radius = _parameters.acceptance_radius;
-		}
-
-		if (_do_takeoff) {
-			/* require only altitude for takeoff */
-			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			float dist = -1.0f;
-			float dist_xy = -1.0f;
-			float dist_z = -1.0f;
-
-			/* calculate AMSL altitude for this waypoint */
-			float wp_alt_amsl = _mission_item.altitude;
-
-			if (_mission_item.altitude_is_relative)
-				wp_alt_amsl += _home_pos.alt;
-
-			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-					&dist_xy, &dist_z);
-
-			if (dist >= 0.0f && dist <= acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: 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) {
-
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-
-			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
-					(double)_mission_item.time_inside);
-			}
-		}
-
-		/* check if the MAV was long enough inside the waypoint orbit */
-		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
-		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			return true;
-		}
-	}
-	return false;
-}
-
-void
-Navigator::reset_reached()
-{
-	_time_first_inside_orbit = 0;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-
-}
-#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index a2eec8589..c5bb06c3b 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -79,7 +79,7 @@ RTL::on_inactive()
 
 	/* reset RTL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
-	    _rtl_state = RTL_STATE_NONE;
+		_rtl_state = RTL_STATE_NONE;
 	}
 }
 
@@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool updated = false;
 
 	if (_first_run) {
-        _first_run = false;
-
-	    /* decide where to enter the RTL procedure when we switch into it */
-	    if (_rtl_state == RTL_STATE_NONE) {
-	        /* for safety reasons don't go into RTL if landed */
-	        if (_navigator->get_vstatus()->condition_landed) {
-	            _rtl_state = RTL_STATE_FINISHED;
-	            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-
-	        /* if lower than return altitude, climb up first */
-	        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-	               + _param_return_alt.get()) {
-	            _rtl_state = RTL_STATE_CLIMB;
-
-	        /* otherwise go straight to return */
-	        } else {
-	            /* set altitude setpoint to current altitude */
-	            _rtl_state = RTL_STATE_RETURN;
-	            _mission_item.altitude_is_relative = false;
-	            _mission_item.altitude = _navigator->get_global_position()->alt;
-	        }
-	    }
+		_first_run = false;
+
+		/* decide where to enter the RTL procedure when we switch into it */
+		if (_rtl_state == RTL_STATE_NONE) {
+			/* for safety reasons don't go into RTL if landed */
+			if (_navigator->get_vstatus()->condition_landed) {
+				_rtl_state = RTL_STATE_FINISHED;
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+			/* if lower than return altitude, climb up first */
+			} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+				   + _param_return_alt.get()) {
+				_rtl_state = RTL_STATE_CLIMB;
+
+			/* otherwise go straight to return */
+			} else {
+				/* set altitude setpoint to current altitude */
+				_rtl_state = RTL_STATE_RETURN;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.altitude = _navigator->get_global_position()->alt;
+			}
+		}
 
 		set_rtl_item(pos_sp_triplet);
 
@@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-    if (_rtl_state == RTL_STATE_FINISHED) {
-        /* RTL finished, nothing to do */
-        pos_sp_triplet->next.valid = false;
-        mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-        return;
-    }
+	if (_rtl_state == RTL_STATE_FINISHED) {
+		/* RTL finished, nothing to do */
+		pos_sp_triplet->next.valid = false;
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
+		return;
+	}
 
-    set_previous_pos_setpoint(pos_sp_triplet);
-    _navigator->set_can_loiter_at_sp(false);
+	set_previous_pos_setpoint(pos_sp_triplet);
+	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-    /* convert mission item to current position setpoint and make it valid */
-    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-    reset_mission_item_reached();
-    pos_sp_triplet->current.valid = true;
-    pos_sp_triplet->next.valid = false;
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	reset_mission_item_reached();
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
 }
 
 void