gimbal: handle angular_velocity setpoint stream timeout

The gimbal would otherwise continue to spin with whatever velocity was commanded
before the input connection was lost.

src/modules/gimbal/gimbal.cpp

@@ -264,6 +264,11 @@ static int gimbal_thread_main(int argc, char *argv[])
 				thread_data.output_obj->set_stabilize(false, false, false);
 			}
 
+			if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) {
+				// Without flagging an update the changes are not processed in the output
+				update_result = InputBase::UpdateResult::UpdatedActive;
+			}
+
 			// Update output
 			thread_data.output_obj->update(
 				thread_data.control_data,

src/modules/gimbal/output.cpp

@@ -43,6 +43,8 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+using namespace time_literals;
+
 namespace gimbal
 {
 
@@ -81,6 +83,27 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 	return atan2f(z, target_distance);
 }
 
+bool OutputBase::check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now)
+{
+	bool ret = false;
+	const bool timeout = (control_data.timestamp_last_update + 2_s < now);
+	const bool type_angle = (control_data.type == ControlData::Type::Angle);
+
+	if (timeout && type_angle) {
+		// Avoid gimbal keeps on spinning if the last setpoint was angular_velocity but it times out
+		for (int i = 0; i < 3; ++i) {
+			float &vel = control_data.type_data.angle.angular_velocity[i];
+
+			if (PX4_ISFINITE(vel) && (fabsf(vel) > FLT_EPSILON)) {
+				vel = 0.f;
+				ret = true;
+			}
+		}
+	}
+
+	return ret;
+}
+
 void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 {
 	switch (control_data.type) {

src/modules/gimbal/output.h

@@ -63,6 +63,14 @@ public:
 
 	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
 
+	/**
+	 * Time out if setpoint that should be streamed stops
+	 * @param control_data setpoint to check timestamp of and amend upon timeout
+	 * @param now Current system timestamp
+	 * @return true iff setpoint was amended because of timeout
+	 */
+	bool check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now);
+
 protected:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);