From 6309aa612be30e4b98dbec7d4dde5068bce17839 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:06:48 +0200 Subject: [PATCH] MAVLink app: Introduce OSD mode --- src/modules/mavlink/mavlink_main.cpp | 13 ++++++++++++- src/modules/mavlink/mavlink_main.h | 3 ++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 348185e693..5cba6c644e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1295,6 +1295,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; } break; @@ -1451,7 +1453,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); @@ -1468,6 +1469,16 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; + case MAVLINK_MODE_OSD: + configure_stream("SYS_STATUS", 5.0f); + configure_stream("ATTITUDE", 30.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("BATTERY_STATUS", 1.0f); + configure_stream("SYSTEM_TIME", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 71749e2923..cf95c4d40c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -128,7 +128,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD }; void set_mode(enum MAVLINK_MODE);