FailureDetector - Ignore attitude check for MC in acro and FW in manu…

…al, acro and rattitude modes
PX4 · Aug 7, 2019 · 622bc94 · 622bc94
1 parent a917f22
commit 622bc94
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 2 deletions.
diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -43,20 +43,69 @@
 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()
+{
+	if (_status != FAILURE_NONE) {
+		_status = FAILURE_NONE;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
 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) {
+			if (nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) {
+				_attitude_is_stabilized = false;
+
+			} else {
+				_attitude_is_stabilized = true;
+			}
+
+		} else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			if (nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
+			    nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
+			    nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE) {
+				_attitude_is_stabilized = false;
+
+			} else {
+				_attitude_is_stabilized = true;
+			}
+		}
+	}
+
+	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
@@ -85,11 +85,15 @@ class FailureDetector : public ModuleParams
 	// 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();
 };