Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Extend Failure Detector functionalities #14766

Merged
merged 3 commits into from
Jul 1, 2020
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions msg/vehicle_status.msg
Original file line number Diff line number Diff line change
@@ -12,10 +12,11 @@ uint8 ARMING_STATE_MAX = 6

# FailureDetector status
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)

# HIL
uint8 HIL_STATE_OFF = 0
39 changes: 30 additions & 9 deletions src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
@@ -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 {
@@ -2145,18 +2151,33 @@ Commander::run()
failure_detector_updated) {

if (_failure_detector.isFailure()) {
if ((hrt_elapsed_time(&_time_at_takeoff) < 3_s) &&
!_lockdown_triggered) {

const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;

if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs

if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
arm_disarm(false, true, &mavlink_log_pub, "Failure detector");
_status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
}

}

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) {

armed.lockdown = true;
_lockdown_triggered = true;
_status_changed = true;
armed.lockdown = true;
_lockdown_triggered = true;
_status_changed = true;

mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
}

} 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 change: 1 addition & 0 deletions src/modules/commander/Commander.hpp
Original file line number Diff line number Diff line change
@@ -210,6 +210,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
(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 changes: 16 additions & 0 deletions src/modules/commander/commander_params.c
Original file line number Diff line number Diff line change
@@ -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);
68 changes: 60 additions & 8 deletions src/modules/commander/failure_detector/FailureDetector.cpp
Original file line number Diff line number Diff line change
@@ -47,28 +47,43 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
{
}

bool FailureDetector::resetStatus()
bool FailureDetector::resetAttitudeStatus()
{
bool status_changed = _status != FAILURE_NONE;
_status = FAILURE_NONE;

int attitude_fields_bitmask = _status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bresch / @bkueng I could not come up with a better naming for this variable.. given that contains FAILURE_ALT and FAILURE_EXT is not really an attitude_fields_bitmask .. any suggestion?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

non_esc_bitmask? Otherwise this is fine.

bool status_changed(false);

if (attitude_fields_bitmask > FAILURE_NONE) {
_status &= ~attitude_fields_bitmask;
status_changed = true;
}

return status_changed;
}

bool
FailureDetector::update(const vehicle_status_s &vehicle_status)
{

bool updated(false);

if (isAttitudeStabilized(vehicle_status)) {
updated = updateAttitudeStatus();
updated |= updateAttitudeStatus();

if (_param_fd_ext_ats_en.get()) {
updated |= updateExternalAtsStatus();
}

} else {
updated = resetStatus();
updated |= resetAttitudeStatus();
}

if (_sub_esc_status.updated()) {

if (_param_escs_en.get()) {
updated |= updateEscsStatus(vehicle_status);
}

}

return updated;
@@ -143,9 +158,9 @@ bool
FailureDetector::updateExternalAtsStatus()
{
pwm_input_s pwm_input;
bool updated = _sub_pwm_input.update(&pwm_input);
bool updated(false);

if (updated) {
if (_sub_pwm_input.update(&pwm_input)) {

uint32_t pulse_width = pwm_input.pulse_width;
bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
@@ -161,7 +176,44 @@ FailureDetector::updateExternalAtsStatus()
if (_ext_ats_failure_hysteresis.get_state()) {
_status |= FAILURE_EXT;
}

updated = true;
}

return updated;
}

bool
FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
{
hrt_abstime time_now = hrt_absolute_time();
bool updated(false);

if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {

esc_status_s esc_status{};
_sub_esc_status.copy(&esc_status);

int all_escs_armed = (1 << esc_status.esc_count) - 1;


_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);

if (_esc_failure_hysteresis.get_state() && !(_status & FAILURE_ARM_ESCS)) {
_status |= FAILURE_ARM_ESCS;
updated = true;
}

} else {
// reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, time_now);

if (_status & FAILURE_ARM_ESCS) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bkueng this should guarantee that the status is updated only if the bit is reset.

_status &= ~FAILURE_ARM_ESCS;
updated = true;
}
}

return updated;
}
}
11 changes: 9 additions & 2 deletions src/modules/commander/failure_detector/FailureDetector.hpp
Original file line number Diff line number Diff line change
@@ -54,6 +54,7 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>

typedef enum {
@@ -62,6 +63,7 @@ typedef enum {
FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
} failure_detector_bitmak;

using uORB::SubscriptionData;
@@ -84,11 +86,14 @@ class FailureDetector : public ModuleParams
(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en

)

// Subscriptions
uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)};
uORB::Subscription _sub_esc_status{ORB_ID(esc_status)};
uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)};


@@ -97,9 +102,11 @@ class FailureDetector : public ModuleParams
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};

bool resetStatus();
bool resetAttitudeStatus();
bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
bool updateAttitudeStatus();
bool updateExternalAtsStatus();
bool updateEscsStatus(const vehicle_status_s &vehicle_status);
};
12 changes: 12 additions & 0 deletions src/modules/commander/failure_detector/failure_detector_params.c
Original file line number Diff line number Diff line change
@@ -129,3 +129,15 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);

/**
* Enable checks on ESCs that report their arming state.
* If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
* Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
*
* @boolean
* @reboot_required true
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ESCS_EN, 1);