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

added auto takeoff support, updated configuration for solo and generalized landing mission items

Browse Source
This commit is contained in:
Andreas Antener
2015-12-28 15:21:50 +01:00
committed by Lorenz Meier
parent 3847c826ec
commit fbf42c8949
13 changed files with 287 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+20
View File
@@ -1083,6 +1083,7 @@ int commander_thread_main(int argc, char *argv[])
status.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
@@ -2348,6 +2349,25 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* handle main state after takeoff and land */
if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) {
// transition back to state we had before takeoff
if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) {
main_state_transition(&status, status.main_state_prev);
}
} else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.condition_landed) {
// transition back to state we had before takeoff
if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) {
main_state_transition(&status, status.main_state_prev);
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
src/modules/commander/state_machine_helper.cpp
+21
View File
@@ -352,6 +352,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
case vehicle_status_s::MAIN_STATE_AUTO_LAND:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
@@ -373,6 +374,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state_prev = status->main_state;
status->main_state = new_main_state;
} else {
ret = TRANSITION_NOT_CHANGED;
@@ -766,6 +768,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LAND:
/* require global position and home */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {