fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/commander/commander.cpp
View File

@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
union px4_custom_mode custom_mode;
custom_mode.data_float = cmd->param2;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
// 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 */
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) {
/* use autopilot-specific mode */
if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* 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 */
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 */
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 */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg)
custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.data_float;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;