NPFG: Add fallback for corner cases

src/lib/npfg/npfg.cpp

@@ -47,6 +47,32 @@
 using matrix::Vector2d;
 using matrix::Vector2f;
 
+float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
+{
+	if (is_wind_valid) {
+		// If we have a valid wind estimate, npfg is able to handle all degenerated cases
+		return 1.f;
+	}
+
+	// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
+	// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
+	const Vector2f ground_vel(local_pos.vx, local_pos.vy);
+	const float ground_speed(ground_vel.norm());
+	const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
+					    local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
+					    0.f, 1.f));
+
+	// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
+	const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
+	const Vector2f ground_vel_norm(ground_vel.normalized());
+	const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
+					  COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
+							  COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
+					  0.f, 1.f));
+
+	return flying_forward_factor * low_ground_speed_factor;
+}
+
 void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel,
 		       const Vector2f &unit_path_tangent,
 		       const Vector2f &position_on_path, const float path_curvature)

src/lib/npfg/npfg.hpp

@@ -63,6 +63,8 @@
 #include <matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
 
+#include <uORB/topics/vehicle_local_position.h>
+
 /*
  * NPFG
  * Lateral-directional nonlinear path following guidance logic with excess wind handling
@@ -71,6 +73,17 @@ class NPFG
 {
 
 public:
+	/**
+	 * @brief Can run
+	 *
+	 * Evaluation if all the necessary information are available such that npfg can produce meaningful results.
+	 *
+	 * @param[in] local_pos is the current vehicle local position uorb message
+	 * @param[in] is_wind_valid flag if the wind estimation is valid
+	 * @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions.
+	 */
+
+	float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
 	/*
 	 * Computes the lateral acceleration and airspeed references necessary to track
 	 * a path in wind (including excess wind conditions).
@@ -270,6 +283,10 @@ public:
 	float getRollSetpoint() { return roll_setpoint_; }
 
 private:
+	static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
+	static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
+	static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
+	static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
 
 	static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon
 	static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]