FailureDetector - use standard topic subscription for attitude topic and

pass vehicle_status from commander instead of subscribing to it.
PX4 · Sep 24, 2019 · 6139812 · 6139812
1 parent 000c1e3
commit 6139812
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 35 deletions.
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
@@ -2207,7 +2207,7 @@ Commander::run()
 		}
 
 		/* Check for failure detector status */
-		const bool failure_detector_updated = _failure_detector.update();
+		const bool failure_detector_updated = _failure_detector.update(status);
 
 		if (failure_detector_updated) {
 

diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -41,10 +41,7 @@
 #include "FailureDetector.hpp"
 
 FailureDetector::FailureDetector(ModuleParams *parent) :
-	ModuleParams(parent),
-	_sub_vehicle_attitude_setpoint(ORB_ID(vehicle_attitude_setpoint)),
-	_sub_vehicule_attitude(ORB_ID(vehicle_attitude)),
-	_sub_vehicle_status(ORB_ID(vehicle_status))
+	ModuleParams(parent)
 {
 }
 
@@ -57,11 +54,11 @@ bool FailureDetector::resetStatus()
 }
 
 bool
-FailureDetector::update()
+FailureDetector::update(const vehicle_status_s &vehicle_status)
 {
 	bool updated(false);
 
-	if (isAttitudeStabilized()) {
+	if (isAttitudeStabilized(vehicle_status)) {
 		updated = updateAttitudeStatus();
 
 	} else {
@@ -72,35 +69,32 @@ FailureDetector::update()
 }
 
 bool
-FailureDetector::isAttitudeStabilized()
+FailureDetector::isAttitudeStabilized(const vehicle_status_s &vehicle_status)
 {
-	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;
-		}
+	bool attitude_is_stabilized{false};
+	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;
+	return attitude_is_stabilized;
 }
 
 bool
 FailureDetector::updateAttitudeStatus()
 {
 	bool updated(false);
+	vehicle_attitude_s attitude;
 
-	if (_sub_vehicule_attitude.update()) {
-		const vehicle_attitude_s &attitude = _sub_vehicule_attitude.get();
+	if (_sub_vehicule_attitude.update(&attitude)) {
 
 		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
 		const float roll(euler.phi());
@@ -134,9 +128,6 @@ FailureDetector::updateAttitudeStatus()
 		}
 
 		updated = true;
-
-	} else {
-		updated = false;
 	}
 
 	return updated;

diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -68,7 +68,7 @@ class FailureDetector : public ModuleParams
 public:
 	FailureDetector(ModuleParams *parent);
 
-	bool update();
+	bool update(const vehicle_status_s &vehicle_status);
 
 	uint8_t getStatus() const { return _status; }
 	bool isFailure() const { return _status != FAILURE_NONE; }
@@ -83,17 +83,14 @@ 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;
+	uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)};
 
 	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 isAttitudeStabilized(const vehicle_status_s &vehicle_status);
 	bool updateAttitudeStatus();
 };