fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 23:13:34 +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-04-16 20:43:33 -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
src/modules/commander/commander.cpp
+2 -6
View File
@@ -389,12 +389,8 @@ int commander_main(int argc, char *argv[])
} }
if (!strcmp(argv[1], "check")) { if (!strcmp(argv[1], "check")) {
int checkres = 0; bool checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
checkres = prearm_check(&mavlink_log_pub, false, true, &status_flags, battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED");
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");
return 0; return 0;
} }
src/modules/commander/state_machine_helper.cpp
+10 -14
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 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, 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, 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 * Get sensing state if necessary
*/ */
int prearm_ret = OK; bool prearm_check_ret = true;
bool checkAirspeed = false; bool checkAirspeed = false;
bool sensor_checks = !hil_enabled; 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->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); 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); arm_requirements, time_since_boot);
if (!preflight_check) { 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) { 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->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->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_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 */ /* enforce lockdown in HIL */
if (hil_enabled) { if (hil_enabled) {
armed->lockdown = true; armed->lockdown = true;
prearm_ret = OK; prearm_check_ret = true;
status_flags->condition_system_sensors_initialized = true; status_flags->condition_system_sensors_initialized = true;
/* recover from a prearm fail */ /* recover from a prearm fail */
@@ -201,7 +197,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
!hil_enabled) { !hil_enabled) {
// Fail transition if pre-arm check fails // Fail transition if pre-arm check fails
if (prearm_ret) { if (!prearm_check_ret) {
/* the prearm check already prints the reject reason */ /* the prearm check already prints the reject reason */
feedback_provided = true; feedback_provided = true;
valid_transition = false; 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, vehicle_status_flags_s *status_flags, const battery_status_s &battery, const uint8_t arm_requirements,
const hrt_abstime &time_since_boot) 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; status_flags->condition_system_prearm_error_reported = true;
} }
return !prearm_ok; return prearm_ok;
} }
src/modules/commander/state_machine_helper.h
+1 -1
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, 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); 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, vehicle_status_flags_s *status_flags, const battery_status_s &battery, const uint8_t arm_requirements,
const hrt_abstime &time_since_boot); const hrt_abstime &time_since_boot);