Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

FailureDetector - use standard topic subscription for attitude topic and

Browse Source 
pass vehicle_status from commander instead of subscribing to it.
This commit is contained in:
bresch
2019-09-24 11:21:24 +02:00
committed by Beat Küng
parent 000c1e364c
commit 6139812293
3 changed files with 21 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+1 -1
View File
@@ -2207,7 +2207,7 @@ Commander::run()
} }
/* Check for failure detector status */ /* 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) { if (failure_detector_updated) {
src/modules/commander/failure_detector/FailureDetector.cpp
+17 -26
View File
@@ -41,10 +41,7 @@
#include "FailureDetector.hpp" #include "FailureDetector.hpp"
FailureDetector::FailureDetector(ModuleParams *parent) : FailureDetector::FailureDetector(ModuleParams *parent) :
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))
{ {
} }
@@ -57,11 +54,11 @@ bool FailureDetector::resetStatus()
} }
bool bool
FailureDetector::update() FailureDetector::update(const vehicle_status_s &vehicle_status)
{ {
bool updated(false); bool updated(false);
if (isAttitudeStabilized()) { if (isAttitudeStabilized(vehicle_status)) {
updated = updateAttitudeStatus(); updated = updateAttitudeStatus();
} else { } else {
@@ -72,35 +69,32 @@ FailureDetector::update()
} }
bool bool
FailureDetector::isAttitudeStabilized() FailureDetector::isAttitudeStabilized(const vehicle_status_s &vehicle_status)
{ {
if (_sub_vehicle_status.update()) { bool attitude_is_stabilized{false};
const vehicle_status_s &vehicle_status = _sub_vehicle_status.get(); const uint8_t vehicle_type = vehicle_status.vehicle_type;
const uint8_t vehicle_type = vehicle_status.vehicle_type; const uint8_t nav_state = vehicle_status.nav_state;
const uint8_t nav_state = vehicle_status.nav_state;
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO && attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO &&
nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
} else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { } else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_MANUAL && 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_ACRO &&
nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; 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 bool
FailureDetector::updateAttitudeStatus() FailureDetector::updateAttitudeStatus()
{ {
bool updated(false); bool updated(false);
vehicle_attitude_s attitude;
if (_sub_vehicule_attitude.update()) { if (_sub_vehicule_attitude.update(&attitude)) {
const vehicle_attitude_s &attitude = _sub_vehicule_attitude.get();
const matrix::Eulerf euler(matrix::Quatf(attitude.q)); const matrix::Eulerf euler(matrix::Quatf(attitude.q));
const float roll(euler.phi()); const float roll(euler.phi());
@@ -134,9 +128,6 @@ FailureDetector::updateAttitudeStatus()
} }
updated = true; updated = true;
} else {
updated = false;
} }
return updated; return updated;
src/modules/commander/failure_detector/FailureDetector.hpp
+3 -6
View File
@@ -68,7 +68,7 @@ class FailureDetector : public ModuleParams
public: public:
FailureDetector(ModuleParams *parent); FailureDetector(ModuleParams *parent);
bool update(); bool update(const vehicle_status_s &vehicle_status);
uint8_t getStatus() const { return _status; } uint8_t getStatus() const { return _status; }
bool isFailure() const { return _status != FAILURE_NONE; } bool isFailure() const { return _status != FAILURE_NONE; }
@@ -83,17 +83,14 @@ private:
) )
// Subscriptions // Subscriptions
SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude_setpoint; uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)};
SubscriptionData<vehicle_attitude_s> _sub_vehicule_attitude;
SubscriptionData<vehicle_status_s> _sub_vehicle_status;
uint8_t _status{FAILURE_NONE}; uint8_t _status{FAILURE_NONE};
bool _attitude_is_stabilized{false};
systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false};
bool resetStatus(); bool resetStatus();
bool isAttitudeStabilized(); bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
bool updateAttitudeStatus(); bool updateAttitudeStatus();
}; };