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

commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-16 17:34:21 +02:00
parent 91f0b9eee4
commit e0ed0625f8
9 changed files with 122 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
}
}
if (ret = TRANSITION_CHANGED) {
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
// XXX also set lockdown here
@@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
return ret;
}
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
void set_nav_state(struct vehicle_status_s *status)
{
switch (status->failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_MANUAL:
status->set_nav_state = NAVIGATION_STATE_MANUAL;
break;
status->failsafe = false;
case MAIN_STATE_ALTCTL:
status->set_nav_state = NAVIGATION_STATE_ALTCTL;
break;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost) {
status->failsafe = true;
case MAIN_STATE_POSCTL:
status->set_nav_state = NAVIGATION_STATE_POSCTL;
break;
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_AUTO_MISSION:
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_AUTO_LOITER:
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_AUTO_RTL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
break;
case MAIN_STATE_ACRO:
status->set_nav_state = NAVIGATION_STATE_ACRO;
break;
default:
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case FAILSAFE_STATE_RC_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
case MAIN_STATE_AUTO_MISSION:
/* require data link and global position */
if (status->data_link_lost || !status->condition_global_position_valid) {
status->failsafe = true;
} else {
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case FAILSAFE_STATE_DL_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
case MAIN_STATE_AUTO_LOITER:
/* require data link and local position */
if (status->data_link_lost || !status->condition_local_position_valid) {
status->failsafe = true;
} else {
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case FAILSAFE_STATE_LAND:
status->set_nav_state = NAVIGATION_STATE_LAND;
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
status->failsafe = true;
case FAILSAFE_STATE_TERMINATION:
status->set_nav_state = NAVIGATION_STATE_TERMINATION;
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
}
break;
default:
break;
}
if (status->failsafe) {
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
}
}