yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander arming state transition consolidate HIL special checks and remove atomic

Browse Source
This commit is contained in:
Daniel Agar
2018-03-29 23:14:34 -04:00
parent 54ea78898c
commit a4703c6be4
1 changed files with 15 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -167,26 +167,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
}
}
/*
* Perform an atomic state update
*/
#ifdef __PX4_NUTTX
irqstate_t flags = px4_enter_critical_section();
#endif
/* enforce lockdown in HIL */
if (hil_enabled) {
armed->lockdown = true;
status_flags->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
} else {
armed->lockdown = false;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
@@ -217,9 +197,20 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
}
}
// HIL can always go to standby
if (hil_enabled && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
if (hil_enabled) {
/* enforce lockdown in HIL */
armed->lockdown = true;
status_flags->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
// HIL can always go to standby
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
}
if (!hil_enabled &&
@@ -227,7 +218,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// Sensors need to be initialized for STANDBY state, except for HIL
if (!status_flags->condition_system_sensors_initialized) {
feedback_provided = true;
valid_transition = false;
@@ -237,8 +227,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
// Finish up the state transition
if (valid_transition) {
armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
@@ -249,11 +238,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
armed->armed_time_ms = 0;
}
}
/* end of atomic state update */
#ifdef __PX4_NUTTX
px4_leave_critical_section(flags);
#endif
}
if (ret == TRANSITION_DENIED) {