fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08: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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/commander/commander.cpp
View File

@ -389,12 +389,8 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "check")) {
int checkres = 0;
checkres = prearm_check(&mavlink_log_pub, false, true, &status_flags, battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
bool checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED");
return 0;
}

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;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -106,7 +106,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
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);