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

Commander: make optional tilt-check after takeoff (failure detector). 

- Introduced COM_LKDOWN_TKO parameter
    - Introduced auto disarm for lockdown state
    - Do not trigger flight termiantion if system is in lockdown

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2020-04-15 19:30:33 +02:00 committed by Mathieu Bresciani
parent 6358dd400a
commit a7f2f2908b
3 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/commander/Commander.cpp
View File

@ -1627,10 +1627,16 @@ Commander::run()
}
// Auto disarm after 5 seconds if kill switch is engaged
_auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time());
_auto_disarm_killed.set_state_and_update(armed.manual_lockdown || armed.lockdown, hrt_absolute_time());
if (_auto_disarm_killed.get_state()) {
arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming");
if (armed.manual_lockdown) {
arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming");
} else {
arm_disarm(false, true, &mavlink_log_pub, "System in lockdown, disarming");
}
}
} else {
@ -2152,10 +2158,9 @@ Commander::run()
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
}
} else if (hrt_elapsed_time(&_time_at_takeoff) < 3_s) {
} else if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) {
// This handles the case where something fails during the early takeoff phase
if (!_lockdown_triggered) {
@ -2167,7 +2172,7 @@ Commander::run()
}
} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered) {
!_flight_termination_triggered && !_lockdown_triggered) {
armed.force_failsafe = true;
_flight_termination_triggered = true;

1
src/modules/commander/Commander.hpp
View File

@ -210,6 +210,7 @@ private:
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
// Engine failure
(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,

16
src/modules/commander/commander_params.c
View File

@ -924,3 +924,19 @@ PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f);
* @max 4
*/
PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);
/**
* Timeout for detecting a failure after takeoff
*
* A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into
* a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R.
* The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW), Rattitude and Manual (FW).
* A zero or negative value means that the check is disabled.
*
* @group Commander
* @unit s
* @min -1.0
* @max 5.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);