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: go to ALTCTL if GPS is lost in POSCTL

Browse Source 
Previously, we renained in POSCTL and would drift away if GPS was lost.
This commit is contained in:
Julian Oes
2016-06-06 13:56:37 +02:00
committed by Lorenz Meier
parent 67a4a57491
commit ef04085ac5
1 changed files with 39 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -603,7 +603,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
case commander_state_s::MAIN_STATE_RATTITUDE:
case commander_state_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_ALTCTL:
case commander_state_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
status->failsafe = true;
@@ -640,10 +639,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case commander_state_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
@@ -651,6 +646,45 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
}
break;
case commander_state_s::MAIN_STATE_POSCTL:
{
const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
PX4_INFO("lost RC: %s", rc_lost ? "yes" : "no");
if (rc_lost && armed && !landed) {
status->failsafe = true;
if (status_flags->condition_global_position_valid &&
status_flags->condition_home_position_valid &&
!status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status_flags->condition_local_position_valid &&
!status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
} else if (status_flags->gps_failure && armed && !landed) {
status->failsafe = true;
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
}
break;
case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe