From 1b1617b5a0c760119b1cff28db5eceb39626167d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 7 Apr 2018 01:48:04 +0300 Subject: [PATCH] Add new mode AUTO_PRECLAND --- src/modules/commander/commander.cpp | 3 +++ src/modules/commander/px4_custom_mode.h | 3 ++- src/modules/mavlink/mavlink_messages.cpp | 6 ++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4a3d724d46..6ac6ea76d9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -729,6 +729,9 @@ Commander::handle_command(vehicle_status_s *status_local, case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags, &internal_state); + break; default: main_ret = TRANSITION_DENIED; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 931d0d6049..e3766c9cb4 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -62,7 +62,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, - PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET + PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, + PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND }; union px4_custom_mode { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index aab08562ce..d278cc283e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -207,6 +207,12 @@ void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, ui custom_mode->sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */