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)

3 files changed, 56 insertions, 19 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 2be25517d..3784337ac 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -312,6 +312,11 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Publish navigation capabilities
+	 */
+	void navigation_capabilities_publish();
+
+	/**
 	 * Control position.
 	 */
 	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
@@ -538,6 +543,12 @@ FixedwingPositionControl::parameters_update()
 	/* Update the landing slope */
 	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
 
+	/* Update and publish the navigation capabilities */
+	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+	_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+	_nav_capabilities.landing_flare_length = landingslope.flare_length();
+	navigation_capabilities_publish();
+
 	return OK;
 }
 
@@ -709,6 +720,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu
 	}
 }
 
+void FixedwingPositionControl::navigation_capabilities_publish()
+{
+	if (_nav_capabilities_pub > 0) {
+		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+	} else {
+		_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+	}
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
 		const struct mission_item_triplet_s &mission_item_triplet)
@@ -875,7 +895,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				 }
 
-				float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
+				float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
 
 				/* avoid climbout */
 				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
@@ -1214,11 +1234,8 @@ FixedwingPositionControl::task_main()
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
 
-					if (_nav_capabilities_pub > 0) {
-						orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
-					} else {
-						_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
-					}
+					navigation_capabilities_publish();
+
 				}
 
 			}
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
index a2d8525b9..b139a6397 100644
--- a/src/modules/fw_pos_control_l1/landingslope.cpp
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -1,9 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Author: @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
@@ -37,7 +35,6 @@
 /*
  * @file: landingslope.cpp
  *
- * @author: Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include "landingslope.h"
@@ -74,11 +71,12 @@ void Landingslope::calculateSlopeValues()
 
 float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
 {
-	return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
 }
 
-float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude)
+float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
 {
-	return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+	return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+
 }
 
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
index b77252e6e..8ff431509 100644
--- a/src/modules/fw_pos_control_l1/landingslope.h
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -1,9 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Author: @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
@@ -37,15 +35,18 @@
 /*
  * @file: landingslope.h
  *
- * @author: Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef LANDINGSLOPE_H_
 #define LANDINGSLOPE_H_
 
+#include <math.h>
+#include <systemlib/err.h>
+
 class Landingslope
 {
 private:
+	//xxx: documentation of landing pending
 	float _landing_slope_angle_rad;
 	float _flare_relative_alt;
 	float _motor_lim_horizontal_distance;
@@ -62,8 +63,29 @@ public:
 	Landingslope() {}
 	~Landingslope() {}
 
-	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
-	float getFlarceCurveAltitude(float wp_distance, float wp_altitude);
+	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
+	}
+
+	/**
+	 *
+	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
+	 */
+	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
+
+	}
+
+
+	float getFlareCurveAltitude(float wp_distance, float wp_altitude);
 
 	void update(float landing_slope_angle_rad,
 			float flare_relative_alt,