gimbal: merge fixes

src/modules/mavlink/mavlink_messages.cpp

@@ -1530,7 +1530,7 @@ protected:
 	explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	bool send(const hrt_abstime t) override
+	bool send() override
 	{
 		if (_att_sub.advertised()) {
 
@@ -2021,7 +2021,7 @@ protected:
 	explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	bool send(const hrt_abstime t) override
+	bool send() override
 	{
 		gimbal_device_attitude_status_s gimbal_device_attitude_status{};
 
@@ -2093,23 +2093,23 @@ protected:
 	explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	bool send(const hrt_abstime t) override
+	bool send() override
 	{
 		gimbal_manager_information_s gimbal_manager_information;
 
-		if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_device_information)) {
-			// send out gimbal_manager_info with info from gimbal_device_information
+		if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_manager_information)) {
+			// send out gimbal_manager_info with info from gimbal_manager_information
 			mavlink_gimbal_manager_information_t msg{};
-			msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
+			msg.time_boot_ms = gimbal_manager_information.timestamp / 1000;
 			msg.gimbal_device_id = 0;
-			msg.cap_flags = gimbal_device_information.cap_flags;
+			msg.cap_flags = gimbal_manager_information.cap_flags;
 
-			msg.roll_min = gimbal_device_information.roll_min;
-			msg.roll_max = gimbal_device_information.roll_max;
-			msg.pitch_min = gimbal_device_information.pitch_min;
-			msg.pitch_max = gimbal_device_information.pitch_max;
-			msg.yaw_min = gimbal_device_information.yaw_min;
-			msg.yaw_max = gimbal_device_information.yaw_max;
+			msg.roll_min = gimbal_manager_information.roll_min;
+			msg.roll_max = gimbal_manager_information.roll_max;
+			msg.pitch_min = gimbal_manager_information.pitch_min;
+			msg.pitch_max = gimbal_manager_information.pitch_max;
+			msg.yaw_min = gimbal_manager_information.yaw_min;
+			msg.yaw_max = gimbal_manager_information.yaw_max;
 
 			mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg);
 
@@ -2166,7 +2166,7 @@ protected:
 	explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	bool send(const hrt_abstime t) override
+	bool send() override
 	{
 		gimbal_manager_status_s gimbal_manager_status;
 
@@ -2174,6 +2174,10 @@ protected:
 			mavlink_gimbal_manager_status_t msg{};
 			msg.time_boot_ms = gimbal_manager_status.timestamp / 1000;
 			msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id;
+			msg.primary_control_sysid = gimbal_manager_status.primary_control_sysid;
+			msg.primary_control_compid = gimbal_manager_status.primary_control_compid;
+			msg.secondary_control_sysid = gimbal_manager_status.secondary_control_sysid;
+			msg.secondary_control_compid = gimbal_manager_status.secondary_control_compid;
 			msg.flags = gimbal_manager_status.flags;
 
 			mavlink_msg_gimbal_manager_status_send_struct(_mavlink->get_channel(), &msg);
@@ -2233,7 +2237,7 @@ protected:
 	explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	bool send(const hrt_abstime t) override
+	bool send() override
 	{
 		gimbal_device_set_attitude_s gimbal_device_set_attitude;
 

src/modules/mavlink/mavlink_receiver.h

@@ -204,7 +204,6 @@ private:
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
-	void handle_message_onboard_computer_status(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
@@ -361,10 +360,8 @@ private:
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
 	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<gimbal_manager_set_manual_control_s>	_gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
-	uORB::Publication<gimbal_device_information_s>			_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
-	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
-	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<gimbal_device_information_s>		_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
+	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
 	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};

src/modules/vmount/input_mavlink.cpp

@@ -811,53 +811,6 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
 						_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
 					}
 
-				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
-
-					switch ((int)vehicle_command.param7) {
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
-						_control_data.gimbal_shutter_retract = true;
-
-					/* FALLTHROUGH */
-
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
-						_control_data.type = ControlData::Type::Neutral;
-
-						*control_data = &_control_data;
-						break;
-
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
-						_control_data.type = ControlData::Type::Angle;
-						_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-						_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-						_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-						// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
-						_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
-						// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
-						_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
-						// both specs have yaw on channel 2
-						_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
-
-						// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
-						if (_control_data.type_data.angle.angles[2] > M_PI_F) {
-							_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
-						}
-
-						*control_data = &_control_data;
-						break;
-
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
-						break;
-
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
-						control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4);
-
-						*control_data = &_control_data;
-						break;
-					}
-
-					_ack_vehicle_command(&vehicle_command);
-
-
 				} else {
 					exit_loop = false;
 				}
@@ -942,7 +895,7 @@ void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t
 	vehicle_command_ack.target_system = cmd->source_system;
 	vehicle_command_ack.target_component = cmd->source_component;
 
-	uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 	cmd_ack_pub.publish(vehicle_command_ack);
 }
 

src/modules/vmount/output_mavlink.cpp

@@ -162,7 +162,7 @@ void OutputMavlinkV2::_request_gimbal_device_information()
 	vehicle_cmd.confirmation = 0;
 	vehicle_cmd.from_external = false;
 
-	uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
 	vehicle_command_pub.publish(vehicle_cmd);
 }
 

src/modules/vmount/output_mavlink.h

@@ -64,7 +64,7 @@ public:
 	virtual void print_status();
 
 private:
-	uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 };
 
 class OutputMavlinkV2 : public OutputBase