From 000c1e364cd121e57db993e1d4272abcc4f01d97 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 7 Aug 2019 11:35:05 +0200
Subject: [PATCH] FailureDetector - Ignore attitude check for MC in acro and
 rattude, and FW in manual, acro and rattitude modes

---
 .../failure_detector/FailureDetector.cpp      | 41 ++++++++++++++++++-
 .../failure_detector/FailureDetector.hpp      |  4 ++
 2 files changed, 43 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
index 49e3482b7d..07736770bd 100644
--- a/src/modules/commander/failure_detector/FailureDetector.cpp
+++ b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -43,20 +43,57 @@
 FailureDetector::FailureDetector(ModuleParams *parent) :
 	ModuleParams(parent),
 	_sub_vehicle_attitude_setpoint(ORB_ID(vehicle_attitude_setpoint)),
-	_sub_vehicule_attitude(ORB_ID(vehicle_attitude))
+	_sub_vehicule_attitude(ORB_ID(vehicle_attitude)),
+	_sub_vehicle_status(ORB_ID(vehicle_status))
 {
 }
 
+bool FailureDetector::resetStatus()
+{
+	bool status_changed = _status != FAILURE_NONE;
+	_status = FAILURE_NONE;
+
+	return status_changed;
+}
+
 bool
 FailureDetector::update()
 {
 	bool updated(false);
 
-	updated = updateAttitudeStatus();
+	if (isAttitudeStabilized()) {
+		updated = updateAttitudeStatus();
+
+	} else {
+		updated = resetStatus();
+	}
 
 	return updated;
 }
 
+bool
+FailureDetector::isAttitudeStabilized()
+{
+	if (_sub_vehicle_status.update()) {
+		const vehicle_status_s &vehicle_status = _sub_vehicle_status.get();
+		const uint8_t vehicle_type = vehicle_status.vehicle_type;
+		const uint8_t nav_state = vehicle_status.nav_state;
+
+		if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			_attitude_is_stabilized =  nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO &&
+						   nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
+
+		} else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			_attitude_is_stabilized =  nav_state != vehicle_status_s::NAVIGATION_STATE_MANUAL &&
+						   nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO &&
+						   nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
+		}
+	}
+
+	// Note that if we do not have an update from the vehicle_status topic, the previous value is sent
+	return _attitude_is_stabilized;
+}
+
 bool
 FailureDetector::updateAttitudeStatus()
 {
diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
index f9cdd4ce12..5492d7cb97 100644
--- a/src/modules/commander/failure_detector/FailureDetector.hpp
+++ b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -85,11 +85,15 @@ private:
 	// Subscriptions
 	SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude_setpoint;
 	SubscriptionData<vehicle_attitude_s> _sub_vehicule_attitude;
+	SubscriptionData<vehicle_status_s> _sub_vehicle_status;
 
 	uint8_t _status{FAILURE_NONE};
+	bool _attitude_is_stabilized{false};
 
 	systemlib::Hysteresis _roll_failure_hysteresis{false};
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
 
+	bool resetStatus();
+	bool isAttitudeStabilized();
 	bool updateAttitudeStatus();
 };