navigator/mission feasibility: prepare for pre-mission fence checking

src/modules/navigator/mission_feasibility_checker.cpp

@@ -48,6 +48,7 @@
 #include <stdio.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <uORB/topics/fence.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
 {
 	/* Init if not done yet */
 	init();
@@ -74,39 +75,39 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 
 
 	if (isRotarywing)
-		return checkMissionFeasibleRotarywing(dm_current, nItems);
+		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence);
 	else
-		return checkMissionFeasibleFixedwing(dm_current, nItems);
+		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence);
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
 {
 
-	return checkGeofence(dm_current, nItems);
+	return checkGeofence(dm_current, nMissionItems, fence);
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
 {
 	/* Update fixed wing navigation capabilites */
 	updateNavigationCapabilities();
 //	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
 
-	return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
+	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence));
 }
 
-bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
 {
 	//xxx: check geofence
 	return true;
 }
 
-bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
+bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
 {
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
 
 
-	for (size_t i = 0; i < nItems; i++) {
+	for (size_t i = 0; i < nMissionItems; i++) {
 		static struct mission_item_s missionitem;
 		const ssize_t len = sizeof(struct mission_item_s);
 		if (dm_read(dm_current, i, &missionitem, len) != len) {

src/modules/navigator/mission_feasibility_checker.h

@@ -57,15 +57,15 @@ private:
 	void init();
 
 	/* Checks for all airframes */
-	bool checkGeofence(dm_item_t dm_current, size_t nItems);
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
-	bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
 public:
 
 	MissionFeasibilityChecker();
@@ -74,7 +74,7 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
+	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
 
 };
 

src/modules/navigator/navigator_main.cpp

@@ -456,7 +456,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count);
+		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
 		_mission.set_current_offboard_mission_index(offboard_mission.current_index);