fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 12:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3307 from UAVenture/sub_mode_support

Browse Source 
Sub mode support
This commit is contained in:
Lorenz Meier
2015-12-02 14:26:41 +01:00
parent 25dbce84d7 756282b195
commit fa49d27d09
2 changed files with 26 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+25 -1
View File
@@ -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,30 @@ 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);
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;
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;
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) {
/* ACRO */
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -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;