From b0d7d19babf8ff8c1582e64144aaa4edf891886d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Nov 2020 15:38:49 +0100 Subject: [PATCH] vmount/mavlink: update gimbal information message We should be using gimbal_manager_information and not gimbal_device_information. Plus, this updates the fields and flags according to the MAVLink changes. --- msg/gimbal_device_information.msg | 15 ++++++++-- msg/gimbal_manager_information.msg | 30 ++++++++++++++----- src/modules/mavlink/mavlink_messages.cpp | 27 ++++++++--------- src/modules/mavlink/mavlink_receiver.cpp | 10 +++++-- src/modules/vmount/input_mavlink.cpp | 37 +++++++++++------------- src/modules/vmount/input_mavlink.h | 4 +-- 6 files changed, 74 insertions(+), 49 deletions(-) diff --git a/msg/gimbal_device_information.msg b/msg/gimbal_device_information.msg index fb899a241c..8f7a416439 100644 --- a/msg/gimbal_device_information.msg +++ b/msg/gimbal_device_information.msg @@ -2,9 +2,12 @@ uint64 timestamp # time since system start (microseconds) uint8[32] vendor_name uint8[32] model_name +uint8[32] custom_name uint32 firmware_version +uint32 hardware_version +uint64 uid -uint16 capability_flags +uint16 cap_flags uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 @@ -17,11 +20,17 @@ 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 +uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 + +uint16 custom_cap_flags + +float32 roll_min # [rad] +float32 roll_max # [rad] -float32 pitch_max # [rad] float32 pitch_min # [rad] +float32 pitch_max # [rad] -float32 yaw_max # [rad] float32 yaw_min # [rad] +float32 yaw_max # [rad] uint8 gimbal_device_compid diff --git a/msg/gimbal_manager_information.msg b/msg/gimbal_manager_information.msg index bb8b25de1c..28db68a456 100644 --- a/msg/gimbal_manager_information.msg +++ b/msg/gimbal_manager_information.msg @@ -1,13 +1,29 @@ uint64 timestamp # time since system start (microseconds) -uint32 capability_flags +uint32 cap_flags + +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 uint8 gimbal_device_id -float32 tilt_max # [rad] -float32 tilt_min # [rad] -float32 tilt_rate_max # [rad / s] +float32 roll_min # [rad] +float32 roll_max # [rad] -float32 pan_max # [rad] -float32 pan_min # [rad] -float32 pan_rate_max # [rad / s] +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 22839ffda1..8ecbf2eecc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -70,8 +70,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -2078,12 +2078,12 @@ public: unsigned get_size() override { - return _gimbal_device_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN + + return _gimbal_manager_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)}; + uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)}; /* do not allow top copying this class */ MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete; @@ -2095,22 +2095,21 @@ protected: bool send(const hrt_abstime t) override { - gimbal_device_information_s gimbal_device_information; + gimbal_manager_information_s gimbal_manager_information; - if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) { + if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_device_information)) { // send out gimbal_manager_info with info from gimbal_device_information 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.cap_flags = gimbal_device_information.cap_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; + msg.roll_min = gimbal_device_information.roll_min; + msg.roll_max = gimbal_device_information.roll_max; + msg.pitch_min = gimbal_device_information.pitch_min; + msg.pitch_max = gimbal_device_information.pitch_max; + msg.yaw_min = gimbal_device_information.yaw_min; + msg.yaw_max = gimbal_device_information.yaw_max; mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg); @@ -2172,7 +2171,6 @@ protected: 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; @@ -2240,7 +2238,6 @@ protected: 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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6de057a277..6c8b00ed1f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3012,13 +3012,20 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg "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"); + static_assert(sizeof(gimbal_information.custom_name) == sizeof(gimbal_device_info_msg.custom_name), + "custom_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)); + memcpy(gimbal_information.custom_name, gimbal_device_info_msg.custom_name, sizeof(gimbal_information.custom_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_device_info_msg.custom_name[sizeof(gimbal_device_info_msg.custom_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.hardware_version = gimbal_device_info_msg.hardware_version; + gimbal_information.cap_flags = gimbal_device_info_msg.cap_flags; + gimbal_information.custom_cap_flags = gimbal_device_info_msg.custom_cap_flags; + gimbal_information.uid = gimbal_device_info_msg.uid; gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max; gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min; @@ -3029,7 +3036,6 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.gimbal_device_compid = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); - } /** diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index d5b40ea955..fa3414c0d7 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -40,7 +40,7 @@ #include "input_mavlink.h" #include -#include +#include #include #include #include @@ -470,29 +470,26 @@ void InputMavlinkGimbalV2::_stream_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"; + // TODO: Take gimbal_device_information into account. - 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_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); - 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_manager_info.cap_flags = + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; - 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_manager_info.pitch_max = M_PI_F / 2; + gimbal_manager_info.pitch_min = -M_PI_F / 2; + gimbal_manager_info.yaw_max = M_PI_F; + gimbal_manager_info.yaw_min = -M_PI_F; - _gimbal_device_info_pub.publish(gimbal_device_info); + _gimbal_manager_info_pub.publish(gimbal_manager_info); } int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index 0a46a8f890..a6367ba534 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -150,7 +150,7 @@ private: 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_info_pub{ORB_ID(gimbal_manager_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;