Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

commander: add orbit state handling

Browse Source
This commit is contained in:
Matthias Grob
2018-11-25 11:11:51 +00:00
parent 77d37c5b7f
commit 34fb52d8bd
2 changed files with 34 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+19 -1
View File
@@ -1029,6 +1029,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
// Switch to orbit state and let the orbit task handle the command further
main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags, &internal_state);
break;
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
@@ -1060,7 +1065,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
/* ignore commands that are handled by other parts of the system */
break;
@@ -3400,6 +3404,20 @@ set_control_mode()
break;
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
default:
break;
}
src/modules/commander/state_machine_helper.cpp
+15 -1
View File
@@ -294,8 +294,9 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
break;
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case commander_state_s::MAIN_STATE_ORBIT:
/* FOLLOW only implemented in MC */
/* Follow and orbit only implemented for multicopter */
if (status.is_rotary_wing) {
ret = TRANSITION_CHANGED;
}
@@ -586,6 +587,19 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
break;
case commander_state_s::MAIN_STATE_ORBIT:
/* require local position */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
}
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
/* require local position */