yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Adding new follow target navigation and main states. New follow target

Browse Source 
topic added. New follow fsm added to the navigator
This commit is contained in:
Jimmy Johnson
2016-02-18 06:57:01 -08:00
committed by Lorenz Meier
parent fc061ea94d
commit bbc8eaefd7
14 changed files with 266 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/commander/state_machine_helper.cpp
View File

@@ -359,6 +359,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
@@ -764,6 +765,27 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
/* 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_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else 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_AUTO_FOLLOW_TARGET;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
/* require global position and home */