From d4a40f5d99339ed124dce9198372ba519c05c07b Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 6 Dec 2018 17:00:18 +0100
Subject: [PATCH] px4_custom_mode: add custom sub mode orbit

---
 src/modules/commander/px4_custom_mode.h  | 5 +++++
 src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++
 2 files changed, 13 insertions(+)

diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index e3766c9cb4..f1a7b38f72 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -66,6 +66,11 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
 };
 
+enum PX4_CUSTOM_SUB_MODE_POSCTL {
+	PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
+	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
+};
+
 union px4_custom_mode {
 	struct {
 		uint16_t reserved;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 57ddc0ba28..5169a348b3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -186,6 +186,14 @@ void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, ui
 		custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+				      | MAV_MODE_FLAG_STABILIZE_ENABLED
+				      | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+		custom_mode->sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 		*mavlink_base_mode |= auto_mode_flags;
 		custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;