diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3dd2ce36d1..5c01017678 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -80,7 +80,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), - _missionFeasiblityChecker(), + _missionFeasibilityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), _on_arrival_yaw(NAN), @@ -255,7 +255,7 @@ Mission::update_offboard_mission() * 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, + failed = !_missionFeasibilityChecker.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(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -841,7 +841,7 @@ Mission::check_mission_valid() dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + _navigator->get_mission_result()->valid = _missionFeasibilityChecker.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(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index d54d91244b..971ba37162 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -196,7 +196,7 @@ private: bool _inited; bool _home_inited; - MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */ float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d8ee9c8628..b9c7c05283 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -36,6 +36,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Sander Smeets */ #include "mission_feasibility_checker.h" @@ -185,25 +186,32 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return false; } - /* always reject relative alt without home set */ - if (missionitem.altitude_is_relative && !home_valid) { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + /* reject relative alt without home set */ + if (missionitem.altitude_is_relative && !home_valid && isPositionCommand(missionitem.nav_cmd)) { + warning_issued = true; - return false; + + if (throw_error) { + mavlink_log_critical(_mavlink_fd, "Rejecting mission: No home pos, WP %d uses rel alt", i+1); + return false; + } else { + mavlink_log_critical(_mavlink_fd, "Warning: No home pos, WP %d uses rel alt", i+1); + return true; + } } /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; - if (home_alt > wp_alt) { + if (home_alt > wp_alt && isPositionCommand(missionitem.nav_cmd)) { warning_issued = true; if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting mission: Waypoint %d below home", i+1); return false; } else { - mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i+1); return true; } } @@ -235,8 +243,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_PATHPLANNING && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && - missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL && - missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; @@ -357,12 +365,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } } /* check only items with valid lat/lon */ - else if ( 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_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + else if (isPositionCommand(mission_item.nav_cmd)) { /* check distance from current position to item */ float dist_to_1wp = get_distance_to_next_waypoint( @@ -401,6 +404,22 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } } +bool +MissionFeasibilityChecker::isPositionCommand(unsigned cmd){ + if( cmd == NAV_CMD_WAYPOINT || + cmd == NAV_CMD_LOITER_TIME_LIMIT || + cmd == NAV_CMD_LOITER_TURN_COUNT || + cmd == NAV_CMD_LOITER_UNLIMITED || + cmd == NAV_CMD_TAKEOFF || + cmd == NAV_CMD_LAND || + cmd == NAV_CMD_PATHPLANNING) { + return true; + } else { + return false; + + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7b44c3246d..d605f76228 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -36,6 +36,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Sander Smeets */ #ifndef MISSION_FEASIBILITY_CHECKER_H_ @@ -65,6 +66,7 @@ private: bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); + bool isPositionCommand(unsigned cmd); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 1539e63052..0a8de21b95 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -64,6 +64,7 @@ enum NAV_CMD { NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_SET_SERVO=183, NAV_CMD_DO_REPEAT_SERVO=184, + NAV_CMD_DO_DIGICAM_CONTROL=203, NAV_CMD_DO_VTOL_TRANSITION=3000, NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */ };