From 246b3ebc234df2eb5b0b85ea5a48d5d031e29302 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 25 Jul 2018 14:05:33 +0200
Subject: [PATCH] FailureDetector - Update failure detector logic in commander.
---
src/modules/commander/Commander.cpp | 41 ++++++++++++++++++++++-------
src/modules/commander/Commander.hpp | 1 -
2 files changed, 31 insertions(+), 11 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index aa2eb87597..2c97cb67cb 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -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();
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 6f8b60c0fe..d543a7f39c 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -108,7 +108,6 @@ private:
(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain,
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _low_bat_action
-
)
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
--
GitLab