From 4e4f780ecc7f873d7398e6b6ea0e6fc4dbde3e05 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Wed, 25 Nov 2015 16:22:23 -0500 Subject: [PATCH 1/3] Add support for switching to auto modes via SET_MODE --- src/modules/commander/commander.cpp | 18 +++++++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc4f2feec7..fd1826a0c0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -559,6 +559,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd->param1; uint8_t custom_main_mode = (uint8_t)cmd->param2; + uint8_t custom_sub_mode = (uint8_t)cmd->param3; transition_result_t arming_ret = TRANSITION_NOT_CHANGED; @@ -596,7 +597,22 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + switch(custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); + break; + + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(mavlink_fd, "Unsupported auto command"); + break; + } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ac659608a..508a1d8c95 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -539,7 +539,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; + vcmd.param3 = custom_mode.sub_mode; vcmd.param4 = 0; vcmd.param5 = 0; vcmd.param6 = 0; From df6fe7f99372b1a9194d811daca385d88452b838 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 29 Nov 2015 19:26:45 +0100 Subject: [PATCH 2/3] transition to auto mission if custom mode param is not set --- src/modules/commander/commander.cpp | 33 +++++++++++++++++------------ 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd1826a0c0..62366f2981 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -597,21 +597,26 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - switch(custom_sub_mode) { - case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); - break; - case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); - break; - case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); - break; + if (custom_sub_mode > 0) { + switch(custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); + break; - default: - main_ret = TRANSITION_DENIED; - mavlink_log_critical(mavlink_fd, "Unsupported auto command"); - break; + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(mavlink_fd, "Unsupported auto mode"); + break; + } + + } else { + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { From 756282b195473844b08b632e5be9665f9807f862 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 2 Dec 2015 12:04:34 +0100 Subject: [PATCH 3/3] added auto takeoff sub mode --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 62366f2981..14aa97d375 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -608,6 +608,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_RTL: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); break; + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); + break; default: main_ret = TRANSITION_DENIED;