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 state machine fix preflight and prearm error

Browse Source 
-  fixes #9155
This commit is contained in:
Daniel Agar
2018-03-29 23:14:34 -04:00
parent 35bf37d617
commit e5d9c826d8
3 changed files with 13 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -85,7 +85,6 @@ const char * const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
};
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
static int last_prearm_ret = 1; ///< initialize to fail
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
@@ -118,7 +117,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
/*
* Get sensing state if necessary
*/
int prearm_ret = OK;
bool prearm_check_ret = true;
bool checkAirspeed = false;
bool sensor_checks = !hil_enabled;
@@ -136,11 +136,11 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true,
status->is_vtol, true, true, time_since_boot);
prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, status_flags, battery,
prearm_check_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, status_flags, battery,
arm_requirements, time_since_boot);
if (!preflight_check) {
prearm_ret = false;
prearm_check_ret = false;
}
}
@@ -153,16 +153,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed,
prearm_check_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed,
(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true,
status->is_vtol, false, false, time_since_boot);
status_flags->condition_system_sensors_initialized = (prearm_ret == OK);
status_flags->condition_system_sensors_initialized = prearm_check_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
} else {
prearm_ret = last_prearm_ret;
}
}
@@ -176,7 +172,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
/* enforce lockdown in HIL */
if (hil_enabled) {
armed->lockdown = true;
prearm_ret = OK;
prearm_check_ret = true;
status_flags->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
@@ -201,7 +197,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
!hil_enabled) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
if (!prearm_check_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
@@ -1035,7 +1031,7 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c
}
}
int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
bool prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
vehicle_status_flags_s *status_flags, const battery_status_s &battery, const uint8_t arm_requirements,
const hrt_abstime &time_since_boot)
{
@@ -1075,5 +1071,5 @@ int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool fo
status_flags->condition_system_prearm_error_reported = true;
}
return !prearm_ok;
return prearm_ok;
}