collision_prevention: improve behavior in case of range data loss

* add test for no incoming range data

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -105,6 +105,18 @@ hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr)
 	return hrt_absolute_time() - *ptr;
 }
 
+bool CollisionPrevention::is_active()
+{
+	bool activated = _param_cp_dist.get() > 0;
+
+	if (activated && !_was_active) {
+		_time_activated = getTime();
+	}
+
+	_was_active = activated;
+	return activated;
+}
+
 void
 CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
 {
@@ -467,9 +479,24 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 		}
 
 	} else {
+		//allow no movement
+		float vel_max = 0.f;
+		setpoint = setpoint * vel_max;
+
 		// if distance data is stale, switch to Loiter
-		_publishVehicleCmdDoLoiter();
+		if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
-		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
+			mavlink_log_critical(&_mavlink_log_pub, "No range data received, no movement allowed.");
+
+			if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
+			    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
+				_publishVehicleCmdDoLoiter();
+				mavlink_log_critical(&_mavlink_log_pub, "No range data received, switch to HOLD.");
+			}
+
+			_last_timeout_warning = getTime();
+		}
+
+
 	}
 }
 
@@ -488,9 +515,9 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
 
 	if (currently_interfering && !_interfering) {
-		if (hrt_elapsed_time(&_last_collision_warning) > 3_s) {
+		if (getElapsedTime(&_last_collision_warning) > 3_s) {
-			mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");
+			mavlink_log_info(&_mavlink_log_pub, "Collision Prevention limiting velocity");
-			_last_collision_warning = hrt_absolute_time();
+			_last_collision_warning = getTime();
 		}
 	}
 
@@ -498,7 +525,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 
 	// publish constraints
 	collision_constraints_s	constraints{};
-	constraints.timestamp = hrt_absolute_time();
+	constraints.timestamp = getTime();
 	original_setpoint.copyTo(constraints.original_setpoint);
 	new_setpoint.copyTo(constraints.adapted_setpoint);
 	_constraints_pub.publish(constraints);
@@ -509,7 +536,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 void CollisionPrevention::_publishVehicleCmdDoLoiter()
 {
 	vehicle_command_s command{};
-	command.timestamp = hrt_absolute_time();
+	command.timestamp = getTime();
 	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
 	command.param1 = (float)1; // base mode
 	command.param3 = (float)0; // sub mode

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -67,10 +67,11 @@ public:
 	CollisionPrevention(ModuleParams *parent);
 
 	virtual ~CollisionPrevention();
+
 	/**
 	 * Returs true if Collision Prevention is running
 	 */
-	bool is_active() { return _param_cp_dist.get() > 0; }
+	bool is_active();
 
 	/**
 	 * Computes collision free setpoints
@@ -122,6 +123,7 @@ protected:
 private:
 
 	bool _interfering{false};		/**< states if the collision prevention interferes with the user input */
+	bool _was_active{false};		/**< states if the collision prevention interferes with the user input */
 
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
 
@@ -134,8 +136,11 @@ private:
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
+	static constexpr uint64_t TIMEOUT_HOLD_US{5_s};
 
 	hrt_abstime	_last_collision_warning{0};
+	hrt_abstime	_last_timeout_warning{0};
+	hrt_abstime	_time_activated{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */

src/lib/collision_prevention/CollisionPreventionTest.cpp

@@ -251,9 +251,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
 TEST_F(CollisionPreventionTest, testPurgeOldData)
 {
 	// GIVEN: a simple setup condition
+	TestTimingCollisionPrevention cp;
 	hrt_abstime start_time = hrt_absolute_time();
 	mocked_time = start_time;
-	TestTimingCollisionPrevention cp;
 	matrix::Vector2f original_setpoint(10, 0);
 	float max_speed = 3;
 	matrix::Vector2f curr_pos(0, 0);
@@ -325,6 +325,74 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
 	orb_unadvertise(vehicle_attitude_pub);
 }
 
+TEST_F(CollisionPreventionTest, testNoRangeData)
+{
+	// GIVEN: a simple setup condition
+	TestTimingCollisionPrevention cp;
+	hrt_abstime start_time = hrt_absolute_time();
+	mocked_time = start_time;
+	matrix::Vector2f original_setpoint(10, 0);
+	float max_speed = 3;
+	matrix::Vector2f curr_pos(0, 0);
+	matrix::Vector2f curr_vel(2, 0);
+	vehicle_attitude_s attitude;
+	attitude.timestamp = start_time;
+	attitude.q[0] = 1.0f;
+	attitude.q[1] = 0.0f;
+	attitude.q[2] = 0.0f;
+	attitude.q[3] = 0.0f;
+
+	// AND: a parameter handle
+	param_t param = param_handle(px4::params::CP_DIST);
+	float value = 10; // try to keep 10m distance
+	param_set(param, &value);
+	cp.paramsChanged();
+
+	// AND: an obstacle message without any obstacle
+	obstacle_distance_s message;
+	memset(&message, 0xDEAD, sizeof(message));
+	message.frame = message.MAV_FRAME_GLOBAL; //north aligned
+	message.min_distance = 100;
+	message.max_distance = 10000;
+	message.angle_offset = 0;
+	message.timestamp = start_time;
+	int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
+	message.increment = 360 / distances_array_size;
+
+	for (int i = 0; i < distances_array_size; i++) {
+		message.distances[i] = 9000;
+	}
+
+
+	// WHEN: we publish the message and set the parameter and then run the setpoint modification
+	orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
+	orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
+	orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
+	orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
+
+	for (int i = 0; i < 10; i++) {
+
+		matrix::Vector2f modified_setpoint = original_setpoint;
+		cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
+
+		//advance time by 0.1 seconds but no new message comes in
+		mocked_time = mocked_time + 100000;
+
+		if (i < 5) {
+			// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
+			// Note: direction will change slightly due to guidance
+			EXPECT_EQ(original_setpoint.norm(), modified_setpoint.norm());
+
+		} else {
+			// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
+			EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
+		}
+	}
+
+	orb_unadvertise(obstacle_distance_pub);
+	orb_unadvertise(vehicle_attitude_pub);
+}
+
 TEST_F(CollisionPreventionTest, noBias)
 {
 	// GIVEN: a simple setup condition