diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 2ecb7f74fe72..f4f084321cd2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 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 @@ -37,6 +37,7 @@ * @author Lorenz Meier * @author Thomas Gubler * @author Sander Smeets + * @author Nuno Marques */ #include "mission_feasibility_checker.h" @@ -83,58 +84,30 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, // VTOL always respects rotary wing feasibility if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) { - failed = failed || !checkRotarywing(mission, home_alt, home_alt_valid); + failed = failed || !checkRotarywing(mission, home_alt); } else { - failed = failed || !checkFixedwing(mission, home_alt, home_alt_valid, land_start_req); + failed = failed || !checkFixedwing(mission, home_alt, land_start_req); } return !failed; } bool -MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid) +MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt) { - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - // look for a takeoff waypoint - if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { - // make sure that the altitude of the waypoint is at least one meter larger than the altitude acceptance radius - // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air - const float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt; - - // check if we should use default acceptance radius - float acceptance_radius = _navigator->get_altitude_acceptance_radius(); - - // if a specific acceptance radius has been defined, use that one instead - if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { - acceptance_radius = missionitem.acceptance_radius; - } - - if (takeoff_alt - 1.0f < acceptance_radius) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); - return false; - } - } - } - - // all checks have passed - return true; + /* + * Perform check and issue feedback to the user + * Mission is only marked as feasible if takeoff check passes + */ + return checkTakeoff(mission, home_alt); } bool -MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool home_alt_valid, - bool land_start_req) +MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req) { /* Perform checks and issue feedback to the user for all checks */ - bool resTakeoff = checkFixedWingTakeoff(mission, home_alt, home_alt_valid); + bool resTakeoff = checkTakeoff(mission, home_alt); bool resLanding = checkFixedWingLanding(mission, land_start_req); /* Mission is only marked as feasible if all checks return true */ @@ -313,8 +286,12 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) } bool -MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid) +MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt) { + bool has_takeoff = false; + bool takeoff_first = false; + int takeoff_index = -1; + for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; const ssize_t len = sizeof(struct mission_item_s); @@ -344,6 +321,82 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); return false; } + + // tell that mission has a takeoff waypoint + has_takeoff = true; + + // tell that a takeoff waypoint is the first "waypoint" + // mission item + if (i == 0) { + takeoff_first = true; + + } else if (takeoff_index == -1) { + // stores the index of the first takeoff waypoint + takeoff_index = i; + } + } + } + + if (takeoff_index != -1) { + // checks if all the mission items before the first takeoff waypoint + // are not waypoints or position-related items; + // this means that, before a takeoff waypoint, one can set + // one of the bellow mission items + for (size_t i = 0; i < (size_t)takeoff_index; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_DELAY && + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != NAV_CMD_DO_LAND_START && + missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { + takeoff_first = false; + + } else { + takeoff_first = true; + + } + } + } + + if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) { + // check for a takeoff waypoint, after the above conditions have been met + // MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission + // while the vehicle is flying and it does not require a takeoff waypoint + if (!has_takeoff) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required."); + return false; + + } else if (!takeoff_first) { + // check if the takeoff waypoint is the first waypoint item on the mission + // i.e, an item with position/attitude change modification + // if it is not, the mission should be rejected + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item"); + return false; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 8eb7a648fb2d..5c2f82538932 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -37,6 +37,7 @@ * @author Lorenz Meier * @author Thomas Gubler * @author Sander Smeets + * @author Nuno Marques */ #pragma once @@ -63,12 +64,12 @@ class MissionFeasibilityChecker bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance); /* Checks specific to fixedwing airframes */ - bool checkFixedwing(const mission_s &mission, float home_alt, bool home_alt_valid, bool land_start_req); - bool checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid); + bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req); + bool checkTakeoff(const mission_s &mission, float home_alt); bool checkFixedWingLanding(const mission_s &mission, bool land_start_req); /* Checks specific to rotarywing airframes */ - bool checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid); + bool checkRotarywing(const mission_s &mission, float home_alt); public: MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {} diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d1b6f48f01b4..17e6bba7abb1 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -57,6 +57,16 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); +/** + * Take-off waypoint required + * + * If set, the mission feasibility checker will check for a takeoff waypoint on the mission. + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0); + /** * Minimum Loiter altitude * diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2c16d97f231e..cd5a2fb4ef7b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -280,6 +280,7 @@ class Navigator : public ModuleBase, public ModuleParams // Param access float get_loiter_min_alt() const { return _param_loiter_min_alt.get(); } float get_takeoff_min_alt() const { return _param_takeoff_min_alt.get(); } + bool get_takeoff_required() const { return _param_takeoff_required.get(); } float get_yaw_timeout() const { return _param_yaw_timeout.get(); } float get_yaw_threshold() const { return _param_yaw_err.get(); } @@ -370,6 +371,7 @@ class Navigator : public ModuleBase, public ModuleParams // Mission (MIS_*) (ParamFloat) _param_loiter_min_alt, (ParamFloat) _param_takeoff_min_alt, + (ParamBool) _param_takeoff_required, (ParamFloat) _param_yaw_timeout, (ParamFloat) _param_yaw_err )