gimbal: introduce timestamp of last setpoint update

To enable implementing a timeout when there's no new setpoint coming in.

src/modules/gimbal/common.h

@@ -83,6 +83,7 @@ struct ControlData {
 	// uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet.
 	// uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet.
 	uint8_t device_compid = 0;
+	uint64_t timestamp_last_update{0}; // Timestamp when there was the last setpoint set by the input used for timeout
 };
 
 

src/modules/gimbal/input.cpp

@@ -42,15 +42,15 @@ InputBase::InputBase(Parameters &parameters) :
 {}
 
 void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude,
-		float roll_angle,
+		uint64_t timestamp)
-		float pitch_fixed_angle)
 {
+	control_data.timestamp_last_update = timestamp;
 	control_data.type = ControlData::Type::LonLat;
 	control_data.type_data.lonlat.lon = lon;
 	control_data.type_data.lonlat.lat = lat;
 	control_data.type_data.lonlat.altitude = altitude;
-	control_data.type_data.lonlat.roll_offset = roll_angle;
+	control_data.type_data.lonlat.roll_offset = NAN;
-	control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
+	control_data.type_data.lonlat.pitch_fixed_angle = NAN;
 	control_data.type_data.lonlat.pitch_offset = 0.f;
 	control_data.type_data.lonlat.yaw_offset = 0.f;
 }

src/modules/gimbal/input.h

@@ -60,8 +60,7 @@ public:
 	virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0;
 	virtual void print_status() const = 0;
 protected:
-	void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN,
+	void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, uint64_t timestamp);
-				      float pitch_fixed_angle = NAN);
 
 	Parameters &_parameters;
 };

src/modules/gimbal/input_mavlink.cpp

@@ -100,6 +100,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool
 		if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
 
 			control_data.type = ControlData::Type::Neutral;
+			control_data.timestamp_last_update = vehicle_roi.timestamp;
 			_cur_roi_mode = vehicle_roi.mode;
 			control_data.sysid_primary_control = _parameters.mav_sysid;
 			control_data.compid_primary_control = _parameters.mav_compid;
@@ -120,7 +121,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool
 			return UpdateResult::UpdatedActive;
 
 		} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
-			control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
+			control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp);
 
 			_cur_roi_mode = vehicle_roi.mode;
 
@@ -155,6 +156,7 @@ void InputMavlinkROI::_read_control_data_from_position_setpoint_sub(ControlData
 {
 	position_setpoint_triplet_s position_setpoint_triplet;
 	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	control_data.timestamp_last_update = position_setpoint_triplet.timestamp;
 	control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
 	control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
 	control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
@@ -261,6 +263,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_
 		// fallthrough
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
 			control_data.type = ControlData::Type::Neutral;
+			control_data.timestamp_last_update = vehicle_command.timestamp;
 			control_data.sysid_primary_control = vehicle_command.source_system;
 			control_data.compid_primary_control = vehicle_command.source_component;
 			update_result = UpdateResult::UpdatedActiveOnce;
@@ -268,6 +271,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_
 
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
 				control_data.type = ControlData::Type::Angle;
+				control_data.timestamp_last_update = vehicle_command.timestamp;
 				control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 				control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 				control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
@@ -302,7 +306,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_
 
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
 			control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5,
-						 vehicle_command.param4);
+						 vehicle_command.param4, vehicle_command.timestamp);
 			control_data.sysid_primary_control = vehicle_command.source_system;
 			control_data.compid_primary_control = vehicle_command.source_component;
 			update_result = UpdateResult::UpdatedActive;
@@ -348,6 +352,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_
 			}
 		}
 
+		control_data.timestamp_last_update = vehicle_command.timestamp;
 		control_data.sysid_primary_control = vehicle_command.source_system;
 		control_data.compid_primary_control = vehicle_command.source_component;
 
@@ -603,7 +608,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(C
 							set_attitude.angular_velocity_y,
 							set_attitude.angular_velocity_z);
 
-		_set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity);
+		_set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity, set_attitude.timestamp);
 		return UpdateResult::UpdatedActive;
 
 	} else {
@@ -619,11 +624,13 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co
 	if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
 
 		control_data.type = ControlData::Type::Neutral;
+		control_data.timestamp_last_update = vehicle_roi.timestamp;
 		_cur_roi_mode = vehicle_roi.mode;
 		return UpdateResult::UpdatedActiveOnce;
 
 	} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
 		control_data.type = ControlData::Type::LonLat;
+		control_data.timestamp_last_update = vehicle_roi.timestamp;
 		_read_control_data_from_position_setpoint_sub(control_data);
 		control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
 
@@ -636,7 +643,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co
 		return UpdateResult::UpdatedActive;
 
 	} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
-		control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
+		control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp);
 
 		_cur_roi_mode = vehicle_roi.mode;
 
@@ -656,6 +663,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_position_setpo
 		const position_setpoint_triplet_s &position_setpoint_triplet)
 {
 	if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
+		control_data.timestamp_last_update = position_setpoint_triplet.timestamp;
 		control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
 		control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
 		control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
@@ -692,11 +700,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
 			control_data.type = ControlData::Type::Neutral;
+			control_data.timestamp_last_update = vehicle_command.timestamp;
 			update_result = InputBase::UpdateResult::UpdatedActiveOnce;
 			break;
 
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
 				control_data.type = ControlData::Type::Angle;
+				control_data.timestamp_last_update = vehicle_command.timestamp;
 				control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 				control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 				control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
@@ -731,7 +741,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
 			control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5,
-						 vehicle_command.param4);
+						 vehicle_command.param4, vehicle_command.timestamp);
 			update_result = UpdateResult::UpdatedActive;
 			break;
 		}
@@ -878,7 +888,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 
 			// TODO: support gimbal device id for multiple gimbals
 
-			_set_control_data_from_set_attitude(control_data, flags, q, angular_velocity);
+			_set_control_data_from_set_attitude(control_data, flags, q, angular_velocity, vehicle_command.timestamp);
 			_ack_vehicle_command(vehicle_command,
 					     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
@@ -922,7 +932,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con
 			matrix::Vector3f(NAN, NAN, NAN);
 
 		_set_control_data_from_set_attitude(control_data, set_manual_control.flags, q,
-						    angular_velocity);
+						    angular_velocity, set_manual_control.timestamp);
 
 		return UpdateResult::UpdatedActive;
 
@@ -934,9 +944,10 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con
 }
 
 void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags,
-		const matrix::Quatf &q,
+		const matrix::Quatf &q, const matrix::Vector3f &angular_velocity, const uint64_t timestamp)
-		const matrix::Vector3f &angular_velocity)
 {
+	control_data.timestamp_last_update = timestamp;
+
 	if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) {
 		// not implemented in ControlData
 	} else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) {
@@ -984,6 +995,7 @@ void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub(Control
 {
 	position_setpoint_triplet_s position_setpoint_triplet;
 	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	control_data.timestamp_last_update = position_setpoint_triplet.timestamp;
 	control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
 	control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
 	control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;

src/modules/gimbal/input_mavlink.h

@@ -112,7 +112,7 @@ private:
 	UpdateResult _process_set_manual_control(ControlData &control_data,
 			const gimbal_manager_set_manual_control_s &set_manual_control);
 	void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
-			const matrix::Vector3f &angular_velocity);
+			const matrix::Vector3f &angular_velocity, const uint64_t timestamp);
 	void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
 	void _stream_gimbal_manager_information(const ControlData &control_data);
 	void _stream_gimbal_manager_status(const ControlData &control_data);

src/modules/gimbal/input_rc.cpp

@@ -99,6 +99,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
 	manual_control_setpoint_s manual_control_setpoint{};
 	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
 	control_data.type = ControlData::Type::Angle;
+	control_data.timestamp_last_update = manual_control_setpoint.timestamp;
 
 	float new_aux_values[3];
 

src/modules/gimbal/input_test.cpp

@@ -53,6 +53,7 @@ InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &
 	}
 
 	control_data.type = ControlData::Type::Angle;
+	control_data.timestamp_last_update = hrt_absolute_time();
 
 	control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 	control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;