diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8ecbf2eecc..96d857a0d5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 6ed2950e2d..8922860d28 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/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_pub{ORB_ID(follow_target)}; uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; - uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; - uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; - uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; + uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 2308b06eae..e6fee59e94 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/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 cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; cmd_ack_pub.publish(vehicle_command_ack); } diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 8a82b378f3..b91ffa4851 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/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_pub{ORB_ID(vehicle_command)}; + uORB::Publication vehicle_command_pub{ORB_ID(vehicle_command)}; vehicle_command_pub.publish(vehicle_cmd); } diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 5cea1c953d..89eb4a67c4 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -64,7 +64,7 @@ public: virtual void print_status(); private: - uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; }; class OutputMavlinkV2 : public OutputBase