gimbal: correctly set gimbal_device_id

msg/GimbalDeviceAttitudeStatus.msg

@@ -16,5 +16,8 @@ float32 angular_velocity_y
 float32 angular_velocity_z
 
 uint32 failure_flags
+float32 delta_yaw
+float32 delta_yaw_velocity
+uint8 gimbal_device_id
 
 bool received_from_mavlink

msg/GimbalDeviceInformation.msg

@@ -33,4 +33,4 @@ float32 pitch_max # [rad]
 float32 yaw_min # [rad]
 float32 yaw_max # [rad]
 
-uint8 gimbal_device_compid
+uint8 gimbal_device_id

src/modules/gimbal/output_mavlink.cpp

@@ -162,7 +162,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
 			_last_update = now;
 		}
 
-		gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0;
+		gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0;
 
 		_publish_gimbal_device_set_attitude();
 	}
@@ -191,7 +191,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information()
 
 	if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
 		_gimbal_device_found = true;
-		_gimbal_device_compid = gimbal_device_information.gimbal_device_compid;
+		_gimbal_device_id = gimbal_device_information.gimbal_device_id;
 	}
 }
 
@@ -210,7 +210,7 @@ void OutputMavlinkV2::print_status() const
 		     (double)_angle_velocity[2]);
 
 	if (_gimbal_device_found) {
-		PX4_INFO_RAW("  gimbal device compid found: %d\n", _gimbal_device_compid);
+		PX4_INFO_RAW("  gimbal device compid found: %d\n", _gimbal_device_id);
 
 	} else {
 		PX4_INFO_RAW("  gimbal device compid not found\n");
@@ -222,7 +222,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
 	gimbal_device_set_attitude_s set_attitude{};
 	set_attitude.timestamp = hrt_absolute_time();
 	set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
-	set_attitude.target_component = _gimbal_device_compid;
+	set_attitude.target_component = _gimbal_device_id;
 
 	set_attitude.angular_velocity_x = _angle_velocity[0];
 	set_attitude.angular_velocity_y = _angle_velocity[1];

src/modules/gimbal/output_mavlink.h

@@ -81,7 +81,7 @@ class OutputMavlinkV2 : public OutputBase
 	uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
 	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
 
-	uint8_t _gimbal_device_compid{0};
+	uint8_t _gimbal_device_id{0};
 	hrt_abstime _last_gimbal_device_checked{0};
 	bool _gimbal_device_found {false};
 };

src/modules/gimbal/output_rc.cpp

@@ -115,6 +115,10 @@ void OutputRC::_stream_device_attitude_status()
 	q.copyTo(attitude_status.q);
 
 	attitude_status.failure_flags = 0;
+
+	// If the output is RC, then we signal this by referring to compid 1.
+	attitude_status.gimbal_device_id = 1;
+
 	_attitude_status_pub.publish(attitude_status);
 }
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -3063,7 +3063,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
 	gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
 	gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
 
-	gimbal_information.gimbal_device_compid = msg->compid;
+	gimbal_information.gimbal_device_id = msg->compid;
 
 	_gimbal_device_information_pub.publish(gimbal_information);
 }

src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp

@@ -94,6 +94,11 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream
 
 			msg.failure_flags = gimbal_device_attitude_status.failure_flags;
 
+			msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
+			msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
+
+			msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
+
 			mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;