fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 01:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Minor fix in "set mode" command handling.

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-25 20:17:42 +02:00
parent 7ab129ba92
commit 725bb7697c
2 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+6 -7
View File
@@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
switch (cmd->command) { switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: { case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1; uint8_t base_mode = (uint8_t) cmd->param1;
union px4_custom_mode custom_mode; uint8_t custom_main_mode = (uint8_t) cmd->param2;
custom_mode.data_float = cmd->param2;
// TODO remove debug code // TODO remove debug code
mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */ /* set arming state */
transition_result_t arming_res = TRANSITION_NOT_CHANGED; transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -357,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ /* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL); main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */ /* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT); main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */ /* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY); main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */ /* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO); main_res = main_state_transition(status, MAIN_STATE_AUTO);
} }
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg)
custom_mode.data = new_mode.custom_mode; custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode; vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.data_float; vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0; vcmd.param3 = 0;
vcmd.param4 = 0; vcmd.param4 = 0;
vcmd.param5 = 0; vcmd.param5 = 0;