void
Mission::update_offboard_mission()
{
bool failed = true;
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
/* determine current index */
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
} else {
/* if less items available, reset to first item */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0;
}
/* otherwise, just leave it */
}
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
_navigator->get_land_detected()->landed);
_navigator->get_mission_result()->valid = !failed;
if (!failed) {
/* reset mission failure if we have an updated valid mission */
_navigator->get_mission_result()->mission_failure = false;
}
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
} else {
PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
}
if (failed) {
_offboard_mission.count = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
warnx("mission check failed");
}
set_current_offboard_mission_item();
}
void
Mission::update_offboard_mission()
{
bool failed = true;
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id,
_offboard_mission.count, _offboard_mission.current_seq);
/* determine current index */
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
} else {
/* if less items available, reset to first item */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
} else if (_current_offboard_mission_index < 0) {
/* if not initialized, set it to 0 */
_current_offboard_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()->mission_failure = false;
/* reset reached info as well */
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
_navigator->get_mission_result()->seq_total = _offboard_mission.count;
}
} else {
PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
}
if (failed) {
_offboard_mission.count = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
warnx("mission check failed");
}
set_current_offboard_mission_item();
}
void
Mission::update_offboard_mission()
{
bool failed = true;
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
/* determine current index */
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
} else {
/* if less items available, reset to first item */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0;
}
/* otherwise, just leave it */
}
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid());
} else {
warnx("offboard mission update failed");
}
if (failed) {
_offboard_mission.count = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
}
set_current_offboard_mission_item();
}
bool Mission::set_current_offboard_mission_index(unsigned index)
{
if (index < _offboard_mission.count) {
_current_offboard_mission_index = index;
set_current_offboard_mission_item();
// 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;
set_mission_items();
}
return true;
}
return false;
}
void
Mission::set_mission_items()
{
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(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, _param_takeoff_alt.get());
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
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();
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_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
return;
}
if (pos_sp_triplet->current.valid) {
_on_arrival_yaw = _mission_item.yaw;
}
/* do takeoff on first waypoint for rotary wing vehicles */
if (_navigator->get_vstatus()->is_rotary_wing) {
/* force takeoff if landed (additional protection) */
if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
/* new current mission item set, check if we need takeoff */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_takeoff = true;
_need_takeoff = false;
}
}
if (_takeoff) {
/* do takeoff before going to setpoint */
/* set mission item as next position setpoint */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* next SP is not takeoff anymore */
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
/* calculate takeoff altitude */
float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "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;
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
} else {
/* skip takeoff */
_takeoff = false;
}
}
if (_takeoff_finished) {
/* we just finished takeoff */
/* in case we still have to move to the takeoff waypoint we need a waypoint mission item */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_takeoff_finished = false;
}
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to read next mission item */
struct mission_item_s mission_item_next;
if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::set_mission_items()
{
/* make sure param is up to date */
updateParams();
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* 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;
/* copy information about the previous mission item */
if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
/* Copy previous mission item altitude */
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* try setting onboard mission item */
if (_param_onboard_enabled.get()
&& prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
} 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 finished, landed");
} else {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "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, _param_takeoff_alt.get());
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
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();
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 position mission items */
if (item_contains_position(&_mission_item)) {
/* force vtol land */
if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
&& _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
/* we have a new position item so set previous position setpoint to current */
set_previous_pos_setpoint();
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
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;
/* ignore yaw for takeoff items */
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* if we just did a takeoff navigate to the actual waypoint now */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
/* check if the vtol_takeoff command is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = _navigator->get_global_position()->yaw;
} else {
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;
}
}
/* 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;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
}
/* move to land wp as fixed wing */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_land_detected()->landed) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
has_next_position_item = true;
float altitude = _navigator->get_global_position()->alt;
if (pos_sp_triplet->current.valid) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* transition to MC */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_mission_item.autocontinue = true;
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
}
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() &&
(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
has_next_position_item = true;
/*
* Ignoring waypoint altitude:
* Set altitude to the same as we have now to prevent descending too fast into
* the ground. Actual landing will descend anyway until it touches down.
* XXX: We might want to change that at some point if it is clear to the user
* what the altitude means on this waypoint type.
*/
float altitude = _navigator->get_global_position()->alt;
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* ignore yaw for landing items */
/* XXX: if specified heading for landing is desired we could add another step before the descent
* that aligns the vehicle first */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
_mission_item.yaw = NAN;
}
/* handle non-position mission items such as commands */
} else {
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_ALIGN
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
/* yaw is aligned now */
if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
set_previous_pos_setpoint();
}
}
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
issue_command(&_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
