From 422bac4bfde74e1a25d02601e61d17000dd5f157 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Oct 2020 15:01:49 +0200 Subject: [PATCH] mavlink: add gimbal mode to talk to gimbal device --- ROMFS/px4fmu_common/init.d-posix/rcS | 2 +- src/modules/mavlink/mavlink_main.cpp | 11 ++++++++++- src/modules/mavlink/mavlink_main.h | 5 ++++- src/modules/mavlink/module.yaml | 2 ++ 4 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index f4db5918f9..c4e8b48530 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -261,7 +261,7 @@ else mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote # Onboard link to gimbal - mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -f -m onboard + mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -f -m gimbal fi # execute autostart post script if any diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8da68e42e9..645ba92343 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1680,6 +1680,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) break; + case MAVLINK_MODE_GIMBAL: + // Note: streams requiring low latency come first + configure_stream_local("AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 20.0f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 20.0f); + break; + case MAVLINK_MODE_EXTVISION: configure_stream_local("HIGHRES_IMU", unlimited_rate); // for VIO @@ -2044,6 +2050,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "extvisionmin") == 0) { _mode = MAVLINK_MODE_EXTVISIONMIN; + } else if (strcmp(myoptarg, "gimbal") == 0) { + _mode = MAVLINK_MODE_GIMBAL; + } else { PX4_ERR("invalid mode"); err_flag = true; @@ -3074,7 +3083,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true); #endif - PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvsision", + PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 096bb6e46e..586b24565a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -194,7 +194,7 @@ public: MAVLINK_MODE_MINIMAL, MAVLINK_MODE_EXTVISION, MAVLINK_MODE_EXTVISIONMIN, - + MAVLINK_MODE_GIMBAL, MAVLINK_MODE_COUNT }; @@ -243,6 +243,9 @@ public: case MAVLINK_MODE_EXTVISIONMIN: return "ExtVisionMin"; + case MAVLINK_MODE_GIMBAL: + return "Gimbal"; + default: return "Unknown"; } diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index 7936df5a58..2924f8c21e 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -44,6 +44,8 @@ parameters: #6: Iridium # as the user does not need to configure this, hide it from the UI 7: Minimal 8: External Vision + #9: External Vision Minimal # hidden + 10: Gimbal reboot_required: true num_instances: *max_num_config_instances default: [0, 2, 0]