From d29f2ff60cafce4e704e8a429f0fb160daad0f63 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Fri, 28 Jun 2019 15:22:29 +0200
Subject: [PATCH] ObstacleAvoidance: use hysteresis on z to check progress
 towards the goal

---
 .../FlightTasks/tasks/Utility/ObstacleAvoidance.cpp  | 12 +++++++++---
 .../FlightTasks/tasks/Utility/ObstacleAvoidance.hpp  |  4 +++-
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index a139155aae..4e2c9d3ae6 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -44,6 +44,7 @@ using namespace time_literals;
 static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
 /** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */
 static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
+static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s;
 
 const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
 	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}},
@@ -60,6 +61,7 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 	_desired_waypoint = empty_trajectory_waypoint;
 	_failsafe_position.setNaN();
 	_avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE);
+	_no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US);
 
 }
 
@@ -133,7 +135,6 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
 	_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
 	_curr_wp = curr_wp;
 	_ext_yaw_active = ext_yaw_active;
-	_curr_wp_type = wp_type;
 
 	curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
 	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
@@ -142,7 +143,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
-	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = _curr_wp_type;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type;
 
 	next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
 	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
@@ -189,12 +190,17 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
 
 	const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
 
+	bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z);
+	_no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time());
+
 	if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
-	    && _curr_wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+	    && _no_progress_z_hysteresis.get_state()) {
 		// vehicle above or below the target waypoint
 		pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
 	}
 
+	_prev_pos_to_target_z = pos_to_target_z;
+
 	// do not check for waypoints yaw acceptance in navigator
 	pos_control_status.yaw_acceptance = NAN;
 
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index 0d1299e3e7..df0a9279f4 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -128,9 +128,11 @@ private:
 	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
 
 	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
+	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
+
+	float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */
 
 	bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
-	int _curr_wp_type = 0;
 
 	/**
 	 * Publishes vehicle command.