Navigator: add MissionFeasibilityChecker class; performs validation of landing waypoint set-up for fixed wing for now, but can be extended for other checks (e.g. check mission against geofence)

1 files changed, 184 insertions, 0 deletions

diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
new file mode 100644
index 000000000..25b2636bb
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission_feasibility_checker.cpp
+ * Provides checks if mission is feasible given the navigation capabilities
+ */
+
+#include "mission_feasibility_checker.h"
+
+#include <geo/geo.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+#include <fw_pos_control_l1/landingslope.h>
+#include <systemlib/err.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
+{
+	_nav_caps = {0};
+}
+
+
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
+{
+	/* Init if not done yet */
+	init();
+
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+
+	if (isRotarywing)
+		return checkMissionFeasibleRotarywing(dm_current, nItems);
+	else
+		return checkMissionFeasibleFixedwing(dm_current, nItems);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
+{
+
+	return checkGeofence(dm_current, nItems);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
+{
+	/* 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));
+}
+
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
+{
+	//xxx: check geofence
+	return true;
+}
+
+bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
+{
+	/* 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++) {
+		static struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(dm_current, 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_LAND) {
+			struct mission_item_s missionitem_previous;
+			if (i != 0) {
+				if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
+				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
+//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
+//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
+//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
+
+				if (wp_distance > _nav_caps.landing_flare_length) {
+					/* Last wp is before flare region */
+
+					if (delta_altitude < 0) {
+						if (missionitem_previous.altitude <= slope_alt_req) {
+							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
+							return true;
+						} else {
+							/* Landing waypoint is above altitude of slope at the given waypoint distance */
+							mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
+							mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+									(double)(slope_alt_req),
+									(double)(wp_distance_req - wp_distance));
+							return false;
+						}
+					} else {
+						/* Landing waypoint is above last waypoint */
+						mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
+						return false;
+					}
+				} else {
+					/* Last wp is in flare region */
+					//xxx give recommendations
+					mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
+					return false;
+				}
+			} else {
+				mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
+				return false;
+			}
+		}
+	}
+
+
+//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+}
+
+void MissionFeasibilityChecker::updateNavigationCapabilities()
+{
+	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+}
+
+void MissionFeasibilityChecker::init()
+{
+	if (!_initDone) {
+
+		_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+
+		_initDone = true;
+	}
+}