gimbal: implement gimbal_device_id

It turns out that I had omitted implementing the gimbal_device_id which
is the component ID of the gimbal device that the gimbal manager (in
this case PX4) is responsible for.

Signed-off-by: Julian Oes <julian@oes.ch>

src/modules/gimbal/common.h

@@ -82,6 +82,7 @@ struct ControlData {
 	uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one.
 	// 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;
 };
 
 

src/modules/gimbal/gimbal.cpp

@@ -211,6 +211,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 			// Reset control as no one is active anymore, or yet.
 			thread_data.control_data.sysid_primary_control = 0;
 			thread_data.control_data.compid_primary_control = 0;
+			thread_data.control_data.device_compid = 0;
 		}
 
 		InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;

src/modules/gimbal/input_mavlink.cpp

@@ -375,7 +375,6 @@ void InputMavlinkCmdMount::print_status() const
 InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters &parameters) :
 	InputBase(parameters)
 {
-	_stream_gimbal_manager_information();
 }
 
 InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
@@ -453,7 +452,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
 		gimbal_manager_status_s gimbal_manager_status{};
 		gimbal_manager_status.timestamp = hrt_absolute_time();
 		gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags;
-		gimbal_manager_status.gimbal_device_id = 0;
+		gimbal_manager_status.gimbal_device_id = control_data.device_compid;
 		gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control;
 		gimbal_manager_status.primary_control_compid = control_data.compid_primary_control;
 		gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control
@@ -462,7 +461,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
 	}
 }
 
-void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
+void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data)
 {
 	// TODO: Take gimbal_device_information into account.
 
@@ -483,6 +482,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
 	gimbal_manager_info.yaw_max = M_PI_F;
 	gimbal_manager_info.yaw_min = -M_PI_F;
 
+	gimbal_manager_info.gimbal_device_id = control_data.device_compid;
+
 	_gimbal_manager_info_pub.publish(gimbal_manager_info);
 }
 
@@ -578,6 +579,11 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data,
 
 	_stream_gimbal_manager_status(control_data);
 
+	if (_last_device_compid != control_data.device_compid) {
+		_last_device_compid = control_data.device_compid;
+		_stream_gimbal_manager_information(control_data);
+	}
+
 	return update_result;
 }
 

src/modules/gimbal/input_mavlink.h

@@ -114,7 +114,7 @@ private:
 	void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
 			const matrix::Vector3f &angular_velocity);
 	void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
-	void _stream_gimbal_manager_information();
+	void _stream_gimbal_manager_information(const ControlData &control_data);
 	void _stream_gimbal_manager_status(const ControlData &control_data);
 	void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
 
@@ -128,6 +128,8 @@ private:
 	uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
 	uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
 	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
+
+	uint8_t _last_device_compid = 0;
 };
 
 } /* namespace gimbal */

src/modules/gimbal/output.h

@@ -55,7 +55,7 @@ public:
 	explicit OutputBase(const Parameters &parameters);
 	virtual ~OutputBase() = default;
 
-	virtual void update(const ControlData &control_data, bool new_setpoints) = 0;
+	virtual void update(ControlData &control_data, bool new_setpoints) = 0;
 
 	virtual void print_status() const = 0;
 

src/modules/gimbal/output_mavlink.cpp

@@ -46,7 +46,7 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
 	: OutputBase(parameters)
 {}
 
-void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints)
+void OutputMavlinkV1::update(ControlData &control_data, bool new_setpoints)
 {
 	vehicle_command_s vehicle_command{};
 	vehicle_command.timestamp = hrt_absolute_time();
@@ -142,7 +142,7 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
 {
 }
 
-void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints)
+void OutputMavlinkV2::update(ControlData &control_data, bool new_setpoints)
 {
 	_check_for_gimbal_device_information();
 
@@ -162,6 +162,8 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
 			_last_update = t;
 		}
 
+		control_data.device_compid = _gimbal_device_found ? _gimbal_device_compid : 0;
+
 		_publish_gimbal_device_set_attitude();
 	}
 }
@@ -206,6 +208,13 @@ void OutputMavlinkV2::print_status() const
 		     (double)_angle_velocity[0],
 		     (double)_angle_velocity[1],
 		     (double)_angle_velocity[2]);
+
+	if (_gimbal_device_found) {
+		PX4_INFO_RAW("  gimbal device compid found: %d\n", _gimbal_device_compid);
+
+	} else {
+		PX4_INFO_RAW("  gimbal device compid not found\n");
+	}
 }
 
 void OutputMavlinkV2::_publish_gimbal_device_set_attitude()

src/modules/gimbal/output_mavlink.h

@@ -51,7 +51,7 @@ public:
 	OutputMavlinkV1(const Parameters &parameters);
 	virtual ~OutputMavlinkV1() = default;
 
-	virtual void update(const ControlData &control_data, bool new_setpoints);
+	virtual void update(ControlData &control_data, bool new_setpoints);
 
 	virtual void print_status() const;
 
@@ -69,7 +69,7 @@ public:
 	OutputMavlinkV2(const Parameters &parameters);
 	virtual ~OutputMavlinkV2() = default;
 
-	virtual void update(const ControlData &control_data, bool new_setpoints);
+	virtual void update(ControlData &control_data, bool new_setpoints);
 
 	virtual void print_status() const;
 

src/modules/gimbal/output_rc.cpp

@@ -48,7 +48,7 @@ OutputRC::OutputRC(const Parameters &parameters)
 {
 }
 
-void OutputRC::update(const ControlData &control_data, bool new_setpoints)
+void OutputRC::update(ControlData &control_data, bool new_setpoints)
 {
 	if (new_setpoints) {
 		_set_angle_setpoints(control_data);
@@ -61,6 +61,9 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints)
 
 	_stream_device_attitude_status();
 
+	// If the output is RC, then it means we are also the gimbal device.
+	control_data.device_compid = (uint8_t)_parameters.mnt_mav_compid_v1;
+
 	// _angle_outputs are in radians, gimbal_controls are in [-1, 1]
 	gimbal_controls_s gimbal_controls{};
 	gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(

src/modules/gimbal/output_rc.h

@@ -50,7 +50,7 @@ public:
 	explicit OutputRC(const Parameters &parameters);
 	virtual ~OutputRC() = default;
 
-	virtual void update(const ControlData &control_data, bool new_setpoints);
+	virtual void update(ControlData &control_data, bool new_setpoints);
 	virtual void print_status() const;
 
 private:

src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp

@@ -71,7 +71,7 @@ private:
 
 			msg.time_boot_ms = gimbal_manager_information.timestamp / 1000;
 			msg.cap_flags = gimbal_manager_information.cap_flags;
-			msg.gimbal_device_id = 0;
+			msg.gimbal_device_id = gimbal_manager_information.gimbal_device_id;
 			msg.roll_min = gimbal_manager_information.roll_min;
 			msg.roll_max = gimbal_manager_information.roll_max;
 			msg.pitch_min = gimbal_manager_information.pitch_min;