From 48b00ff67881f4f2cfeffa18980191e38f896fd3 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 12 May 2020 10:32:16 +0200 Subject: [PATCH] Support for gimbal v2 protocol - add command to request a message - add gimbal attitude message mavlink_receiver handle GIMBAL_MANAGER_SET_ATTITUDE first implementation of new vmount input MavlinkGimbalV2 - setup class - decode gimbal_manager_set_attitude in ControlData add gimbal information message add gimbal manager information and vehicle command ack mavlink messages: add stream for GIMBAL_MANAGER_INFORMATION mavlink_receiver: handle GIMBAL_DEVICE_INFORMATION remove mavlink cmd handling from vmount input MavlinkGimbalV2 complete gimbal manager: - send out fake gimbal_device_information for dummy gimbals - complete ROI handling with nudging small fixes fix typos cleanup - gimbal device information - flags lock - check sanity of string add support for CMD_DO_GIMBAL_MANAGER_ATTITUDE stream GimbalDeviceAttitudeStatus for dummy gimbals - add uROB gimbal_attitude_status - fill status in vmount output_rc for dummy gimbals not able to send the status themselves - stream mavlink GimbalDeviceAttitudeStatus better handle the request for gimbal infomation request clean up bring gimbal information back on vmount init add new gimbal message to mavlink normal stream fix publication of gimbal device information rename gimbal_attitude_status to gimbal_device_attitude_status stream gimbal_manager_status at 5Hz mavlink: send information only on request Sending the information message once on request should now work and we don't need to keep publishing it. mavlink: debug output for now make sure to copy over control data mavlink: add missing copyright header, pragma once mavlink: address review comments mavlink: handle stream not updated Our answer does not just depend on whether the stream was found but whether we actually were able to send out an update. mavlink: remove outdated comment vmount: add option for yaw stabilization only The stabilize flag is used for gimbals which do not have an internal IMU and need the autopilot's attitude in order to do stabilization. These gimbals are probably quite rare by now but it makes sense to keep the functionality given it can e.g. be used by simple servo gimbals for sensors other than highres cameras. The stabilize flag can also be re-used for gimbals which are capable of stabilizing pitch and roll but not absolute yaw (e.g. locked to North). For such gimbals we can now set the param MNT_DO_STAB to 2. We still support configuring which axes are stabilized by the MAVLink command DO_MOUNT_CONFIGURE, however, this is generally not recommended anymore. vmount: fix incorrect check for bit flag mavlink_messages: remove debug message Signed-off-by: Claudio Micheli use device id remove debug print gimbal attitude fix mistake clang tidy fix split: - gimbal_attitude -> gimbal_device_set_attitude, gimbal_manager_set_attitude - gimbal_information -> gimbal_device_informatio, gimbal_manager_information add gimbal protocol messages to rtps msg ids support set attitude for gimbal directly speaking mavlink clean up gimbal urob messages vmount: address a few small review comments vmount: split output into v1 and v2 protocol This way we can continue to support the MAVLink v1 protocol. Also, we don't send the old vehicle commands when actually using the new v2 protocol. vmount: config via ctor instead of duplicate param vmount: use loop to poll all topics Otherwise we might give up too soon and miss some data, or run too fast based on commands that have nothing to do with the gimbal. typhoon_h480: use gimbal v2 protocol, use yaw stab Let's by default use the v2 protocol with typhoon_h480 and enable yaw lock mode by stabilizing yaw. --- msg/CMakeLists.txt | 6 + msg/gimbal_device_attitude_status.msg | 18 + msg/gimbal_device_information.msg | 27 ++ msg/gimbal_device_set_attitude.msg | 17 + msg/gimbal_manager_information.msg | 13 + msg/gimbal_manager_set_attitude.msg | 23 ++ msg/gimbal_manager_status.msg | 4 + msg/tools/uorb_rtps_message_ids.yaml | 12 + msg/vehicle_command.msg | 4 + src/modules/mavlink/mavlink_main.cpp | 6 + src/modules/mavlink/mavlink_messages.cpp | 297 +++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 68 ++++ src/modules/mavlink/mavlink_receiver.h | 7 + src/modules/vmount/input_mavlink.cpp | 451 ++++++++++++++++++++++- src/modules/vmount/input_mavlink.h | 61 ++- src/modules/vmount/input_rc.cpp | 6 +- src/modules/vmount/input_rc.h | 6 +- src/modules/vmount/input_test.cpp | 4 +- src/modules/vmount/output.cpp | 37 +- src/modules/vmount/output.h | 8 +- src/modules/vmount/output_mavlink.cpp | 96 ++++- src/modules/vmount/output_mavlink.h | 27 +- src/modules/vmount/output_rc.cpp | 27 +- src/modules/vmount/output_rc.h | 4 + src/modules/vmount/vmount.cpp | 37 +- src/modules/vmount/vmount_params.c | 25 +- 26 files changed, 1221 insertions(+), 70 deletions(-) create mode 100644 msg/gimbal_device_attitude_status.msg create mode 100644 msg/gimbal_device_information.msg create mode 100644 msg/gimbal_device_set_attitude.msg create mode 100644 msg/gimbal_manager_information.msg create mode 100644 msg/gimbal_manager_set_attitude.msg create mode 100644 msg/gimbal_manager_status.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 25f4f0c734..ab36952df7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -66,6 +66,12 @@ set(msg_files follow_target.msg generator_status.msg geofence_result.msg + gimbal_device_set_attitude.msg + gimbal_manager_set_attitude.msg + gimbal_device_attitude_status.msg + gimbal_device_information.msg + gimbal_manager_information.msg + gimbal_manager_status.msg gps_dump.msg gps_inject_data.msg heater_status.msg diff --git a/msg/gimbal_device_attitude_status.msg b/msg/gimbal_device_attitude_status.msg new file mode 100644 index 0000000000..de2d7e892b --- /dev/null +++ b/msg/gimbal_device_attitude_status.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component +uint16 device_flags + +uint16 DEVICE_FLAGS_RETRACT = 1 +uint16 DEVICE_FLAGS_NEUTRAL = 2 +uint16 DEVICE_FLAGS_ROLL_LOCK = 4 +uint16 DEVICE_FLAGS_PITCH_LOCK = 8 +uint16 DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z + +uint32 failure_flags diff --git a/msg/gimbal_device_information.msg b/msg/gimbal_device_information.msg new file mode 100644 index 0000000000..47d9cee6a2 --- /dev/null +++ b/msg/gimbal_device_information.msg @@ -0,0 +1,27 @@ +uint64 timestamp # time since system start (microseconds) + +uint8[32] vendor_name +uint8[32] model_name +uint32 firmware_version + +uint16 capability_flags + +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128 +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 pan_max # [rad] +float32 pan_min # [rad] +float32 pan_rate_max # [rad / s] diff --git a/msg/gimbal_device_set_attitude.msg b/msg/gimbal_device_set_attitude.msg new file mode 100644 index 0000000000..f224a23441 --- /dev/null +++ b/msg/gimbal_device_set_attitude.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component + +uint16 flags +uint32 GIMBAL_DEVICE_FLAGS_RETRACT = 1 +uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/msg/gimbal_manager_information.msg b/msg/gimbal_manager_information.msg new file mode 100644 index 0000000000..bb8b25de1c --- /dev/null +++ b/msg/gimbal_manager_information.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 capability_flags + +uint8 gimbal_device_id + +float32 tilt_max # [rad] +float32 tilt_min # [rad] +float32 tilt_rate_max # [rad / s] + +float32 pan_max # [rad] +float32 pan_min # [rad] +float32 pan_rate_max # [rad / s] diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/gimbal_manager_set_attitude.msg new file mode 100644 index 0000000000..6ec9e76141 --- /dev/null +++ b/msg/gimbal_manager_set_attitude.msg @@ -0,0 +1,23 @@ +uint64 timestamp # time since system start (microseconds) + +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 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 + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/msg/gimbal_manager_status.msg b/msg/gimbal_manager_status.msg new file mode 100644 index 0000000000..f672bc1121 --- /dev/null +++ b/msg/gimbal_manager_status.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 flags +uint8 gimbal_device_id diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 9727a1886e..b57bd5c8a0 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -315,6 +315,18 @@ rtps: id: 149 - msg: heater_status id: 150 + - msg: gimbal_device_attitude_status + id: 151 + - msg: gimbal_device_information + id: 152 + - msg: gimbal_device_set_attitude + id: 153 + - msg: gimbal_manager_information + id: 154 + - msg: gimbal_manager_set_attitude + id: 155 + - msg: gimbal_manager_status + id: 156 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 9b73b96faa..0989cd96d4 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -66,12 +66,16 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| +uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal + uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +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_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/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a8fbf7454f..0814e14b19 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1587,6 +1587,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ESTIMATOR_STATUS", 0.5f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 5.0f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("GPS2_RAW", 1.0f); configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("GPS_STATUS", 1.0f); @@ -1642,6 +1645,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 5.0f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("GLOBAL_POSITION_INT", 50.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_RAW_INT", unlimited_rate); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 170b83868f..82eba94c69 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -69,6 +69,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -1867,6 +1871,295 @@ protected: } }; +class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalDeviceAttitudeStatus::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_DEVICE_ATTITUDE_STATUS"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalDeviceAttitudeStatus(mavlink); + } + + unsigned get_size() override + { + return _gimbal_device_attitude_status_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalDeviceAttitudeStatus(MavlinkStreamGimbalDeviceAttitudeStatus &) = delete; + MavlinkStreamGimbalDeviceAttitudeStatus &operator = (const MavlinkStreamGimbalDeviceAttitudeStatus &) = delete; + +protected: + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.update(&gimbal_device_attitude_status)) { + mavlink_gimbal_device_attitude_status_t msg{}; + + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.flags = gimbal_device_attitude_status.device_flags; + + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +class MavlinkStreamGimbalManagerInformation : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalManagerInformation::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_MANAGER_INFORMATION"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalManagerInformation(mavlink); + } + + unsigned get_size() override + { + return _gimbal_device_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete; + MavlinkStreamGimbalManagerInformation &operator = (const MavlinkStreamGimbalManagerInformation &) = delete; + +protected: + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + gimbal_device_information_s gimbal_device_information; + + if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) { + // send out gimbal_manager_info with info from gimbal_device_information + mavlink_gimbal_manager_information_t msg{}; + msg.time_boot_ms = gimbal_device_information.timestamp / 1000; + msg.gimbal_device_id = 0; + msg.cap_flags = gimbal_device_information.capability_flags; + + msg.tilt_max = gimbal_device_information.tilt_max; + msg.tilt_min = gimbal_device_information.tilt_min; + msg.tilt_rate_max = gimbal_device_information.tilt_rate_max; + + msg.pan_max = gimbal_device_information.pan_max; + msg.pan_min = gimbal_device_information.pan_min; + msg.pan_rate_max = gimbal_device_information.pan_rate_max; + + mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + +class MavlinkStreamGimbalManagerStatus : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalManagerStatus::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_MANAGER_STATUS"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalManagerStatus(mavlink); + } + + unsigned get_size() override + { + return _gimbal_manager_status_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalManagerStatus(MavlinkStreamGimbalManagerStatus &) = delete; + MavlinkStreamGimbalManagerStatus &operator = (const MavlinkStreamGimbalManagerStatus &) = delete; + +protected: + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + gimbal_manager_status_s gimbal_manager_status; + + if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) { + // send out gimbal_manager_info with info from gimbal_device_information + 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.flags = gimbal_manager_status.flags; + + mavlink_msg_gimbal_manager_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + + + +class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalDeviceSetAttitude::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_DEVICE_SET_ATTITUDE"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalDeviceSetAttitude(mavlink); + } + + unsigned get_size() override + { + return _gimbal_device_set_attitude_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalDeviceSetAttitude(MavlinkStreamGimbalDeviceSetAttitude &) = delete; + MavlinkStreamGimbalDeviceSetAttitude &operator = (const MavlinkStreamGimbalDeviceSetAttitude &) = delete; + +protected: + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + gimbal_device_set_attitude_s gimbal_device_set_attitude; + + if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) { + // send out gimbal_manager_info with info from gimbal_device_information + mavlink_gimbal_device_set_attitude_t msg{}; + msg.flags = gimbal_device_set_attitude.flags; + msg.target_system = gimbal_device_set_attitude.target_system; + msg.target_component = gimbal_device_set_attitude.target_component; + + msg.q[0] = gimbal_device_set_attitude.q[0]; + msg.q[1] = gimbal_device_set_attitude.q[1]; + msg.q[2] = gimbal_device_set_attitude.q[2]; + msg.q[3] = gimbal_device_set_attitude.q[3]; + + msg.angular_velocity_x = gimbal_device_set_attitude.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_set_attitude.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_set_attitude.angular_velocity_z; + + mavlink_msg_gimbal_device_set_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + class MavlinkStreamCameraTrigger : public MavlinkStream { public: @@ -3046,6 +3339,10 @@ 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(), create_stream_list_item(), create_stream_list_item >(), create_stream_list_item >(), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56a63bac3b..8f252a3cf4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -279,6 +279,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; #endif // !CONSTRAINED_FLASH + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(msg); + break; + default: break; } @@ -2938,6 +2946,66 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) } } + +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.target_system = set_attitude_msg.target_system; + gimbal_attitude.target_component = set_attitude_msg.target_component; + gimbal_attitude.flags = set_attitude_msg.flags; + gimbal_attitude.gimbal_device_id = set_attitude_msg.gimbal_device_id; + + matrix::Quatf q(set_attitude_msg.q); + q.copyTo(gimbal_attitude.q); + + gimbal_attitude.angular_velocity_x = set_attitude_msg.angular_velocity_x; + gimbal_attitude.angular_velocity_y = set_attitude_msg.angular_velocity_y; + gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z; + + _gimbal_manager_set_attitude_pub.publish(gimbal_attitude); + +} + +void +MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg) +{ + + mavlink_gimbal_device_information_t gimbal_device_info_msg; + mavlink_msg_gimbal_device_information_decode(msg, &gimbal_device_info_msg); + + gimbal_device_information_s gimbal_information{}; + gimbal_information.timestamp = hrt_absolute_time(); + + static_assert(sizeof(gimbal_information.vendor_name) == sizeof(gimbal_device_info_msg.vendor_name), + "vendor_name length doesn't match"); + static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name), + "model_name length doesn't match"); + memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name)); + memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name)); + gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0'; + gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0'; + + 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.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_device_information_pub.publish(gimbal_information); + +} + /** * Receive data from UART/UDP */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1c8171054c..f252707e6b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -67,6 +67,8 @@ #include #include #include +#include +#include #include #include #include @@ -201,6 +203,9 @@ 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_device_information(mavlink_message_t *msg); #if !defined(CONSTRAINED_FLASH) void handle_message_debug(mavlink_message_t *msg); @@ -353,6 +358,8 @@ private: uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; 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 _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 570dd2d086..710bbab523 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,7 @@ #include "input_mavlink.h" #include +#include #include #include #include @@ -49,6 +50,9 @@ #include #include #include +#include + +using matrix::wrap_pi; namespace vmount { @@ -137,11 +141,6 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ } _cur_roi_mode = vehicle_roi.mode; - - //set all other control data fields to defaults - for (int i = 0; i < 3; ++i) { - _control_data.stabilize_axis[i] = false; - } } // check whether the position setpoint got updated @@ -175,8 +174,7 @@ void InputMavlinkROI::print_status() } -InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize) - : _stabilize {stabilize, stabilize, stabilize} +InputMavlinkCmdMount::InputMavlinkCmdMount() { param_t handle = param_find("MAV_SYS_ID"); @@ -259,10 +257,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con continue; } - for (int i = 0; i < 3; ++i) { - _control_data.stabilize_axis[i] = _stabilize[i]; - } - _control_data.gimbal_shutter_retract = false; if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { @@ -312,9 +306,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _ack_vehicle_command(&vehicle_command); } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - _stabilize[0] = (int)(vehicle_command.param2 + 0.5f) == 1; - _stabilize[1] = (int)(vehicle_command.param3 + 0.5f) == 1; - _stabilize[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + _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), @@ -378,5 +374,430 @@ void InputMavlinkCmdMount::print_status() PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } +InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device) +{ + 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 { + /* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */ + _stream_gimbal_manager_information(); + } +} + +InputMavlinkGimbalV2::~InputMavlinkGimbalV2() +{ + if (_vehicle_roi_sub >= 0) { + orb_unsubscribe(_vehicle_roi_sub); + } + + if (_position_setpoint_triplet_sub >= 0) { + orb_unsubscribe(_position_setpoint_triplet_sub); + } + + if (_gimbal_manager_set_attitude_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_attitude_sub); + } + + if (_vehicle_command_sub >= 0) { + orb_unsubscribe(_vehicle_command_sub); + } +} + + +void InputMavlinkGimbalV2::print_status() +{ + PX4_INFO("Input: Mavlink (Gimbal V2)"); +} + +int InputMavlinkGimbalV2::initialize() +{ + _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); + + if (_vehicle_roi_sub < 0) { + return -errno; + } + + _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + if (_position_setpoint_triplet_sub < 0) { + return -errno; + } + + _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); + + if (_gimbal_manager_set_attitude_sub < 0) { + return -errno; + } + + if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 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. + orb_set_interval(_vehicle_command_sub, 10); + + return 0; +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_status() +{ + + if (_gimbal_device_attitude_status_sub.updated()) { + _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.gimbal_device_id = 0; + _gimbal_manager_status_pub.publish(gimbal_manager_status); + +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_information() +{ + gimbal_device_information_s gimbal_device_info; + gimbal_device_info.timestamp = hrt_absolute_time(); + const char vendor_name[] = "PX4"; + const char model_name[] = "AUX gimbal"; + + strncpy((char *)gimbal_device_info.vendor_name, vendor_name, sizeof(gimbal_device_info.vendor_name)); + strncpy((char *)gimbal_device_info.model_name, model_name, sizeof(gimbal_device_info.model_name)); + + gimbal_device_info.firmware_version = 0; + gimbal_device_info.capability_flags = 0; + gimbal_device_info.capability_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK | + 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_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(); + + // Default to no change, set if we receive anything. + *control_data = nullptr; + + const int num_poll = 4; + px4_pollfd_struct_t polls[num_poll]; + polls[0].fd = _gimbal_manager_set_attitude_sub; + polls[0].events = POLLIN; + polls[1].fd = _vehicle_roi_sub; + polls[1].events = POLLIN; + polls[2].fd = _position_setpoint_triplet_sub; + polls[2].events = POLLIN; + polls[3].fd = _vehicle_command_sub; + polls[3].events = POLLIN; + + int poll_timeout = (int)timeout_ms; + + bool exit_loop = false; + + while (!exit_loop && poll_timeout >= 0) { + hrt_abstime poll_start = hrt_absolute_time(); + + int ret = px4_poll(polls, num_poll, poll_timeout); + + if (ret < 0) { + return -errno; + } + + poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + + // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout + exit_loop = true; + + if (ret == 0) { + // Timeout control_data already null. + + } else { + if (polls[0].revents & POLLIN) { + 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(); + + _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; + } + + if (polls[1].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + + _control_data.gimbal_shutter_retract = false; + + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + + _control_data.type = ControlData::Type::Neutral; + *control_data = &_control_data; + _is_roi_set = false; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + double lat, lon, alt = 0.; + _read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt); + _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + + _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; + _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; + _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + + _transform_lon_lat_to_angle(lon, lat, alt); + + *control_data = &_control_data; + _is_roi_set = true; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + + _transform_lon_lat_to_angle(vehicle_roi.lon, vehicle_roi.lat, (double)vehicle_roi.alt); + *control_data = &_control_data; + _is_roi_set = true; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + //TODO is this even suported? + exit_loop = false; + + } else { + exit_loop = false; + } + } + + // check whether the position setpoint got updated + if (polls[2].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + double lat, lon, alt = 0.; + _read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt); + _transform_lon_lat_to_angle(lon, lat, alt); + *control_data = &_control_data; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + exit_loop = false; + } + } + + if (polls[3].revents & POLLIN) { + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id) || (vehicle_command.target_system == 0); + const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + exit_loop = false; + 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); + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command); + + } else { + exit_loop = false; + } + } + } + } + + return 0; +} + +void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, + const double roi_alt) +{ + vehicle_global_position_s vehicle_global_position; + _vehicle_global_position_sub.copy(&vehicle_global_position); + const double &vlat = vehicle_global_position.lat; + const double &vlon = vehicle_global_position.lon; + + _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; + + _control_data.type_data.angle.angles[0] = 0.f; + + // 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; + + } else { + _control_data.type_data.angle.angles[1] = _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; + + // 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; + + // 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]); +} + +float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude, + const vehicle_global_position_s &global_position) +{ + if (!map_projection_initialized(&_projection_reference)) { + map_projection_init(&_projection_reference, global_position.lat, global_position.lon); + } + + float x1, y1, x2, y2; + map_projection_project(&_projection_reference, lat, lon, &x1, &y1); + map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2); + float dx = x1 - x2, dy = y1 - y2; + float target_distance = sqrtf(dx * dx + dy * dy); + float z = altitude - global_position.alt; + + return atan2f(z, target_distance); +} + +void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp) +{ + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + lon_sp = position_setpoint_triplet.current.lon; + lat_sp = position_setpoint_triplet.current.lat; + 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) +{ + + 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; + + } 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; + } + + 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; + } + + 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 + } + + 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; + } + } +} + +//TODO move this one to input.cpp such that it can be shared across functions +void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd) +{ + 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.target_system = cmd->source_system; + vehicle_command_ack.target_component = cmd->source_component; + + uORB::PublicationQueued cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); +} } /* namespace vmount */ diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index b3661466ec..2a70284294 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,8 +43,17 @@ #include "input_rc.h" #include +#include +#include +#include +#include +#include +#include #include #include +#include +#include + namespace vmount { @@ -69,7 +78,7 @@ private: int _vehicle_roi_sub = -1; int _position_setpoint_triplet_sub = -1; - uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; + uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE}; }; @@ -80,7 +89,7 @@ private: class InputMavlinkCmdMount : public InputBase { public: - InputMavlinkCmdMount(bool stabilize); + InputMavlinkCmdMount(); virtual ~InputMavlinkCmdMount(); virtual void print_status(); @@ -93,11 +102,55 @@ private: void _ack_vehicle_command(vehicle_command_s *cmd); int _vehicle_command_sub = -1; - bool _stabilize[3] = { false, false, false }; int32_t _mav_sys_id{1}; ///< our mavlink system id int32_t _mav_comp_id{1}; ///< our mavlink component id }; +class InputMavlinkGimbalV2 : public InputBase +{ +public: + InputMavlinkGimbalV2(bool has_v2_gimbal_device); + virtual ~InputMavlinkGimbalV2(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + +private: + + void _transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, const double roi_alt); + 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 _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 _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 + + bool _is_roi_set{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Publication _gimbal_device_info_pub{ORB_ID(gimbal_device_information)}; + 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 b917914807..df1311270b 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,8 +49,7 @@ namespace vmount { -InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) - : _do_stabilization(do_stabilization) +InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) { _aux_channels[0] = aux_channel_roll; _aux_channels[1] = aux_channel_pitch; @@ -134,7 +133,6 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo 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; - control_data.stabilize_axis[i] = _do_stabilization; _last_set_aux_values[i] = new_aux_values[i]; } diff --git a/src/modules/vmount/input_rc.h b/src/modules/vmount/input_rc.h index 14c39c4053..89cf3a6034 100644 --- a/src/modules/vmount/input_rc.h +++ b/src/modules/vmount/input_rc.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,12 +57,11 @@ class InputRC : public InputBase public: /** - * @param do_stabilization * @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0) * @param aux_channel_pitch * @param aux_channel_yaw */ - InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); + InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); virtual ~InputRC(); virtual void print_status(); @@ -81,7 +80,6 @@ protected: float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); private: - const bool _do_stabilization; int _aux_channels[3]; int _manual_control_setpoint_sub = -1; diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index 2654b8500e..ef8cc947bc 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,8 +71,6 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool for (int i = 0; i < 3; ++i) { _control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F; - - _control_data.stabilize_axis[i] = true; } _control_data.gimbal_shutter_retract = false; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 706190eab9..82c2cdc929 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -74,6 +74,13 @@ void OutputBase::publish() _mount_orientation_pub.publish(mount_orientation); } +void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize) +{ + _stabilize[0] = roll_stabilize; + _stabilize[1] = pitch_stabilize; + _stabilize[2] = yaw_stabilize; +} + float OutputBase::_calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position) { @@ -96,18 +103,26 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) _cur_control_data = control_data; for (int i = 0; i < 3; ++i) { - _stabilize[i] = control_data->stabilize_axis[i]; _angle_speeds[i] = 0.f; } switch (control_data->type) { case ControlData::Type::Angle: for (int i = 0; i < 3; ++i) { - if (control_data->type_data.angle.frames[i] == ControlData::TypeData::TypeAngle::Frame::AngularRate) { + 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; - } else { + case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame: + _absolute_angle[i] = true; _angle_setpoints[i] = control_data->type_data.angle.angles[i]; + break; } } @@ -188,13 +203,23 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t) vehicle_attitude_s vehicle_attitude{}; matrix::Eulerf euler; - if (_stabilize[0] || _stabilize[1] || _stabilize[2]) { + // We only need to apply additional compensation if the required angle is + // absolute (world frame) as well as the gimbal is not capable of doing that + // calculation. (Most gimbals stabilize at least roll and pitch + // and only need compensation for yaw, if at all.) + bool compensate[3]; + + for (int i = 0; i < 3; ++i) { + compensate[i] = _stabilize[i] && _absolute_angle[i]; + } + + if (compensate[0] || compensate[1] || compensate[2]) { _vehicle_attitude_sub.copy(&vehicle_attitude); euler = matrix::Quatf(vehicle_attitude.q); } for (int i = 0; i < 3; ++i) { - if (_stabilize[i]) { + if (compensate[i]) { _angle_outputs[i] = _angle_setpoints[i] - euler(i); } else { diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index 5b34a6c4d5..7356ec9edc 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -97,6 +97,8 @@ public: /** Publish _angle_outputs as a mount_orientation message. */ void publish(); + void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); + protected: float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position); @@ -116,6 +118,10 @@ protected: float _angle_speeds[3] = { 0.f, 0.f, 0.f }; bool _stabilize[3] = { false, false, false }; + // Pitch and role are by default aligned with the horizon. + // Yaw follows the vehicle (not lock/absolute mode). + bool _absolute_angle[3] = {true, true, false }; + /** calculate the _angle_outputs (with speed) and stabilize if needed */ void _calculate_output_angles(const hrt_abstime &t); diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 9babd00497..2ed277ff4b 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,7 @@ #include "output_mavlink.h" #include +#include #include #include @@ -49,12 +50,12 @@ namespace vmount { -OutputMavlink::OutputMavlink(const OutputConfig &output_config) +OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config) : OutputBase(output_config) { } -int OutputMavlink::update(const ControlData *control_data) +int OutputMavlinkV1::update(const ControlData *control_data) { vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); @@ -83,9 +84,9 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); } - vehicle_command.param2 = control_data->stabilize_axis[0]; - vehicle_command.param3 = control_data->stabilize_axis[1]; - vehicle_command.param4 = control_data->stabilize_axis[2]; + vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f; + vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f; + vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f; _vehicle_command_pub.publish(vehicle_command); } @@ -112,10 +113,87 @@ int OutputMavlink::update(const ControlData *control_data) return 0; } -void OutputMavlink::print_status() +void OutputMavlinkV1::print_status() { - PX4_INFO("Output: Mavlink"); + PX4_INFO("Output: MAVLink gimbal protocol v1"); } -} /* namespace vmount */ +OutputMavlinkV2::OutputMavlinkV2(const OutputConfig &output_config) + : OutputBase(output_config) +{ +} + +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(); + hrt_abstime t = hrt_absolute_time(); + _calculate_output_angles(t); + + _last_update = t; + + return 0; +} + +void OutputMavlinkV2::print_status() +{ + PX4_INFO("Output: MAVLink gimbal protocol v2"); +} + +void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *control_data) +{ + 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; + + 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) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL; + } + + 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[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; + } + + _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 5ace7f6582..8c0a4808a0 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,8 @@ #include #include +#include + namespace vmount { @@ -50,20 +52,33 @@ namespace vmount ** class OutputMavlink * Output via vehicle_command topic */ -class OutputMavlink : public OutputBase +class OutputMavlinkV1 : public OutputBase { public: - OutputMavlink(const OutputConfig &output_config); - virtual ~OutputMavlink() = default; + OutputMavlinkV1(const OutputConfig &output_config); + virtual ~OutputMavlinkV1() = default; virtual int update(const ControlData *control_data); virtual void print_status(); private: - - uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; }; +class OutputMavlinkV2 : public OutputBase +{ +public: + OutputMavlinkV2(const OutputConfig &output_config); + virtual ~OutputMavlinkV2() = default; + + virtual int update(const ControlData *control_data); + + virtual void print_status(); + +private: + void _publish_gimbal_device_set_attitude(const ControlData *control_data); + uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; +}; } /* namespace vmount */ diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index 196772a6fc..a829d4cb3b 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/src/modules/vmount/output_rc.cpp @@ -42,7 +42,7 @@ #include #include - +#include namespace vmount { @@ -73,6 +73,8 @@ int OutputRC::update(const ControlData *control_data) actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale; actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value; + _stream_device_attitude_status(); + _actuator_controls_pub.publish(actuator_controls); _last_update = t; @@ -85,5 +87,26 @@ void OutputRC::print_status() PX4_INFO("Output: AUX"); } -} /* namespace vmount */ +void OutputRC::_stream_device_attitude_status() +{ + gimbal_device_attitude_status_s attitude_status{}; + attitude_status.timestamp = hrt_absolute_time(); + attitude_status.target_system = 0; + attitude_status.target_component = 0; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | + gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + + 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); + + attitude_status.failure_flags = 0; + _attitude_status_pub.publish(attitude_status); +} +} /* namespace vmount */ diff --git a/src/modules/vmount/output_rc.h b/src/modules/vmount/output_rc.h index 340e913cc7..52839cac69 100644 --- a/src/modules/vmount/output_rc.h +++ b/src/modules/vmount/output_rc.h @@ -43,6 +43,7 @@ #include #include +#include namespace vmount { @@ -63,7 +64,10 @@ public: virtual void print_status(); private: + void _stream_device_attitude_status(); + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_2)}; + uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; bool _retract_gimbal = true; }; diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index d853d3bd0c..406cf74aff 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -243,20 +243,22 @@ static int vmount_thread_main(int argc, char *argv[]) case 0: // Automatic - thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab); + thread_data.input_objs[0] = new InputMavlinkCmdMount(); thread_data.input_objs[1] = new InputMavlinkROI(); // RC is on purpose last here so that if there are any mavlink // messages, they will take precedence over RC. // This logic is done further below while update() is called. - thread_data.input_objs[2] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, + thread_data.input_objs[2] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, params.mnt_man_yaw); thread_data.input_objs_len = 3; break; case 1: //RC - thread_data.input_objs[0] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, + thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, params.mnt_man_yaw); break; @@ -265,7 +267,11 @@ static int vmount_thread_main(int argc, char *argv[]) break; case 3: //MAVLINK_DO_MOUNT - thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab); + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + break; + + case 4: //MAVLINK_V2 + thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2); break; default: @@ -288,8 +294,15 @@ static int vmount_thread_main(int argc, char *argv[]) break; - case 1: //MAVLINK - thread_data.output_obj = new OutputMavlink(output_config); + case 1: //MAVLink v1 gimbal protocol + thread_data.output_obj = new OutputMavlinkV1(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + + case 2: //MAVLink v2 gimbal protocol + thread_data.output_obj = new OutputMavlinkV2(output_config); if (!thread_data.output_obj) { alloc_failed = true; } @@ -318,6 +331,16 @@ static int vmount_thread_main(int argc, char *argv[]) thread_should_exit = true; break; } + + if (params.mnt_do_stab == 1) { + thread_data.output_obj->set_stabilize(true, true, true); + + } else if (params.mnt_do_stab == 2) { + thread_data.output_obj->set_stabilize(false, false, true); + + } else { + thread_data.output_obj->set_stabilize(false, false, false); + } } if (thread_data.input_objs_len > 0) { diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c index 556987360e..6d067da61b 100644 --- a/src/modules/vmount/vmount_params.c +++ b/src/modules/vmount/vmount_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,8 +48,9 @@ * @value -1 DISABLED * @value 0 AUTO * @value 1 RC -* @value 2 MAVLINK_ROI -* @value 3 MAVLINK_DO_MOUNT +* @value 2 MAVLINK_ROI (protocol v1) +* @value 3 MAVLINK_DO_MOUNT (protocol v1) +* @value 4 MAVlink gimbal protocol v2 * @min -1 * @max 3 * @group Mount @@ -65,9 +66,10 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1); * to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID) * * @value 0 AUX -* @value 1 MAVLINK +* @value 1 MAVLink gimbal protocol v1 +* @value 2 MAVLink gimbal protocol v2 * @min 0 -* @max 1 +* @max 2 * @group Mount */ PARAM_DEFINE_INT32(MNT_MODE_OUT, 0); @@ -75,7 +77,7 @@ PARAM_DEFINE_INT32(MNT_MODE_OUT, 0); /** * Mavlink System ID of the mount * -* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this target ID. +* If MNT_MODE_OUT is MAVLink gimbal protocol v1, mount configure/control commands will be sent with this target ID. * * @group Mount */ @@ -84,7 +86,7 @@ PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1); /** * Mavlink Component ID of the mount * -* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this component ID. +* If MNT_MODE_OUT is MAVLink protocol v2, mount configure/control commands will be sent with this component ID. * * @group Mount */ @@ -162,9 +164,14 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); /** * Stabilize the mount (set to true for servo gimbal, false for passthrough). -* Does not affect MAVLINK_ROI input. +* (This is required for a gimbal which is not capable of stabilizing itself +* and relies on the IMU's attitude estimation.) * -* @boolean +* @value 0 Disable +* @value 1 Stabilize all axis +* @value 2 Stabilize yaw for absolute/lock mode. +* @min 0 +* @max 2 * @group Mount */ PARAM_DEFINE_INT32(MNT_DO_STAB, 0);