<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_mission.cpp
 *
 * Helper class to access missions
 *
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 */

#include "mission.h"
#include "navigator.h"

#include <string.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <lib/ecl/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>

Mission::Mission(Navigator *navigator) :
	MissionBlock(navigator),
	ModuleParams(navigator)
{
}

void
Mission::on_inactive()
{
	/* We need to reset the mission cruising speed, otherwise the
	 * mission velocity which might have been set using mission items
	 * is used for missions such as RTL. */
	_navigator->set_cruising_speed();

	/* Without home a mission can't be valid yet anyway, let's wait. */
	if (!_navigator->home_position_valid()) {
		return;
	}

	if (_inited) {
		if (_mission_sub.updated()) {
			update_mission();

			if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
				_mission_type = MISSION_TYPE_MISSION;
			}
		}

		/* reset the current mission if needed */
		if (need_to_reset_mission(false)) {
			reset_mission(_mission);
			update_mission();
			_navigator->reset_cruising_speed();
		}

	} else {

		/* load missions from storage */
		mission_s mission_state = {};

		dm_lock(DM_KEY_MISSION_STATE);

		/* read current state */
		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));

		dm_unlock(DM_KEY_MISSION_STATE);

		if (read_res == sizeof(mission_s)) {
			_mission.dataman_id = mission_state.dataman_id;
			_mission.count = mission_state.count;
			_current_mission_index = mission_state.current_seq;

			// find and store landing start marker (if available)
			find_mission_land_start();
		}

		/* On init let's check the mission, maybe there is already one available. */
		check_mission_valid(false);

		_inited = true;
	}

	/* require takeoff after non-loiter or landing */
	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
		_need_takeoff = true;
	}

	/* reset so current mission item gets restarted if mission was paused */
	_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	/* reset so MISSION_ITEM_REACHED isn't published */
	_navigator->get_mission_result()->seq_reached = -1;
}

void
Mission::on_inactivation()
{
	// Disable camera trigger
	vehicle_command_s cmd = {};
	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
	// Pause trigger
	cmd.param1 = -1.0f;
	cmd.param3 = 1.0f;
	_navigator->publish_vehicle_cmd(&cmd);
}

void
Mission::on_activation()
{
	if (_mission_waypoints_changed) {
		// do not set the closest mission item in the normal mission mode
		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
			_current_mission_index = index_closest_mission_item();
		}

		_mission_waypoints_changed = false;
	}

	// we already reset the mission items
	_execution_mode_changed = false;

	set_mission_items();

	// unpause triggering if it was paused
	vehicle_command_s cmd = {};
	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
	// unpause trigger
	cmd.param1 = -1.0f;
	cmd.param3 = 0.0f;
	_navigator->publish_vehicle_cmd(&cmd);
}

void
Mission::on_active()
{
	check_mission_valid(false);

	/* check if anything has changed */
	bool mission_sub_updated = _mission_sub.updated();

	if (mission_sub_updated) {
		update_mission();
	}

	/* reset the current mission if needed */
	if (need_to_reset_mission(true)) {
		reset_mission(_mission);
		update_mission();
		_navigator->reset_cruising_speed();
		mission_sub_updated = true;
	}

	_mission_changed = false;

	/* reset mission items if needed */
	if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) {
		if (_mission_waypoints_changed) {
			// do not set the closest mission item in the normal mission mode
			if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
				_current_mission_index = index_closest_mission_item();
			}

			_mission_waypoints_changed = false;
		}

		_execution_mode_changed = false;
		set_mission_items();
	}

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		/* If we just completed a takeoff which was inserted before the right waypoint,
		   there is no need to report that we reached it because we didn't. */
		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
			set_mission_item_reached();
		}

		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
		}

	} else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) {

		altitude_sp_foh_update();

	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
		}
	}

	/* check if a cruise speed change has been commanded */
	if (_mission_type != MISSION_TYPE_NONE) {
		cruising_speed_sp_update();
	}

	/* see if we need to update the current yaw heading */
	if (!_param_mis_mnt_yaw_ctl.get()
	    && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
	    && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
	    && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
		 || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
		 || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
		 || _mission_item.nav_cmd == NAV_CMD_LAND
		 || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
		 || _work_item_type == WORK_ITEM_TYPE_ALIGN)) {
		// Mount control is disabled If the vehicle is in ROI-mode, the vehicle
		// needs to rotate such that ROI is in the field of view.
		// ROI only makes sense for multicopters.
		heading_sp_update();
	}

	// TODO: Add vtol heading update method if required.
	// Question: Why does vtol ever have to update heading?

	/* check if landing needs to be aborted */
	if ((_mission_item.nav_cmd == NAV_CMD_LAND)
	    && (_navigator->abort_landing())) {

		do_abort_landing();
	}

	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
		// switch out of precision land once landed
		if (_navigator->get_land_detected()->landed) {
			_navigator->get_precland()->on_inactivation();
			_work_item_type = WORK_ITEM_TYPE_DEFAULT;

		} else {
			_navigator->get_precland()->on_active();
		}
	}
}

bool
Mission::set_current_mission_index(uint16_t index)
{
	if (_navigator->get_mission_result()->valid &&
	    (index != _current_mission_index) && (index < _mission.count)) {

		_current_mission_index = index;

		// a mission index is set manually which has the higher priority than the closest mission item
		// as it is set by the user
		_mission_waypoints_changed = false;

		// update mission items if already in active mission
		if (_navigator->is_planned_mission()) {
			// prevent following "previous - current" line
			_navigator->get_position_setpoint_triplet()->previous.valid = false;
			_navigator->get_position_setpoint_triplet()->current.valid = false;
			_navigator->get_position_setpoint_triplet()->next.valid = false;
			set_mission_items();
		}

		return true;
	}

	return false;
}

void
Mission::set_closest_item_as_current()
{
	_current_mission_index = index_closest_mission_item();
}

void
Mission::set_execution_mode(const uint8_t mode)
{
	if (_mission_execution_mode != mode) {
		_execution_mode_changed = true;
		_navigator->get_mission_result()->execution_mode = mode;


		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD:
			if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
				// command a transition if in vtol mc mode
				if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
				    _navigator->get_vstatus()->is_vtol &&
				    !_navigator->get_land_detected()->landed) {

					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);

					position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
					pos_sp_triplet->previous = pos_sp_triplet->current;
					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
					_navigator->set_position_setpoint_triplet_updated();
					issue_command(_mission_item);
				}

				if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
					_mission_type = MISSION_TYPE_MISSION;
				}

				if (_current_mission_index > _mission.count - 1) {
					_current_mission_index = _mission.count - 1;

				} else if (_current_mission_index > 0) {
					--_current_mission_index;
				}

				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			}

			break;

		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE:
			if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) ||
			    (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) {
				// handle switch from reverse to forward mission
				if (_current_mission_index < 0) {
					_current_mission_index = 0;

				} else if (_current_mission_index < _mission.count - 1) {
					++_current_mission_index;
				}

				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			}

			break;

		}

		_mission_execution_mode = mode;
	}
}

bool
Mission::find_mission_land_start()
{
	/* return true if a MAV_CMD_DO_LAND_START is found and internally save the index
	 *  return false if not found
	 *
	 * TODO: implement full spec and find closest landing point geographically
	 */

	const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;

	for (size_t i = 0; i < _mission.count; i++) {
		struct mission_item_s missionitem = {};
		const ssize_t len = sizeof(missionitem);

		if (dm_read(dm_current, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			PX4_ERR("dataman read failure");
			break;
		}

		if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) ||
		    ((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
		    (missionitem.nav_cmd == NAV_CMD_LAND)) {
			_land_start_available = true;
			_land_start_index = i;
			return true;
		}
	}

	_land_start_available = false;
	return false;
}

bool
Mission::land_start()
{
	// if not currently landing, jump to do_land_start
	if (_land_start_available) {
		if (landing()) {
			return true;

		} else {
			set_current_mission_index(get_land_start_index());
			return landing();
		}
	}

	return false;
}

bool
Mission::landing()
{
	// vehicle is currently landing if
	//  mission valid, still flying, and in the landing portion of mission

	const bool mission_valid = _navigator->get_mission_result()->valid;
	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());

	return mission_valid && on_landing_stage;
}

void
Mission::update_mission()
{

	bool failed = true;

	/* reset triplets */
	_navigator->reset_triplets();

	/* Reset vehicle_roi
	 * Missions that do not explicitly configure ROI would not override
	 * an existing ROI setting from previous missions */
	_navigator->reset_vroi();

	const mission_s old_mission = _mission;

	if (_mission_sub.copy(&_mission)) {
		/* determine current index */
		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
			_current_mission_index = _mission.current_seq;

		} else {
			/* if less items available, reset to first item */
			if (_current_mission_index >= (int)_mission.count) {
				_current_mission_index = 0;

			} else if (_current_mission_index < 0) {
				/* if not initialized, set it to 0 */
				_current_mission_index = 0;
			}

			/* otherwise, just leave it */
		}

		check_mission_valid(true);

		failed = !_navigator->get_mission_result()->valid;

		if (!failed) {
			/* reset mission failure if we have an updated valid mission */
			_navigator->get_mission_result()->failure = false;

			/* reset sequence info as well */
			_navigator->get_mission_result()->seq_reached = -1;
			_navigator->get_mission_result()->seq_total = _mission.count;

			/* reset work item if new mission has been accepted */
			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			_mission_changed = true;
		}

		/* check if the mission waypoints changed while the vehicle is in air
		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
		if (((_mission.count != old_mission.count) ||
		     (_mission.dataman_id != old_mission.dataman_id)) &&
		    !_navigator->get_land_detected()->landed) {
			_mission_waypoints_changed = true;
		}

	} else {
		PX4_ERR("mission update failed");
	}

	if (failed) {
		_mission.count = 0;
		_mission.current_seq = 0;
		_current_mission_index = 0;

		PX4_ERR("mission check failed");
	}

	// find and store landing start marker (if available)
	find_mission_land_start();

	set_current_mission_item();
}


void
Mission::advance_mission()
{
	/* do not advance mission item if we're processing sub mission work items */
	if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) {
		return;
	}

	switch (_mission_type) {
	case MISSION_TYPE_MISSION:
		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
				_current_mission_index++;
				break;
			}

		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
				// find next position item in reverse order
				dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);

				for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
					struct mission_item_s missionitem = {};
					const ssize_t len = sizeof(missionitem);

					if (dm_read(dm_current, i, &missionitem, len) != len) {
						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
						PX4_ERR("dataman read failure");
						break;
					}

					if (item_contains_position(missionitem)) {
						_current_mission_index = i;
						return;
					}
				}

				// finished flying back the mission
				_current_mission_index = -1;
				break;
			}

		default:
			_current_mission_index++;
		}

		break;

	case MISSION_TYPE_NONE:
	default:
		break;
	}
}

void
Mission::set_mission_items()
{
	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
	_min_current_sp_distance_xy = FLT_MAX;

	/* the home dist check provides user feedback, so we initialize it to this */
	bool user_feedback_done = false;

	/* mission item that comes after current if available */
	struct mission_item_s mission_item_next_position;
	bool has_next_position_item = false;

	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_MISSION) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(),
					 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission" :
					 "Executing Mission");
			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_MISSION;

	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {

			if (_navigator->get_land_detected()->landed) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(),
						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" :
						 "Mission finished, landed.");

			} else {
				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
				mavlink_log_info(_navigator->get_mavlink_log_pub(),
						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering" :
						 "Mission finished, loitering.");

				/* use last setpoint for loiter */
				_navigator->set_can_loiter_at_sp(true);
			}

			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());

		/* update position setpoint triplet  */
		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
		pos_sp_triplet->previous.valid = false;
		mission_apply_limitation(_mission_item);
		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
		pos_sp_triplet->next.valid = false;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		// set mission finished
		_navigator->get_mission_result()->finished = true;
		_navigator->set_mission_result_updated();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			if (_navigator->get_land_detected()->landed) {
				/* landed, refusing to take off without a mission */
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff.");

			} else {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering.");
			}

			user_feedback_done = true;
		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	/*********************************** handle mission item *********************************************/

	/* handle mission items depending on the mode */

	const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current;

	if (item_contains_position(_mission_item)) {
		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
				/* force vtol land */
				if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
					_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
				}

				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

				// allow weather vane in mission
				pos_sp_triplet->current.allow_weather_vane = true;

				/* do takeoff before going to setpoint if needed and not already in takeoff */
				/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
				if (do_need_vertical_takeoff() &&
				    _work_item_type == WORK_ITEM_TYPE_DEFAULT &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

					/* use current mission item as next position item */
					mission_item_next_position = _mission_item;
					mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
					has_next_position_item = true;

					float takeoff_alt = calculate_takeoff_altitude(&_mission_item);

					mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home.",
							 (double)(takeoff_alt - _navigator->get_home_position()->alt));

					_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
					_mission_item.lat = _navigator->get_global_position()->lat;
					_mission_item.lon = _navigator->get_global_position()->lon;
					/* hold heading for takeoff items */
					_mission_item.yaw = _navigator->get_global_position()->yaw;
					_mission_item.altitude = takeoff_alt;
					_mission_item.altitude_is_relative = false;
					_mission_item.autocontinue = true;
					_mission_item.time_inside = 0.0f;

				} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {

					/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
					 * Note also that resetting time_inside to zero will cause pitch_min to be zero as well.
					 */
					_mission_item.time_inside = 0.0f;

				} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
						/* haven't transitioned yet, trigger vtol takeoff logic below */
						_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

					} else {
						/* already in fixed-wing, go to waypoint */
						_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					}

					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
				}

				/* if we just did a normal takeoff navigate to the actual waypoint now */
				if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF &&
				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
					 */
					_mission_item.time_inside = 0.0f;
				}

				/* if we just did a VTOL takeoff, prepare transition */
				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
				    !_navigator->get_land_detected()->landed) {

					/* disable weathervane before front transition for allowing yaw to align */
				    !_navigator->get_land_detected()->landed) {

					/* check if the vtol_takeoff waypoint is on top of us */
					if (do_need_move_to_takeoff()) {
						new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
					}

					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);

					/* set position setpoint to target during the transition */
					// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
				}

				/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
				    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {<commit_msg>Navigator: VTOL: set heading of transition of a VTOL_TAKEOFF to the VTOL_TAKEOFF waypoint (and enforce heading)<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_mission.cpp
 *
 * Helper class to access missions
 *
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 */

#include "mission.h"
#include "navigator.h"

#include <string.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <lib/ecl/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>

Mission::Mission(Navigator *navigator) :
	MissionBlock(navigator),
	ModuleParams(navigator)
{
}

void
Mission::on_inactive()
{
	/* We need to reset the mission cruising speed, otherwise the
	 * mission velocity which might have been set using mission items
	 * is used for missions such as RTL. */
	_navigator->set_cruising_speed();

	/* Without home a mission can't be valid yet anyway, let's wait. */
	if (!_navigator->home_position_valid()) {
		return;
	}

	if (_inited) {
		if (_mission_sub.updated()) {
			update_mission();

			if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
				_mission_type = MISSION_TYPE_MISSION;
			}
		}

		/* reset the current mission if needed */
		if (need_to_reset_mission(false)) {
			reset_mission(_mission);
			update_mission();
			_navigator->reset_cruising_speed();
		}

	} else {

		/* load missions from storage */
		mission_s mission_state = {};

		dm_lock(DM_KEY_MISSION_STATE);

		/* read current state */
		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));

		dm_unlock(DM_KEY_MISSION_STATE);

		if (read_res == sizeof(mission_s)) {
			_mission.dataman_id = mission_state.dataman_id;
			_mission.count = mission_state.count;
			_current_mission_index = mission_state.current_seq;

			// find and store landing start marker (if available)
			find_mission_land_start();
		}

		/* On init let's check the mission, maybe there is already one available. */
		check_mission_valid(false);

		_inited = true;
	}

	/* require takeoff after non-loiter or landing */
	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
		_need_takeoff = true;
	}

	/* reset so current mission item gets restarted if mission was paused */
	_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	/* reset so MISSION_ITEM_REACHED isn't published */
	_navigator->get_mission_result()->seq_reached = -1;
}

void
Mission::on_inactivation()
{
	// Disable camera trigger
	vehicle_command_s cmd = {};
	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
	// Pause trigger
	cmd.param1 = -1.0f;
	cmd.param3 = 1.0f;
	_navigator->publish_vehicle_cmd(&cmd);
}

void
Mission::on_activation()
{
	if (_mission_waypoints_changed) {
		// do not set the closest mission item in the normal mission mode
		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
			_current_mission_index = index_closest_mission_item();
		}

		_mission_waypoints_changed = false;
	}

	// we already reset the mission items
	_execution_mode_changed = false;

	set_mission_items();

	// unpause triggering if it was paused
	vehicle_command_s cmd = {};
	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
	// unpause trigger
	cmd.param1 = -1.0f;
	cmd.param3 = 0.0f;
	_navigator->publish_vehicle_cmd(&cmd);
}

void
Mission::on_active()
{
	check_mission_valid(false);

	/* check if anything has changed */
	bool mission_sub_updated = _mission_sub.updated();

	if (mission_sub_updated) {
		update_mission();
	}

	/* reset the current mission if needed */
	if (need_to_reset_mission(true)) {
		reset_mission(_mission);
		update_mission();
		_navigator->reset_cruising_speed();
		mission_sub_updated = true;
	}

	_mission_changed = false;

	/* reset mission items if needed */
	if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) {
		if (_mission_waypoints_changed) {
			// do not set the closest mission item in the normal mission mode
			if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
				_current_mission_index = index_closest_mission_item();
			}

			_mission_waypoints_changed = false;
		}

		_execution_mode_changed = false;
		set_mission_items();
	}

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		/* If we just completed a takeoff which was inserted before the right waypoint,
		   there is no need to report that we reached it because we didn't. */
		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
			set_mission_item_reached();
		}

		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
		}

	} else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) {

		altitude_sp_foh_update();

	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
		}
	}

	/* check if a cruise speed change has been commanded */
	if (_mission_type != MISSION_TYPE_NONE) {
		cruising_speed_sp_update();
	}

	/* see if we need to update the current yaw heading */
	if (!_param_mis_mnt_yaw_ctl.get()
	    && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
	    && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
	    && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
		 || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
		 || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
		 || _mission_item.nav_cmd == NAV_CMD_LAND
		 || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
		 || _work_item_type == WORK_ITEM_TYPE_ALIGN)) {
		// Mount control is disabled If the vehicle is in ROI-mode, the vehicle
		// needs to rotate such that ROI is in the field of view.
		// ROI only makes sense for multicopters.
		heading_sp_update();
	}

	// TODO: Add vtol heading update method if required.
	// Question: Why does vtol ever have to update heading?

	/* check if landing needs to be aborted */
	if ((_mission_item.nav_cmd == NAV_CMD_LAND)
	    && (_navigator->abort_landing())) {

		do_abort_landing();
	}

	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
		// switch out of precision land once landed
		if (_navigator->get_land_detected()->landed) {
			_navigator->get_precland()->on_inactivation();
			_work_item_type = WORK_ITEM_TYPE_DEFAULT;

		} else {
			_navigator->get_precland()->on_active();
		}
	}
}

bool
Mission::set_current_mission_index(uint16_t index)
{
	if (_navigator->get_mission_result()->valid &&
	    (index != _current_mission_index) && (index < _mission.count)) {

		_current_mission_index = index;

		// a mission index is set manually which has the higher priority than the closest mission item
		// as it is set by the user
		_mission_waypoints_changed = false;

		// update mission items if already in active mission
		if (_navigator->is_planned_mission()) {
			// prevent following "previous - current" line
			_navigator->get_position_setpoint_triplet()->previous.valid = false;
			_navigator->get_position_setpoint_triplet()->current.valid = false;
			_navigator->get_position_setpoint_triplet()->next.valid = false;
			set_mission_items();
		}

		return true;
	}

	return false;
}

void
Mission::set_closest_item_as_current()
{
	_current_mission_index = index_closest_mission_item();
}

void
Mission::set_execution_mode(const uint8_t mode)
{
	if (_mission_execution_mode != mode) {
		_execution_mode_changed = true;
		_navigator->get_mission_result()->execution_mode = mode;


		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD:
			if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
				// command a transition if in vtol mc mode
				if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
				    _navigator->get_vstatus()->is_vtol &&
				    !_navigator->get_land_detected()->landed) {

					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);

					position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
					pos_sp_triplet->previous = pos_sp_triplet->current;
					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
					_navigator->set_position_setpoint_triplet_updated();
					issue_command(_mission_item);
				}

				if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
					_mission_type = MISSION_TYPE_MISSION;
				}

				if (_current_mission_index > _mission.count - 1) {
					_current_mission_index = _mission.count - 1;

				} else if (_current_mission_index > 0) {
					--_current_mission_index;
				}

				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			}

			break;

		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE:
			if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) ||
			    (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) {
				// handle switch from reverse to forward mission
				if (_current_mission_index < 0) {
					_current_mission_index = 0;

				} else if (_current_mission_index < _mission.count - 1) {
					++_current_mission_index;
				}

				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			}

			break;

		}

		_mission_execution_mode = mode;
	}
}

bool
Mission::find_mission_land_start()
{
	/* return true if a MAV_CMD_DO_LAND_START is found and internally save the index
	 *  return false if not found
	 *
	 * TODO: implement full spec and find closest landing point geographically
	 */

	const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;

	for (size_t i = 0; i < _mission.count; i++) {
		struct mission_item_s missionitem = {};
		const ssize_t len = sizeof(missionitem);

		if (dm_read(dm_current, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			PX4_ERR("dataman read failure");
			break;
		}

		if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) ||
		    ((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
		    (missionitem.nav_cmd == NAV_CMD_LAND)) {
			_land_start_available = true;
			_land_start_index = i;
			return true;
		}
	}

	_land_start_available = false;
	return false;
}

bool
Mission::land_start()
{
	// if not currently landing, jump to do_land_start
	if (_land_start_available) {
		if (landing()) {
			return true;

		} else {
			set_current_mission_index(get_land_start_index());
			return landing();
		}
	}

	return false;
}

bool
Mission::landing()
{
	// vehicle is currently landing if
	//  mission valid, still flying, and in the landing portion of mission

	const bool mission_valid = _navigator->get_mission_result()->valid;
	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());

	return mission_valid && on_landing_stage;
}

void
Mission::update_mission()
{

	bool failed = true;

	/* reset triplets */
	_navigator->reset_triplets();

	/* Reset vehicle_roi
	 * Missions that do not explicitly configure ROI would not override
	 * an existing ROI setting from previous missions */
	_navigator->reset_vroi();

	const mission_s old_mission = _mission;

	if (_mission_sub.copy(&_mission)) {
		/* determine current index */
		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
			_current_mission_index = _mission.current_seq;

		} else {
			/* if less items available, reset to first item */
			if (_current_mission_index >= (int)_mission.count) {
				_current_mission_index = 0;

			} else if (_current_mission_index < 0) {
				/* if not initialized, set it to 0 */
				_current_mission_index = 0;
			}

			/* otherwise, just leave it */
		}

		check_mission_valid(true);

		failed = !_navigator->get_mission_result()->valid;

		if (!failed) {
			/* reset mission failure if we have an updated valid mission */
			_navigator->get_mission_result()->failure = false;

			/* reset sequence info as well */
			_navigator->get_mission_result()->seq_reached = -1;
			_navigator->get_mission_result()->seq_total = _mission.count;

			/* reset work item if new mission has been accepted */
			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			_mission_changed = true;
		}

		/* check if the mission waypoints changed while the vehicle is in air
		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
		if (((_mission.count != old_mission.count) ||
		     (_mission.dataman_id != old_mission.dataman_id)) &&
		    !_navigator->get_land_detected()->landed) {
			_mission_waypoints_changed = true;
		}

	} else {
		PX4_ERR("mission update failed");
	}

	if (failed) {
		_mission.count = 0;
		_mission.current_seq = 0;
		_current_mission_index = 0;

		PX4_ERR("mission check failed");
	}

	// find and store landing start marker (if available)
	find_mission_land_start();

	set_current_mission_item();
}


void
Mission::advance_mission()
{
	/* do not advance mission item if we're processing sub mission work items */
	if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) {
		return;
	}

	switch (_mission_type) {
	case MISSION_TYPE_MISSION:
		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
				_current_mission_index++;
				break;
			}

		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
				// find next position item in reverse order
				dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);

				for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
					struct mission_item_s missionitem = {};
					const ssize_t len = sizeof(missionitem);

					if (dm_read(dm_current, i, &missionitem, len) != len) {
						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
						PX4_ERR("dataman read failure");
						break;
					}

					if (item_contains_position(missionitem)) {
						_current_mission_index = i;
						return;
					}
				}

				// finished flying back the mission
				_current_mission_index = -1;
				break;
			}

		default:
			_current_mission_index++;
		}

		break;

	case MISSION_TYPE_NONE:
	default:
		break;
	}
}

void
Mission::set_mission_items()
{
	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
	_min_current_sp_distance_xy = FLT_MAX;

	/* the home dist check provides user feedback, so we initialize it to this */
	bool user_feedback_done = false;

	/* mission item that comes after current if available */
	struct mission_item_s mission_item_next_position;
	bool has_next_position_item = false;

	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_MISSION) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(),
					 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission" :
					 "Executing Mission");
			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_MISSION;

	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {

			if (_navigator->get_land_detected()->landed) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(),
						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" :
						 "Mission finished, landed.");

			} else {
				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
				mavlink_log_info(_navigator->get_mavlink_log_pub(),
						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering" :
						 "Mission finished, loitering.");

				/* use last setpoint for loiter */
				_navigator->set_can_loiter_at_sp(true);
			}

			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());

		/* update position setpoint triplet  */
		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
		pos_sp_triplet->previous.valid = false;
		mission_apply_limitation(_mission_item);
		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
		pos_sp_triplet->next.valid = false;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		// set mission finished
		_navigator->get_mission_result()->finished = true;
		_navigator->set_mission_result_updated();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			if (_navigator->get_land_detected()->landed) {
				/* landed, refusing to take off without a mission */
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff.");

			} else {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering.");
			}

			user_feedback_done = true;
		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	/*********************************** handle mission item *********************************************/

	/* handle mission items depending on the mode */

	const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current;

	if (item_contains_position(_mission_item)) {
		switch (_mission_execution_mode) {
		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
				/* force vtol land */
				if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
					_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
				}

				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

				// allow weather vane in mission
				pos_sp_triplet->current.allow_weather_vane = true;

				/* do takeoff before going to setpoint if needed and not already in takeoff */
				/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
				if (do_need_vertical_takeoff() &&
				    _work_item_type == WORK_ITEM_TYPE_DEFAULT &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

					/* use current mission item as next position item */
					mission_item_next_position = _mission_item;
					mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
					has_next_position_item = true;

					float takeoff_alt = calculate_takeoff_altitude(&_mission_item);

					mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home.",
							 (double)(takeoff_alt - _navigator->get_home_position()->alt));

					_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
					_mission_item.lat = _navigator->get_global_position()->lat;
					_mission_item.lon = _navigator->get_global_position()->lon;
					/* hold heading for takeoff items */
					_mission_item.yaw = _navigator->get_global_position()->yaw;
					_mission_item.altitude = takeoff_alt;
					_mission_item.altitude_is_relative = false;
					_mission_item.autocontinue = true;
					_mission_item.time_inside = 0.0f;

				} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {

					/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
					 * Note also that resetting time_inside to zero will cause pitch_min to be zero as well.
					 */
					_mission_item.time_inside = 0.0f;

				} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
						/* haven't transitioned yet, trigger vtol takeoff logic below */
						_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

					} else {
						/* already in fixed-wing, go to waypoint */
						_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					}

					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
				}

				/* if we just did a normal takeoff navigate to the actual waypoint now */
				if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF &&
				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {

					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
					_mission_item.yaw = NAN;
					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
					 */
					_mission_item.time_inside = 0.0f;
				}

				/* if we just did a VTOL takeoff, prepare transition */
				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
				    !_navigator->get_land_detected()->landed) {

					/* disable weathervane before front transition for allowing yaw to align */
					pos_sp_triplet->current.allow_weather_vane = false;

					/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
								    _mission_item.lat, _mission_item.lon);

					_mission_item.force_heading = true;

					new_work_item_type = WORK_ITEM_TYPE_ALIGN;

					/* set position setpoint to current while aligning */
					_mission_item.lat = _navigator->get_global_position()->lat;
					_mission_item.lon = _navigator->get_global_position()->lon;
				}

				/* heading is aligned now, prepare transition */
				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
				    _work_item_type == WORK_ITEM_TYPE_ALIGN &&
				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
				    !_navigator->get_land_detected()->landed) {

					/* check if the vtol_takeoff waypoint is on top of us */
					if (do_need_move_to_takeoff()) {
						new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
					}

					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);

					/* set position setpoint to target during the transition */
					// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
				}

				/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
				    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
{'fraction_non_alphanumeric': 0.10511881207514644, 'fraction_numerical': 0.002507493094599095, 'mean_word_length': 4.178332318928788, 'pattern_counts': {'":': 0, '<': 83, '<?xml version=': 0, '>': 310, 'https://': 4, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4605852', 'n_tokens_mistral': 19093, 'n_tokens_neox': 16415, 'n_words': 3970}