Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

commander arming_state_transition add HIL boolean

Browse Source
This commit is contained in:
Daniel Agar
2018-03-27 16:15:29 -04:00
parent c547866029
commit 4ccfc280c8
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+9 -7
View File
@@ -116,6 +116,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
arming_state_t current_arming_state = status->arming_state; arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false; bool feedback_provided = false;
const bool hil_enabled = (status->hil_state == vehicle_status_s::HIL_STATE_ON);
/* only check transition if the new state is actually different from the current one */ /* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) { if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED; ret = TRANSITION_NOT_CHANGED;
@@ -127,7 +129,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
*/ */
int prearm_ret = OK; int prearm_ret = OK;
bool checkAirspeed = false; bool checkAirspeed = false;
bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); bool sensor_checks = !hil_enabled;
/* Perform airspeed check only if circuit breaker is not /* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */ * engaged and it's not a rotary wing */
@@ -137,7 +139,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
/* only perform the pre-arm check if we have to */ /* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) { && !hil_enabled) {
bool preflight_check = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, bool preflight_check = Preflight::preflightCheck(mavlink_log_pub, sensor_checks,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
@@ -158,7 +160,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
if (!status_flags->condition_system_sensors_initialized if (!status_flags->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) { && !hil_enabled) {
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) {
@@ -183,7 +185,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
#endif #endif
/* enforce lockdown in HIL */ /* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { if (hil_enabled) {
armed->lockdown = true; armed->lockdown = true;
prearm_ret = OK; prearm_ret = OK;
status_flags->condition_system_sensors_initialized = true; status_flags->condition_system_sensors_initialized = true;
@@ -207,7 +209,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
// Do not perform pre-arm checks if coming from in air restore // Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON // Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) { !hil_enabled) {
// Fail transition if pre-arm check fails // Fail transition if pre-arm check fails
if (prearm_ret) { if (prearm_ret) {
@@ -260,7 +262,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
} }
// HIL can always go to standby // HIL can always go to standby
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { if (hil_enabled && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true; valid_transition = true;
} }
@@ -290,7 +292,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
// Sensors need to be initialized for STANDBY state, except for HIL // Sensors need to be initialized for STANDBY state, except for HIL
} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && } else if (!hil_enabled &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {