From e6b1775bb654a7e4bbd8961412376e87848e64b6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 9 Oct 2020 18:12:53 +0200 Subject: [PATCH] vmount/navigator/mavlink: gimbal v2 changes This is a collection of commits all having to do with changes in the Mavlink gimbal v2 protocol as described in: https://mavlink.io/en/services/gimbal_v2.html --- ROMFS/px4fmu_common/init.d-posix/rcS | 4 +- msg/CMakeLists.txt | 1 + msg/gimbal_device_information.msg | 12 +- msg/gimbal_manager_set_attitude.msg | 7 +- msg/gimbal_manager_set_manual_control.msg | 21 + msg/gimbal_manager_status.msg | 4 + msg/vehicle_command.msg | 4 +- src/modules/commander/Commander.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 106 ++++ src/modules/mavlink/mavlink_mission.cpp | 6 + src/modules/mavlink/mavlink_receiver.cpp | 42 +- src/modules/mavlink/mavlink_receiver.h | 5 + src/modules/navigator/mission.cpp | 4 +- src/modules/navigator/mission_block.cpp | 8 +- .../navigator/mission_feasibility_checker.cpp | 6 + src/modules/navigator/navigation.h | 3 + src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 24 + src/modules/vmount/common.h | 17 +- src/modules/vmount/input.h | 3 +- src/modules/vmount/input_mavlink.cpp | 463 ++++++++++++------ src/modules/vmount/input_mavlink.h | 25 +- src/modules/vmount/input_rc.cpp | 11 +- src/modules/vmount/input_test.cpp | 14 +- src/modules/vmount/output.cpp | 32 +- src/modules/vmount/output.h | 4 +- src/modules/vmount/output_mavlink.cpp | 137 ++++-- src/modules/vmount/output_mavlink.h | 13 +- src/modules/vmount/output_rc.cpp | 5 +- src/modules/vmount/vmount.cpp | 69 ++- src/modules/vmount/vmount_params.c | 20 + 31 files changed, 790 insertions(+), 285 deletions(-) create mode 100644 msg/gimbal_manager_set_manual_control.msg diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index c4e8b48530..5b65256e2d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -243,7 +243,7 @@ then sh etc/init.d-posix/rc.mavlink_override else # GCS link - mavlink start -x -u $udp_gcs_port_local -r 4000000 + mavlink start -x -u $udp_gcs_port_local -r 4000000 -f mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local @@ -255,7 +255,7 @@ else mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local # API/Offboard link - mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote + mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote # Onboard link to camera mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ab36952df7..bf80aa6fbf 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -68,6 +68,7 @@ set(msg_files geofence_result.msg gimbal_device_set_attitude.msg gimbal_manager_set_attitude.msg + gimbal_manager_set_manual_control.msg gimbal_device_attitude_status.msg gimbal_device_information.msg gimbal_manager_information.msg diff --git a/msg/gimbal_device_information.msg b/msg/gimbal_device_information.msg index 47d9cee6a2..fb899a241c 100644 --- a/msg/gimbal_device_information.msg +++ b/msg/gimbal_device_information.msg @@ -18,10 +18,10 @@ uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 -float32 tilt_max # [rad] -float32 tilt_min # [rad] -float32 tilt_rate_max # [rad / s] +float32 pitch_max # [rad] +float32 pitch_min # [rad] -float32 pan_max # [rad] -float32 pan_min # [rad] -float32 pan_rate_max # [rad / s] +float32 yaw_max # [rad] +float32 yaw_min # [rad] + +uint8 gimbal_device_compid diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/gimbal_manager_set_attitude.msg index 6ec9e76141..d88acca8b6 100644 --- a/msg/gimbal_manager_set_attitude.msg +++ b/msg/gimbal_manager_set_attitude.msg @@ -1,5 +1,8 @@ uint64 timestamp # time since system start (microseconds) +uint8 origin_sysid +uint8 origin_compid + uint8 target_system uint8 target_component @@ -8,10 +11,6 @@ uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 -uint32 GIMBAL_MANAGER_FLAGS_ANGULAR_VELOCITY_RELATIVE_TO_FOCAL_LENGTH = 1048576 -uint32 GIMBAL_MANAGER_FLAGS_NUDGE = 2097152 -uint32 GIMBAL_MANAGER_FLAGS_OVERRIDE = 4194304 -uint32 GIMBAL_MANAGER_FLAGS_NONE = 8388608 uint32 flags uint8 gimbal_device_id diff --git a/msg/gimbal_manager_set_manual_control.msg b/msg/gimbal_manager_set_manual_control.msg new file mode 100644 index 0000000000..4061438f72 --- /dev/null +++ b/msg/gimbal_manager_set_manual_control.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32 pitch # unitless -1..1, can be NAN +float32 yaw # unitless -1..1, can be NAN +float32 pitch_rate # unitless -1..1, can be NAN +float32 yaw_rate # unitless -1..1, can be NAN diff --git a/msg/gimbal_manager_status.msg b/msg/gimbal_manager_status.msg index f672bc1121..002e5c90e7 100644 --- a/msg/gimbal_manager_status.msg +++ b/msg/gimbal_manager_status.msg @@ -2,3 +2,7 @@ uint64 timestamp # time since system start (microseconds) uint32 flags uint8 gimbal_device_id +uint8 primary_control_sysid +uint8 primary_control_compid +uint8 secondary_control_sysid +uint8 secondary_control_compid diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 0989cd96d4..6cf61a0af5 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -75,7 +75,9 @@ uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom -uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal attitude +uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c85c62b7c8..38797b8545 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1258,6 +1258,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL: case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE: case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM: + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS: case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: case vehicle_command_s::VEHICLE_CMD_DO_LAND_START: case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 82eba94c69..22839ffda1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1482,6 +1482,111 @@ protected: } }; +class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamAutopilotStateForGimbalDevice::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink); + } + + unsigned get_size() override + { + return _att_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _est_sub{ORB_ID(estimator_status)}; + uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + + /* do not allow top copying this class */ + MavlinkStreamAutopilotStateForGimbalDevice(MavlinkStreamAutopilotStateForGimbalDevice &) = delete; + MavlinkStreamAutopilotStateForGimbalDevice &operator = (const MavlinkStreamAutopilotStateForGimbalDevice &) = delete; + +protected: + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + if (_att_sub.advertised()) { + + mavlink_autopilot_state_for_gimbal_device_t msg{}; + + { + vehicle_attitude_s att{}; + _att_sub.copy(&att); + msg.time_boot_us = att.timestamp; + msg.q[0] = att.q[0]; + msg.q[1] = att.q[1]; + msg.q[2] = att.q[2]; + msg.q[3] = att.q[3]; + msg.q_estimated_delay_us = 0; // I don't know. + } + + { + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + msg.vx = lpos.vx; + msg.vy = lpos.vy; + msg.vz = lpos.vz; + msg.v_estimated_delay_us = 0; // I don't know. + } + + { + vehicle_attitude_setpoint_s att_sp{}; + _att_sp_sub.copy(&att_sp); + msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate; + } + + { + estimator_status_s est{}; + _est_sub.copy(&est); + msg.estimator_status = est.solution_status_flags; + } + + { + vehicle_land_detected_s land_detected{}; + _landed_sub.copy(&land_detected); + + // Ignore take-off and landing states for now. + msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR; + } + + mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + + class MavlinkStreamSystemTime : public MavlinkStream { public: @@ -3342,6 +3447,7 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), + create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), create_stream_list_item >(), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5d50dc2c29..c823487da2 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1475,6 +1475,9 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_SET_CAMERA_FOCUS: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case MAV_CMD_IMAGE_START_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_VIDEO_START_CAPTURE: @@ -1574,12 +1577,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_VTOL_TRANSITION: break; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8f252a3cf4..6de057a277 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -283,6 +283,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_gimbal_manager_set_attitude(msg); break; + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(msg); + break; + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: handle_message_gimbal_device_information(msg); break; @@ -2946,16 +2950,39 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) } } +void +MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg; + mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg); + + gimbal_manager_set_manual_control_s set_manual_control{}; + set_manual_control.timestamp = hrt_absolute_time(); + set_manual_control.origin_sysid = msg->sysid; + set_manual_control.origin_compid = msg->compid; + set_manual_control.target_system = set_manual_control_msg.target_system; + set_manual_control.target_component = set_manual_control_msg.target_component; + set_manual_control.flags = set_manual_control_msg.flags; + set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id; + + set_manual_control.pitch = set_manual_control_msg.pitch; + set_manual_control.yaw = set_manual_control_msg.yaw; + set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate; + set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate; + + _gimbal_manager_set_manual_control_pub.publish(set_manual_control); +} void MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg) { - mavlink_gimbal_manager_set_attitude_t set_attitude_msg; mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg); gimbal_manager_set_attitude_s gimbal_attitude{}; gimbal_attitude.timestamp = hrt_absolute_time(); + gimbal_attitude.origin_sysid = msg->sysid; + gimbal_attitude.origin_compid = msg->compid; gimbal_attitude.target_system = set_attitude_msg.target_system; gimbal_attitude.target_component = set_attitude_msg.target_component; gimbal_attitude.flags = set_attitude_msg.flags; @@ -2969,7 +2996,6 @@ MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *m gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z; _gimbal_manager_set_attitude_pub.publish(gimbal_attitude); - } void @@ -2994,13 +3020,13 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version; gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags; - gimbal_information.tilt_max = gimbal_device_info_msg.tilt_max; - gimbal_information.tilt_min = gimbal_device_info_msg.tilt_min; - gimbal_information.tilt_rate_max = gimbal_device_info_msg.tilt_rate_max; + gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max; + gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min; - gimbal_information.pan_max = gimbal_device_info_msg.pan_max; - gimbal_information.pan_min = gimbal_device_info_msg.pan_min; - gimbal_information.pan_rate_max = gimbal_device_info_msg.pan_rate_max; + 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_device_information_pub.publish(gimbal_information); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f252707e6b..6ed2950e2d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -205,6 +206,7 @@ private: 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); #if !defined(CONSTRAINED_FLASH) @@ -357,6 +359,9 @@ private: uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; 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)}; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d436e740fe..3fce626d15 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -146,13 +146,15 @@ void Mission::on_inactivation() { // Disable camera trigger - vehicle_command_s cmd = {}; + vehicle_command_s cmd {}; cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; // Pause trigger cmd.param1 = -1.0f; cmd.param3 = 1.0f; _navigator->publish_vehicle_cmd(&cmd); + _navigator->release_gimbal_control(); + if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a37c2226c1..6230d8141f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -96,6 +96,8 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: @@ -105,6 +107,7 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: return true; case NAV_CMD_DO_VTOL_TRANSITION: @@ -472,7 +475,10 @@ MissionBlock::issue_command(const mission_item_s &item) } else { _action_start = hrt_absolute_time(); - // mission_item -> vehicle_command + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } // we're expecting a mission command item here so assign the "raw" inputs to the command // (MAV_FRAME_MISSION mission item) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 39e93dbdb8..886831e8fa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -262,6 +262,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && @@ -271,6 +273,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1), @@ -389,6 +392,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && @@ -398,6 +403,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); } } diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 50cd362f26..5c6251d603 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -86,6 +86,9 @@ enum NAV_CMD { NAV_CMD_OBLIQUE_SURVEY = 260, NAV_CMD_SET_CAMERA_MODE = 530, NAV_CMD_SET_CAMERA_ZOOM = 531, + NAV_CMD_SET_CAMERA_FOCUS = 532, + NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000, + NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001, NAV_CMD_IMAGE_START_CAPTURE = 2000, NAV_CMD_IMAGE_STOP_CAPTURE = 2001, NAV_CMD_DO_TRIGGER_CONTROL = 2003, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index af96b4ce45..12a40909a0 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -302,6 +302,9 @@ public: bool force_vtol(); + void acquire_gimbal_control(); + void release_gimbal_control(); + private: DEFINE_PARAMETERS( (ParamFloat) _param_nav_loiter_rad, /**< loiter radius for fixedwing */ @@ -403,7 +406,6 @@ private: float _mission_cruising_speed_fw{-1.0f}; float _mission_throttle{NAN}; - bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern // if mission mode is inactive, this flag will be cleared after 2 seconds diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1f7c5debae..06c6cf45a1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1374,6 +1374,30 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res _vehicle_cmd_ack_pub.publish(command_ack); } +void +Navigator::acquire_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = _vstatus.system_id; + vcmd.param2 = _vstatus.component_id; + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + +void +Navigator::release_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = -3.0f; // Remove control if it had it. + vcmd.param2 = -3.0f; // Remove control if it had it. + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + int Navigator::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/vmount/common.h b/src/modules/vmount/common.h index a1b3a6eece..d4ecfd8c0c 100644 --- a/src/modules/vmount/common.h +++ b/src/modules/vmount/common.h @@ -41,11 +41,9 @@ #include - namespace vmount { - /** * @struct ControlData * This defines the common API between an input and an output of the vmount driver. @@ -58,7 +56,6 @@ struct ControlData { Neutral = 0, /**< move to neutral position */ Angle, /**< control the roll, pitch & yaw angle directly */ LonLat /**< control via lon, lat */ - //TODO: add more, like smooth curve, ... ? }; @@ -66,13 +63,15 @@ struct ControlData { union TypeData { struct TypeAngle { - float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] or rad/s (if is angular rate) */ + float q[4]; /**< attitude quaternion */ + float angular_velocity[3]; // angular velocity + // according to DO_MOUNT_CONFIGURE enum class Frame : uint8_t { - AngleBodyFrame = 0, /**< Angle in body frame. */ - AngularRate, /**< Angular rate in rad/s. */ - AngleAbsoluteFrame /**< Angle in absolute frame. */ - } frames[3]; /**< Frame of given angle. */ + AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */ + AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */ + AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */ + } frames[3]; /**< Mode. */ } angle; struct TypeLonLat { @@ -89,10 +88,8 @@ struct ControlData { bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis (if the output supports it, this can also be done externally) */ - bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */ }; - } /* namespace vmount */ diff --git a/src/modules/vmount/input.h b/src/modules/vmount/input.h index 9c2b2603e4..2a7c113219 100644 --- a/src/modules/vmount/input.h +++ b/src/modules/vmount/input.h @@ -52,7 +52,8 @@ namespace vmount class InputBase { public: - virtual ~InputBase() {} + InputBase() = default; + virtual ~InputBase() = default; /** * Wait for an input update, with a timeout. diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 710bbab523..ec9e9134c4 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include #include @@ -176,17 +178,6 @@ void InputMavlinkROI::print_status() InputMavlinkCmdMount::InputMavlinkCmdMount() { - param_t handle = param_find("MAV_SYS_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_sys_id); - } - - handle = param_find("MAV_COMP_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_comp_id); - } } InputMavlinkCmdMount::~InputMavlinkCmdMount() @@ -273,24 +264,30 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con *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; + 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; - // 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; + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F; + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F; + // both specs have yaw on channel 2 + float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F; + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *control_data = &_control_data; } - - *control_data = &_control_data; break; case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: @@ -374,25 +371,14 @@ void InputMavlinkCmdMount::print_status() PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } -InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device) +InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, + float &mnt_rate_pitch, float &mnt_rate_yaw) : + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id), + _mnt_rate_pitch(mnt_rate_pitch), + _mnt_rate_yaw(mnt_rate_yaw) { - param_t handle = param_find("MAV_SYS_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_sys_id); - } - - handle = param_find("MAV_COMP_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_comp_id); - } - - if (has_v2_gimbal_device) { - /* smart gimbal: ask GIMBAL_DEVICE_INFORMATION to it */ - _request_gimbal_device_information(); - - } else { + if (!has_v2_gimbal_device) { /* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */ _stream_gimbal_manager_information(); } @@ -415,6 +401,10 @@ InputMavlinkGimbalV2::~InputMavlinkGimbalV2() if (_vehicle_command_sub >= 0) { orb_unsubscribe(_vehicle_command_sub); } + + if (_gimbal_manager_set_manual_control_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_manual_control_sub); + } } @@ -447,6 +437,10 @@ int InputMavlinkGimbalV2::initialize() return -errno; } + if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) { + return -errno; + } + // rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode, // it will publish vehicle_command's as well, causing the input poll() in here to return // immediately, which in turn will cause an output update and thus a busy loop. @@ -457,17 +451,21 @@ int InputMavlinkGimbalV2::initialize() void InputMavlinkGimbalV2::_stream_gimbal_manager_status() { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; if (_gimbal_device_attitude_status_sub.updated()) { - _gimbal_device_attitude_status_sub.copy(&_gimbal_device_attitude_status); + _gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status); } 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.flags = gimbal_device_attitude_status.device_flags; gimbal_manager_status.gimbal_device_id = 0; + gimbal_manager_status.primary_control_sysid = _sys_id_primary_control; + gimbal_manager_status.primary_control_compid = _comp_id_primary_control; + gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control + gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control _gimbal_manager_status_pub.publish(gimbal_manager_status); - } void InputMavlinkGimbalV2::_stream_gimbal_manager_information() @@ -489,33 +487,14 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information() gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK; - gimbal_device_info.tilt_max = M_PI_F / 2; - gimbal_device_info.tilt_min = -M_PI_F / 2; - gimbal_device_info.tilt_rate_max = 1; - gimbal_device_info.pan_max = M_PI_F; - gimbal_device_info.pan_min = -M_PI_F; - gimbal_device_info.pan_rate_max = 1; + gimbal_device_info.pitch_max = M_PI_F / 2; + gimbal_device_info.pitch_min = -M_PI_F / 2; + gimbal_device_info.yaw_max = M_PI_F; + gimbal_device_info.yaw_min = -M_PI_F; _gimbal_device_info_pub.publish(gimbal_device_info); } -void InputMavlinkGimbalV2::_request_gimbal_device_information() -{ - vehicle_command_s vehicle_cmd{}; - vehicle_cmd.timestamp = hrt_absolute_time(); - vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE; - vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION; - vehicle_cmd.target_system = 0; - vehicle_cmd.target_component = 0; - vehicle_cmd.source_system = _mav_sys_id; - vehicle_cmd.source_component = _mav_comp_id; - vehicle_cmd.confirmation = 0; - vehicle_cmd.from_external = false; - - uORB::PublicationQueued vehicle_command_pub{ORB_ID(vehicle_command)}; - vehicle_command_pub.publish(vehicle_cmd); -} - int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { _stream_gimbal_manager_status(); @@ -523,7 +502,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con // Default to no change, set if we receive anything. *control_data = nullptr; - const int num_poll = 4; + const int num_poll = 5; px4_pollfd_struct_t polls[num_poll]; polls[0].fd = _gimbal_manager_set_attitude_sub; polls[0].events = POLLIN; @@ -533,6 +512,8 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con polls[2].events = POLLIN; polls[3].fd = _vehicle_command_sub; polls[3].events = POLLIN; + polls[4].fd = _gimbal_manager_set_manual_control_sub; + polls[4].events = POLLIN; int poll_timeout = (int)timeout_ms; @@ -560,13 +541,20 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con gimbal_manager_set_attitude_s set_attitude; orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); - const float pitch = matrix::Eulerf(matrix::Quatf(set_attitude.q)).phi(); // rad - const float roll = matrix::Eulerf(matrix::Quatf(set_attitude.q)).theta(); - const float yaw = matrix::Eulerf(matrix::Quatf(set_attitude.q)).psi(); + if (set_attitude.origin_sysid == _sys_id_primary_control && + set_attitude.origin_compid == _comp_id_primary_control) { + const matrix::Quatf q(set_attitude.q); + const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, + set_attitude.angular_velocity_y, + set_attitude.angular_velocity_z); - _set_control_data_from_set_attitude(set_attitude.flags, pitch, set_attitude.angular_velocity_y, yaw, - set_attitude.angular_velocity_z, roll, set_attitude.angular_velocity_x); - *control_data = &_control_data; + _set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_WARN("Ignoring gimbal_manager_set_attitude from %d/%d", + set_attitude.origin_sysid, set_attitude.origin_compid); + } } if (polls[1].revents & POLLIN) { @@ -642,16 +630,233 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con continue; } - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE) { - _set_control_data_from_set_attitude((uint32_t)vehicle_command.param5, vehicle_command.param3, vehicle_command.param1, - vehicle_command.param3, vehicle_command.param2); + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + // FIXME: Remove me later. For now, we support this for legacy missions + // using gimbal v1 protocol. + + 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 + const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F; + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F; + // both specs have yaw on channel 2 + float yaw = 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 (yaw > M_PI_F) { + yaw -= 2 * M_PI_F; + } + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *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, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; + _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; + _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + + const int params[] = { + (int)((float)vehicle_command.param5 + 0.5f), + (int)((float)vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + _control_data.type = ControlData::Type::Neutral; //always switch to neutral position + *control_data = &_control_data; - _ack_vehicle_command(&vehicle_command); + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + + const int param_sys_id = roundf(vehicle_command.param1); + const int param_comp_id = roundf(vehicle_command.param2); + + uint8_t new_sys_id_primary_control = [&]() { + if (param_sys_id >= 0 && param_sys_id < 256) { + // Valid new sysid. + return (uint8_t)param_sys_id; + + } else if (param_sys_id == -1) { + // leave unchanged + return _sys_id_primary_control; + + } else if (param_sys_id == -2) { + // set itself + return _mav_sys_id; + + } else if (param_sys_id == -3) { + // release control if in control + if (_sys_id_primary_control == vehicle_command.source_system) { + return (uint8_t)0; + + } else { + return _sys_id_primary_control; + } + + } else { + PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _sys_id_primary_control; + } + }(); + + uint8_t new_comp_id_primary_control = [&]() { + if (param_comp_id >= 0 && param_comp_id < 256) { + // Valid new compid. + return (uint8_t)param_comp_id; + + } else if (param_comp_id == -1) { + // leave unchanged + return _comp_id_primary_control; + + } else if (param_comp_id == -2) { + // set itself + return _mav_comp_id; + + } else if (param_comp_id == -3) { + // release control if in control + if (_comp_id_primary_control == vehicle_command.source_component) { + return (uint8_t)0; + + } else { + return _comp_id_primary_control; + } + + } else { + PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _comp_id_primary_control; + } + }(); + + + if (new_sys_id_primary_control != _sys_id_primary_control || + new_comp_id_primary_control != _comp_id_primary_control) { + PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", + _sys_id_primary_control, _comp_id_primary_control, + new_sys_id_primary_control, new_comp_id_primary_control); + _sys_id_primary_control = new_sys_id_primary_control; + _comp_id_primary_control = new_comp_id_primary_control; + } + + // TODO: support secondary control + // TODO: support gimbal device id for multiple gimbals + + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { + + if (vehicle_command.source_system == _sys_id_primary_control && + vehicle_command.source_component == _comp_id_primary_control) { + + const matrix::Eulerf euler(0.0f, vehicle_command.param1 * M_DEG_TO_RAD_F, vehicle_command.param2 * M_DEG_TO_RAD_F); + const matrix::Quatf q(euler); + const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); + const uint32_t flags = vehicle_command.param5; + + // TODO: support gimbal device id for multiple gimbals + + _set_control_data_from_set_attitude(flags, q, angular_velocity); + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + PX4_ERR("GIMBAL_MANAGER_PITCHYAW denied, not in control"); + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } } else { exit_loop = false; } } + + if (polls[4].revents & POLLIN) { + gimbal_manager_set_manual_control_s set_manual_control; + orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control); + + if (set_manual_control.origin_sysid == _sys_id_primary_control && + set_manual_control.origin_compid == _comp_id_primary_control) { + + const matrix::Quatf q = + (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? + matrix::Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) : + matrix::Quatf(NAN, NAN, NAN, NAN); + + const matrix::Vector3f angular_velocity = + (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? + matrix::Vector3f(0.0f, + _mnt_rate_pitch * M_DEG_TO_RAD_F * set_manual_control.pitch_rate, + _mnt_rate_yaw * M_DEG_TO_RAD_F * set_manual_control.yaw_rate) : + matrix::Vector3f(NAN, NAN, NAN); + + _set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_WARN("Ignoring gimbal_manager_set_manual_control from %d/%d", + set_manual_control.origin_sysid, set_manual_control.origin_compid); + } + } } } @@ -671,25 +876,30 @@ void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, con _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.angles[0] = 0.f; + const float roll = 0.0f; + float pitch = 0.0f; + float yaw = 0.0f; // interface: use fixed pitch value > -pi otherwise consider ROI altitude if (_control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) { - _control_data.type_data.angle.angles[1] = _control_data.type_data.lonlat.pitch_fixed_angle; + pitch = _control_data.type_data.lonlat.pitch_fixed_angle; } else { - _control_data.type_data.angle.angles[1] = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position); + pitch = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position); } - _control_data.type_data.angle.angles[2] = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, - roi_lon) - vehicle_global_position.yaw; + yaw = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, roi_lon) - vehicle_local_position.yaw; // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET - _control_data.type_data.angle.angles[1] += _control_data.type_data.lonlat.pitch_angle_offset; - _control_data.type_data.angle.angles[2] += _control_data.type_data.lonlat.yaw_angle_offset; + pitch += _control_data.type_data.lonlat.pitch_angle_offset; + yaw += _control_data.type_data.lonlat.yaw_angle_offset; // make sure yaw is wrapped correctly for the output - _control_data.type_data.angle.angles[2] = wrap_pi(_control_data.type_data.angle.angles[2]); + yaw = wrap_pi(yaw); + + matrix::Eulerf euler(roll, pitch, yaw); + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); } float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude, @@ -718,81 +928,48 @@ void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double & alt_sp = (double)position_setpoint_triplet.current.alt; } -void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle, - const float pitch_rate, const float yaw_angle, const float yaw_rate, float roll_angle, float roll_rate) +void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity) { - 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) { _control_data.type = ControlData::Type::Neutral; - } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NONE) != 0) { - // don't do anything } else { _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; - if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NUDGE) != 0) { - // add set_attitude.q to existing tracking angle or ROI - // track message not yet implemented - _control_data.type_data.angle.angles[0] += pitch_angle; - _control_data.type_data.angle.angles[1] += roll_angle; - _control_data.type_data.angle.angles[2] += yaw_angle; + _control_data.type_data.angle.q[0] = q(0); + _control_data.type_data.angle.q[1] = q(1); + _control_data.type_data.angle.q[2] = q(2); + _control_data.type_data.angle.q[3] = q(3); - } else { - _control_data.type_data.angle.angles[0] = pitch_angle; - _control_data.type_data.angle.angles[1] = roll_angle; - _control_data.type_data.angle.angles[2] = yaw_angle; - } + _control_data.type_data.angle.angular_velocity[0] = angular_velocity(0); + _control_data.type_data.angle.angular_velocity[1] = angular_velocity(1); + _control_data.type_data.angle.angular_velocity[2] = angular_velocity(2); - if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_OVERRIDE) != 0) { - // overides tracking or ROI angle with set_attitude.q, respects flag GIMBAL_MANAGER_FLAGS_YAW_LOCK - _control_data.type_data.angle.angles[0] = pitch_angle; - _control_data.type_data.angle.angles[1] = roll_angle; - _control_data.type_data.angle.angles[2] = yaw_angle; - } + _control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - if (PX4_ISFINITE(roll_rate)) { //roll - _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate; - _control_data.type_data.angle.angles[0] = roll_rate; //rad/s - } + _control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - if (PX4_ISFINITE(pitch_rate)) { //pitch - _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate; - _control_data.type_data.angle.angles[1] = pitch_rate; //rad/s - } - - if (PX4_ISFINITE(yaw_rate)) { //yaw - _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate; - _control_data.type_data.angle.angles[2] = yaw_rate; //rad/s - } - - if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) { - // stay horizontal with the horizon - _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - } - - if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) { - _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - } - - if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) { - _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - } + _control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; } } //TODO move this one to input.cpp such that it can be shared across functions -void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd) +void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t result) { vehicle_command_ack_s vehicle_command_ack{}; vehicle_command_ack.timestamp = hrt_absolute_time(); vehicle_command_ack.command = cmd->command; - vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + vehicle_command_ack.result = result; vehicle_command_ack.target_system = cmd->source_system; vehicle_command_ack.target_component = cmd->source_component; diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index 2a70284294..0a46a8f890 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -45,7 +45,6 @@ #include #include -#include #include #include #include @@ -110,7 +109,8 @@ private: class InputMavlinkGimbalV2 : public InputBase { public: - InputMavlinkGimbalV2(bool has_v2_gimbal_device); + InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, + float &mnt_rate_yaw); virtual ~InputMavlinkGimbalV2(); virtual void print_status(); @@ -125,20 +125,26 @@ private: float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position); void _read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp); - void _set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle, const float pitch_rate, - const float yaw_angle, const float yaw_rate, float roll_angle = 0, float roll_rate = 0); - void _ack_vehicle_command(vehicle_command_s *cmd); + void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity); + void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result); void _stream_gimbal_manager_information(); - void _request_gimbal_device_information(); void _stream_gimbal_manager_status(); int _vehicle_roi_sub = -1; int _gimbal_manager_set_attitude_sub = -1; + int _gimbal_manager_set_manual_control_sub = -1; int _position_setpoint_triplet_sub = -1; int _vehicle_command_sub = -1; - int32_t _mav_sys_id{1}; ///< our mavlink system id - int32_t _mav_comp_id{1}; ///< our mavlink component id + uint8_t _mav_sys_id{1}; ///< our mavlink system id + uint8_t _mav_comp_id{1}; ///< our mavlink component id + + uint8_t _sys_id_primary_control{0}; + uint8_t _comp_id_primary_control{0}; + + float &_mnt_rate_pitch; + float &_mnt_rate_yaw; bool _is_roi_set{false}; @@ -148,9 +154,6 @@ private: uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; - - gimbal_device_attitude_status_s _gimbal_device_attitude_status{}; - }; } /* namespace vmount */ diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index df1311270b..de5a5d8633 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -130,10 +131,14 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo _first_time = false; - for (int i = 0; i < 3; ++i) { - control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F; + matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F, + new_aux_values[2] * M_PI_F); + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); + for (int i = 0; i < 3; ++i) { + // We always use follow mode with RC input for now. + control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; _last_set_aux_values[i] = new_aux_values[i]; } diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index ef8cc947bc..bbe0e49cc3 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -42,6 +42,7 @@ #include #include +#include namespace vmount @@ -69,9 +70,16 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - for (int i = 0; i < 3; ++i) { - _control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F; - } + matrix::Eulerf euler( + _angles[0] * M_DEG_TO_RAD_F, + _angles[1] * M_DEG_TO_RAD_F, + _angles[2] * M_DEG_TO_RAD_F); + matrix::Quatf q(euler); + + _control_data.type_data.angle.q[0] = q(0); + _control_data.type_data.angle.q[1] = q(1); + _control_data.type_data.angle.q[2] = q(2); + _control_data.type_data.angle.q[3] = q(3); _control_data.gimbal_shutter_retract = false; *control_data = &_control_data; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 82c2cdc929..29c03e05fa 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -108,21 +108,27 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) switch (control_data->type) { case ControlData::Type::Angle: - for (int i = 0; i < 3; ++i) { - switch (control_data->type_data.angle.frames[i]) { - case ControlData::TypeData::TypeAngle::Frame::AngularRate: - _angle_speeds[i] = control_data->type_data.angle.angles[i]; - break; - case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame: - _absolute_angle[i] = false; - _angle_setpoints[i] = control_data->type_data.angle.angles[i]; - break; + { + matrix::Quatf q(control_data->type_data.angle.q); + matrix::Eulerf euler(q); - case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame: - _absolute_angle[i] = true; - _angle_setpoints[i] = control_data->type_data.angle.angles[i]; - break; + for (int i = 0; i < 3; ++i) { + switch (control_data->type_data.angle.frames[i]) { + case ControlData::TypeData::TypeAngle::Frame::AngularRate: + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame: + _absolute_angle[i] = false; + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame: + _absolute_angle[i] = true; + break; + } + + _angle_speeds[i] = control_data->type_data.angle.angular_velocity[i]; + _angle_setpoints[i] = euler(i); } } diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index 7356ec9edc..14011707ee 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -67,8 +67,8 @@ struct OutputConfig { float roll_offset; /**< Offset for roll channel in radians */ float yaw_offset; /**< Offset for yaw channel in radians */ - uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */ - uint32_t mavlink_comp_id; + uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */ + uint32_t mavlink_comp_id_v1; }; diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 2ed277ff4b..9cda74650f 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -59,8 +59,8 @@ int OutputMavlinkV1::update(const ControlData *control_data) { vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); - vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id; - vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id; + vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1; + vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1; if (control_data) { //got new command @@ -118,29 +118,68 @@ void OutputMavlinkV1::print_status() PX4_INFO("Output: MAVLink gimbal protocol v1"); } -OutputMavlinkV2::OutputMavlinkV2(const OutputConfig &output_config) - : OutputBase(output_config) +OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config) + : OutputBase(output_config), + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id) { } int OutputMavlinkV2::update(const ControlData *control_data) { - if (control_data) { - //got new command - _set_angle_setpoints(control_data); - _publish_gimbal_device_set_attitude(control_data); - } - - _handle_position_update(); + _check_for_gimbal_device_information(); hrt_abstime t = hrt_absolute_time(); - _calculate_output_angles(t); - _last_update = t; + if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) { + _request_gimbal_device_information(); + _last_gimbal_device_checked = t; + + } else { + // Only start sending attitude setpoints once a device has been discovered. + + if (control_data) { + //got new command + _set_angle_setpoints(control_data); + _publish_gimbal_device_set_attitude(control_data); + } + + _handle_position_update(); + + _calculate_output_angles(t); + _last_update = t; + } return 0; } +void OutputMavlinkV2::_request_gimbal_device_information() +{ + vehicle_command_s vehicle_cmd{}; + vehicle_cmd.timestamp = hrt_absolute_time(); + vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE; + vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION; + vehicle_cmd.target_system = 0; + vehicle_cmd.target_component = 0; + vehicle_cmd.source_system = _mav_sys_id; + vehicle_cmd.source_component = _mav_comp_id; + vehicle_cmd.confirmation = 0; + vehicle_cmd.from_external = false; + + uORB::PublicationQueued vehicle_command_pub{ORB_ID(vehicle_command)}; + vehicle_command_pub.publish(vehicle_cmd); +} + +void OutputMavlinkV2::_check_for_gimbal_device_information() +{ + gimbal_device_information_s 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; + } +} + void OutputMavlinkV2::print_status() { PX4_INFO("Output: MAVLink gimbal protocol v2"); @@ -150,50 +189,50 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *con { gimbal_device_set_attitude_s set_attitude{}; set_attitude.timestamp = hrt_absolute_time(); - set_attitude.target_system = (uint8_t)_config.mavlink_sys_id; - set_attitude.target_component = (uint8_t)_config.mavlink_comp_id; + set_attitude.target_system = (uint8_t)_mav_sys_id; + set_attitude.target_component = _gimbal_device_compid; - matrix::Eulerf euler(control_data->type_data.angle.angles[0], control_data->type_data.angle.angles[1], - control_data->type_data.angle.angles[2]); - matrix::Quatf q(euler); - - set_attitude.q[0] = q(0); - set_attitude.q[1] = q(1); - set_attitude.q[2] = q(2); - set_attitude.q[3] = q(3); - - - if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngularRate) { - set_attitude.angular_velocity_x = control_data->type_data.angle.angles[0]; //roll - } - - if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngularRate) { - set_attitude.angular_velocity_y = control_data->type_data.angle.angles[1]; //pitch - - } - - if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngularRate) { - set_attitude.angular_velocity_z = control_data->type_data.angle.angles[2]; - } - - if (control_data->type == ControlData::Type::Neutral) { + switch (control_data->type) { + case ControlData::Type::Neutral: set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL; - } + set_attitude.angular_velocity_x = NAN; + set_attitude.angular_velocity_y = NAN; + set_attitude.angular_velocity_z = NAN; + set_attitude.q[0] = NAN; + set_attitude.q[1] = NAN; + set_attitude.q[2] = NAN; + set_attitude.q[3] = NAN; + break; - if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; - } + case ControlData::Type::Angle: + set_attitude.angular_velocity_x = control_data->type_data.angle.angular_velocity[0]; + set_attitude.angular_velocity_y = control_data->type_data.angle.angular_velocity[1]; + set_attitude.angular_velocity_z = control_data->type_data.angle.angular_velocity[2]; + set_attitude.q[0] = control_data->type_data.angle.q[0]; + set_attitude.q[1] = control_data->type_data.angle.q[1]; + set_attitude.q[2] = control_data->type_data.angle.q[2]; + set_attitude.q[3] = control_data->type_data.angle.q[3]; - if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - } + if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; + } - if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; + if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + } + + if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; + } + + break; + + case ControlData::Type::LonLat: + // FIXME: needs conversion to angle. + break; } _gimbal_device_set_attitude_pub.publish(set_attitude); - } } /* namespace vmount */ diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 8c0a4808a0..934be1a19a 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace vmount @@ -69,7 +70,7 @@ private: class OutputMavlinkV2 : public OutputBase { public: - OutputMavlinkV2(const OutputConfig &output_config); + OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config); virtual ~OutputMavlinkV2() = default; virtual int update(const ControlData *control_data); @@ -78,7 +79,17 @@ public: private: void _publish_gimbal_device_set_attitude(const ControlData *control_data); + void _request_gimbal_device_information(); + void _check_for_gimbal_device_information(); + uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; + + int32_t _mav_sys_id{1}; ///< our mavlink system id + int32_t _mav_comp_id{1}; ///< our mavlink component id + uint8_t _gimbal_device_compid{0}; + hrt_abstime _last_gimbal_device_checked{0}; + bool _gimbal_device_found {false}; }; } /* namespace vmount */ diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index a829d4cb3b..6aaa5b76f8 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/src/modules/vmount/output_rc.cpp @@ -100,10 +100,7 @@ void OutputRC::_stream_device_attitude_status() matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); matrix::Quatf q(euler); - attitude_status.q[0] = q(0); - attitude_status.q[1] = q(1); - attitude_status.q[2] = q(2); - attitude_status.q[3] = q(3); + q.copyTo(attitude_status.q); attitude_status.failure_flags = 0; _attitude_status_pub.publish(attitude_status); diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index 406cf74aff..28b0889dbf 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -88,8 +88,8 @@ static volatile ThreadData *g_thread_data = nullptr; struct Parameters { int32_t mnt_mode_in; int32_t mnt_mode_out; - int32_t mnt_mav_sysid; - int32_t mnt_mav_compid; + int32_t mnt_mav_sys_id_v1; + int32_t mnt_mav_comp_id_v1; float mnt_ob_lock_mode; float mnt_ob_norm_mode; int32_t mnt_man_pitch; @@ -102,6 +102,10 @@ struct Parameters { float mnt_off_pitch; float mnt_off_roll; float mnt_off_yaw; + int32_t mav_sys_id; + int32_t mav_comp_id; + float mnt_rate_pitch; + float mnt_rate_yaw; bool operator!=(const Parameters &p) { @@ -109,8 +113,8 @@ struct Parameters { #pragma GCC diagnostic ignored "-Wfloat-equal" return mnt_mode_in != p.mnt_mode_in || mnt_mode_out != p.mnt_mode_out || - mnt_mav_sysid != p.mnt_mav_sysid || - mnt_mav_compid != p.mnt_mav_compid || + mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 || + mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 || fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f || fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f || mnt_man_pitch != p.mnt_man_pitch || @@ -122,7 +126,9 @@ struct Parameters { mnt_range_yaw != p.mnt_range_yaw || mnt_off_pitch != p.mnt_off_pitch || mnt_off_roll != p.mnt_off_roll || - mnt_off_yaw != p.mnt_off_yaw; + mnt_off_yaw != p.mnt_off_yaw || + mav_sys_id != p.mav_sys_id || + mav_comp_id != p.mav_comp_id; #pragma GCC diagnostic pop } @@ -131,8 +137,8 @@ struct Parameters { struct ParameterHandles { param_t mnt_mode_in; param_t mnt_mode_out; - param_t mnt_mav_sysid; - param_t mnt_mav_compid; + param_t mnt_mav_sys_id_v1; + param_t mnt_mav_comp_id_v1; param_t mnt_ob_lock_mode; param_t mnt_ob_norm_mode; param_t mnt_man_pitch; @@ -145,6 +151,10 @@ struct ParameterHandles { param_t mnt_off_pitch; param_t mnt_off_roll; param_t mnt_off_yaw; + param_t mav_sys_id; + param_t mav_comp_id; + param_t mnt_rate_pitch; + param_t mnt_rate_yaw; }; @@ -229,8 +239,8 @@ static int vmount_thread_main(int argc, char *argv[]) output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F; output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F; output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F; - output_config.mavlink_sys_id = params.mnt_mav_sysid; - output_config.mavlink_comp_id = params.mnt_mav_compid; + output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1; + output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1; bool alloc_failed = false; thread_data.input_objs_len = 1; @@ -271,7 +281,11 @@ static int vmount_thread_main(int argc, char *argv[]) break; case 4: //MAVLINK_V2 - thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2); + thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2, + params.mav_sys_id, + params.mav_comp_id, + params.mnt_rate_pitch, + params.mnt_rate_yaw); break; default: @@ -302,7 +316,7 @@ static int vmount_thread_main(int argc, char *argv[]) break; case 2: //MAVLink v2 gimbal protocol - thread_data.output_obj = new OutputMavlinkV2(output_config); + thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config); if (!thread_data.output_obj) { alloc_failed = true; } @@ -375,9 +389,9 @@ static int vmount_thread_main(int argc, char *argv[]) break; } - //only publish the mount orientation if the mode is not mavlink - //if the gimbal speaks mavlink it publishes its own orientation - if (params.mnt_mode_out != 1) { // 1 = MAVLINK + // Only publish the mount orientation if the mode is not mavlink v1 or v2 + // If the gimbal speaks mavlink it publishes its own orientation. + if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2 thread_data.output_obj->publish(); } @@ -550,8 +564,8 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go Parameters prev_params = params; param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in); param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out); - param_get(param_handles.mnt_mav_sysid, ¶ms.mnt_mav_sysid); - param_get(param_handles.mnt_mav_compid, ¶ms.mnt_mav_compid); + param_get(param_handles.mnt_mav_sys_id_v1, ¶ms.mnt_mav_sys_id_v1); + param_get(param_handles.mnt_mav_comp_id_v1, ¶ms.mnt_mav_comp_id_v1); param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode); param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode); param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); @@ -564,6 +578,10 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll); param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw); + param_get(param_handles.mav_sys_id, ¶ms.mav_sys_id); + param_get(param_handles.mav_comp_id, ¶ms.mav_comp_id); + param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); + param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); got_changes = prev_params != params; } @@ -572,8 +590,8 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) { param_handles.mnt_mode_in = param_find("MNT_MODE_IN"); param_handles.mnt_mode_out = param_find("MNT_MODE_OUT"); - param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID"); - param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID"); + param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID"); + param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID"); param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE"); param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE"); param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); @@ -586,11 +604,15 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH"); param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL"); param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW"); + param_handles.mav_sys_id = param_find("MAV_SYS_ID"); + param_handles.mav_comp_id = param_find("MAV_COMP_ID"); + param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH"); + param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW"); if (param_handles.mnt_mode_in == PARAM_INVALID || param_handles.mnt_mode_out == PARAM_INVALID || - param_handles.mnt_mav_sysid == PARAM_INVALID || - param_handles.mnt_mav_compid == PARAM_INVALID || + param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID || + param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID || param_handles.mnt_ob_lock_mode == PARAM_INVALID || param_handles.mnt_ob_norm_mode == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || @@ -602,7 +624,12 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_range_yaw == PARAM_INVALID || param_handles.mnt_off_pitch == PARAM_INVALID || param_handles.mnt_off_roll == PARAM_INVALID || - param_handles.mnt_off_yaw == PARAM_INVALID) { + param_handles.mnt_off_yaw == PARAM_INVALID || + param_handles.mav_sys_id == PARAM_INVALID || + param_handles.mav_comp_id == PARAM_INVALID || + param_handles.mnt_rate_pitch == PARAM_INVALID || + param_handles.mnt_rate_yaw == PARAM_INVALID + ) { return false; } diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c index 6d067da61b..565b729db2 100644 --- a/src/modules/vmount/vmount_params.c +++ b/src/modules/vmount/vmount_params.c @@ -235,3 +235,23 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f); * @group Mount */ PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); + +/** + * Angular pitch rate for manual input in degrees/second. + * Full stick input [-1..1] translats to [-pitch rate..pitch rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f); + +/** + * Angular yaw rate for manual input in degrees/second. + * Full stick input [-1..1] translats to [-yaw rate..yaw rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);