Mission feasibility fixes

- Fixes PX4#3656 - Fixes PX4#3648 - Corrected spelling on method
mazgch · Feb 4, 2016 · fdcfb7c · fdcfb7c
1 parent 0bde2b6
commit fdcfb7c
Show file tree

Hide file tree

Showing 5 changed files with 41 additions and 19 deletions.
diff --git 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
@@ -196,7 +196,7 @@ class Mission : public MissionBlock
 	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
@@ -36,6 +36,7 @@
  *
  * @author Lorenz Meier <[email protected]>
  * @author Thomas Gubler <[email protected]>
+ * @author Sander Smeets <[email protected]>
  */
 
 #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
@@ -36,6 +36,7 @@
  *
  * @author Lorenz Meier <[email protected]>
  * @author Thomas Gubler <[email protected]>
+ * @author Sander Smeets <[email protected]>
  */
 
 #ifndef MISSION_FEASIBILITY_CHECKER_H_
@@ -65,6 +66,7 @@ class MissionFeasibilityChecker
 	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
@@ -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 */
 };