From e7a65e7fd98d59de8202afc5bb9742c8bfb76cfe 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 adf1e998a7eb..1a41205d46a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -724,6 +724,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 c65432f5e9e3..03f865f6cddb 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 37522edae1f4..93db523034fc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -204,6 +204,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st 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 */