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

FailureDetector - Update failure detector logic in commander.

Browse Source
This commit is contained in:
bresch
2018-07-25 14:05:33 +02:00
committed by Daniel Agar
parent 7908f75b8b
commit 246b3ebc23
2 changed files with 31 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+31 -10
View File
@@ -2181,16 +2181,6 @@ Commander::run()
}
}
if (_failure_detector.update()) {
const auto _failure_status = _failure_detector.get();
if (_failure_status.roll) {
PX4_ERR("Roll angle exceeded");
}
if (_failure_status.pitch) {
PX4_ERR("Pitch angle exceeded");
}
}
/* Reset main state to loiter or auto-mission after takeoff is completed.
* Sometimes, the mission result topic is outdated and the mission is still signaled
* as finished even though we only just started with the takeoff. Therefore, we also
@@ -2293,6 +2283,37 @@ Commander::run()
}
}
/* Check for failure detector status */
if (armed.armed) {
if (_failure_detector.update()) {
const auto _failure_status = _failure_detector.get();
if (_failure_status.roll ||
_failure_status.pitch) {
armed.force_failsafe = true;
status_changed = true;
// Only display an user message if the circuit-breaker is disabled
if (!status_flags.circuit_breaker_flight_termination_disabled) {
static bool parachute_termination_printed = false;
if (!parachute_termination_printed) {
warnx("Flight termination because of attitude exceeding maximum values");
parachute_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe");
}
}
}
}
}
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();