Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 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 */
const bool failure_detector_updated = _failure_detector.update();
const bool failure_detector_updated = _failure_detector.update(status);
if (failure_detector_updated) {
src/modules/commander/failure_detector/FailureDetector.cpp
+17 -26
View File
@@ -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;
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;
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;
}
} 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;
src/modules/commander/failure_detector/FailureDetector.hpp
+3 -6
View File
@@ -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 @@ private:
)
// 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();
};