dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: add guard for parachute deployment

This commit is contained in:
Julian Oes 2014-02-13 16:08:49 +01:00
parent f6694c2cef
commit 036ebdbe78
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/commander/commander.cpp
View File

@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0;
static bool on_usb_power = false; static bool on_usb_power = false;
static float takeoff_alt = 5.0f; static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static struct vehicle_status_s status; static struct vehicle_status_s status;
static struct actuator_armed_s armed; static struct actuator_armed_s armed;
@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* Flight termination */ /* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO //XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED; result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true; ret = true;
@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
/* welcome user */ /* welcome user */
warnx("starting"); warnx("starting");
@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */ /* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
}
/* navigation parameters */ /* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_takeoff_alt, &takeoff_alt);
} param_get(_param_enable_parachute, &parachute_enabled);
} }
orb_check(sp_man_sub, &updated); orb_check(sp_man_sub, &updated);
@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO remove this hack // TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */ /* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(); tune_positive();
} }

1
src/modules/navigator/navigator_params.c
View File

@ -61,3 +61,4 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment