fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 19:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added simple flight termination state machine which enbales parachute on request

Browse Source
This commit is contained in:
Thomas Gubler
2013-12-08 16:52:41 +01:00
parent 4d846b480c
commit 278e05e425
6 changed files with 117 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+51
View File
@@ -65,6 +65,7 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -451,6 +452,18 @@ check_navigation_state_changed()
}
}
bool
check_flighttermination_state_changed()
{
if (flighttermination_state_changed) {
flighttermination_state_changed = false;
return true;
} else {
return false;
}
}
void
set_navigation_state_changed()
{
@@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
}
/**
* Transition from one flightermination state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
{
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->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
return ret;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status