fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Only change flight mode immediately if GCS requested it

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-24 12:10:23 +02:00
parent 2244ff167f
commit 588133e85d
2 changed files with 15 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+11 -5
View File
@@ -675,14 +675,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// doing something sensible with the coordinates. Its designed // doing something sensible with the coordinates. Its designed
// to not require navigator and command to receive / process // to not require navigator and command to receive / process
// the data at the exact same time. // the data at the exact same time.
transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if ((main_ret != TRANSITION_DENIED)) { // Check if a mode switch had been requested
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; if ((((uint8_t)cmd->param1) & 1) > 0) {
transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if ((main_ret != TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
}
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
} }
} }
break; break;
src/modules/navigator/navigator_main.cpp
+4 -4
View File
@@ -419,25 +419,25 @@ Navigator::task_main()
rep->previous.alt = get_global_position()->alt; rep->previous.alt = get_global_position()->alt;
// Go on and check which changes had been requested // Go on and check which changes had been requested
if (isfinite(cmd.param4)) { if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4; rep->current.yaw = cmd.param4;
} else { } else {
rep->current.yaw = NAN; rep->current.yaw = NAN;
} }
if (isfinite(cmd.param5)) { if (PX4_ISFINITE(cmd.param5)) {
rep->current.lat = cmd.param5 / (double)1e7; rep->current.lat = cmd.param5 / (double)1e7;
} else { } else {
rep->current.lat = get_global_position()->lat; rep->current.lat = get_global_position()->lat;
} }
if (isfinite(cmd.param6)) { if (PX4_ISFINITE(cmd.param6)) {
rep->current.lon = cmd.param6 / (double)1e7; rep->current.lon = cmd.param6 / (double)1e7;
} else { } else {
rep->current.lon = get_global_position()->lon; rep->current.lon = get_global_position()->lon;
} }
if (isfinite(cmd.param7)) { if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7; rep->current.alt = cmd.param7;
} else { } else {
rep->current.alt = get_global_position()->alt; rep->current.alt = get_global_position()->alt;