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

Fix gps circuit breaker logic in state machine.

Browse Source
This commit is contained in:
James Goppert
2016-10-12 01:51:09 -04:00
committed by James Goppert
parent cf658638f4
commit 43b665ae01
3 changed files with 34 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,6 +80,12 @@ using namespace DriverFramework;
#define AVIONICS_WARN_VOLTAGE 4.9f
#endif
const char *reason_no_rc = "no rc";
const char *reason_no_gps = "no gps";
const char *reason_no_gps_cmd = "no gps cmd";
const char *reason_no_home = "no home";
const char *reason_no_datalink = "no datalink";
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -630,6 +636,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
orb_advert_t *mavlink_log_pub,
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
@@ -651,6 +658,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
/* require RC for all manual modes */
if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
@@ -700,6 +708,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
if (rc_lost && armed) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid &&
status_flags->condition_home_position_valid &&
@@ -726,6 +735,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
(!status->is_rotary_wing && !status_flags->condition_global_position_valid))
&& armed) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
@@ -758,6 +768,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps_cmd);
} else if (status_flags->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
@@ -769,6 +780,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
status->failsafe = true;
} else if (status->engine_failure) {
@@ -784,6 +796,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink);
status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
@@ -805,6 +818,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
@@ -836,11 +850,13 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
@@ -860,6 +876,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status->rc_signal_lost && rc_loss_enabled && !data_link_loss_enabled) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
@@ -898,10 +915,12 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_home);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -928,6 +947,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (!status_flags->condition_global_position_valid) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -955,6 +975,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -982,6 +1003,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -1001,6 +1023,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
/* require offboard control, otherwise stay where you are */
if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
@@ -1040,6 +1063,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc);
if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
if (offb_loss_act == 2 && status_flags->condition_global_position_valid