From 8637a9255ec4c24cf486362690dc859ae8faf4a4 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Tue, 25 Jun 2019 09:42:17 +0200
Subject: [PATCH] CollisionPrevention: add failsafe for stale distance data

---
 .../CollisionPrevention.cpp                   | 36 +++++++++++++++++--
 .../CollisionPrevention.hpp                   |  7 ++--
 2 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index d1f1407695..b8067231c4 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -265,9 +265,14 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
 			}
 		}
 
-	} else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) {
-		mavlink_log_critical(&_mavlink_log_pub, "No range data received");
-		_last_message = hrt_absolute_time();
+	} else {
+		// if distance data are stale, switch to Loiter and disable Collision Prevention
+		// such that it is still possible to fly in Position Control Mode
+		_publishVehicleCmdDoLoiter();
+		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
+		float col_prev_d = -1.f;
+		param_set(param_find("MPC_COL_PREV_D"), &col_prev_d);
+		mavlink_log_critical(&_mavlink_log_pub, "Collision Prevention disabled.");
 	}
 }
 
@@ -292,3 +297,28 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
 	publishConstrainedSetpoint(original_setpoint, new_setpoint);
 	original_setpoint = new_setpoint;
 }
+
+void CollisionPrevention::_publishVehicleCmdDoLoiter()
+{
+	vehicle_command_s command{};
+	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+	command.param1 = (float)1; // base mode
+	command.param3 = (float)0; // sub mode
+	command.target_system = 1;
+	command.target_component = 1;
+	command.source_system = 1;
+	command.source_component = 1;
+	command.confirmation = false;
+	command.from_external = false;
+	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
+	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+
+	// publish the vehicle command
+	if (_pub_vehicle_command == nullptr) {
+		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
+				       vehicle_command_s::ORB_QUEUE_LENGTH);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
+	}
+}
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp
index 83aba8a809..f49d950062 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.hpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp
@@ -48,11 +48,13 @@
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/collision_constraints.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
 #include <mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <systemlib/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 class CollisionPrevention : public ModuleParams
 {
@@ -73,15 +75,13 @@ private:
 	orb_advert_t _constraints_pub{nullptr};  	/**< constraints publication */
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
 	orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */
+	orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
 
 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
 	uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000};
-	static constexpr uint64_t MESSAGE_THROTTLE_US{5000000};
-
-	hrt_abstime _last_message{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
@@ -125,5 +125,6 @@ private:
 
 	void updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
 	void updateDistanceSensor(obstacle_distance_s &obstacle);
+	void _publishVehicleCmdDoLoiter();
 
 };