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

Merge branch 'beta' into offboard2

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-29 21:21:16 +01:00
parent 01c9092213 6f559b279e
commit cf6d89301b
73 changed files with 2599 additions and 1873 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,7 +63,7 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool flighttermination_state_changed = true;
static bool failsafe_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -220,61 +220,66 @@ check_arming_state_changed()
}
transition_result_t
main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_main_state == current_state->main_state) {
ret = TRANSITION_NOT_CHANGED;
/* transition may be denied even if requested the same state because conditions may be changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
} else {
case MAIN_STATE_SEATBELT:
switch (new_main_state) {
case MAIN_STATE_MANUAL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
/* need at minimum altitude estimate */
if (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_EASY:
/* need at minimum local position estimate */
if (current_state->condition_local_position_valid ||
current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_OFFBOARD:
ret = TRANSITION_CHANGED;
break;
}
if (ret == TRANSITION_CHANGED) {
current_state->main_state = new_main_state;
break;
case MAIN_STATE_EASY:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_OFFBOARD:
/* need global position estimate */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
@@ -294,10 +299,10 @@ check_main_state_changed()
}
bool
check_flighttermination_state_changed()
check_failsafe_state_changed()
{
if (flighttermination_state_changed) {
flighttermination_state_changed = false;
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
@@ -368,28 +373,49 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/**
* Transition from one flightermination state to another
* Transition from one failsafe state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
ret = TRANSITION_NOT_CHANGED;
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
}
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
default:
@@ -397,9 +423,13 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
}
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
}