Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

Commander: separate failure detector cases

Browse Source 
One case could lead to the action for the other!
This commit is contained in:
Matthias Grob
2020-07-30 16:37:36 +02:00
parent f47bb95b26
commit 390edb770d
2 changed files with 18 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+18 -20
View File
@@ -2238,40 +2238,38 @@ Commander::run()
status.failure_detector_status = _failure_detector.getStatus();
_status_changed = true;
if (armed.armed && _failure_detector.isFailure()) {
const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;
if (armed.armed) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
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) {
if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::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) {
if (status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get());
if (is_second_after_takeoff && !_lockdown_triggered) {
// This handles the case where something fails during the early takeoff phase
armed.lockdown = true;
_lockdown_triggered = true;
_status_changed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered && !_lockdown_triggered) {
armed.force_failsafe = true;
_flight_termination_triggered = true;
_status_changed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
}
} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered && !_lockdown_triggered) {
armed.force_failsafe = true;
_flight_termination_triggered = true;
_status_changed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
}
}
}
src/modules/commander/failure_detector/FailureDetector.hpp
-2
View File
@@ -74,9 +74,7 @@ public:
FailureDetector(ModuleParams *parent);
bool update(const vehicle_status_s &vehicle_status);
uint8_t getStatus() const { return _status; }
bool isFailure() const { return _status != FAILURE_NONE; }
private:
void resetAttitudeStatus();