Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

commander: simplify failure detector is attitude stabilized check

Browse Source
This commit is contained in:
Daniel Agar
2021-03-03 20:55:16 -05:00
parent dff975698e
commit a11d2207e4
3 changed files with 5 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+1 -1
View File
@@ -2296,7 +2296,7 @@ Commander::run()
}
/* Check for failure detector status */
if (_failure_detector.update(_status)) {
if (_failure_detector.update(_status, _vehicle_control_mode)) {
_status.failure_detector_status = _failure_detector.getStatus();
_status_changed = true;
src/modules/commander/failure_detector/FailureDetector.cpp
+2 -19
View File
@@ -47,11 +47,11 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
{
}
bool FailureDetector::update(const vehicle_status_s &vehicle_status)
bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
{
uint8_t previous_status = _status;
if (isAttitudeStabilized(vehicle_status)) {
if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus();
if (_param_fd_ext_ats_en.get()) {
@@ -69,23 +69,6 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status)
return _status != previous_status;
}
bool FailureDetector::isAttitudeStabilized(const vehicle_status_s &vehicle_status)
{
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;
} 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;
}
return attitude_is_stabilized;
}
void FailureDetector::updateAttitudeStatus()
{
vehicle_attitude_s attitude;
src/modules/commander/failure_detector/FailureDetector.hpp
+2 -3
View File
@@ -48,11 +48,11 @@
#include <px4_platform_common/module_params.h>
#include <hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>
@@ -73,11 +73,10 @@ class FailureDetector : public ModuleParams
public:
FailureDetector(ModuleParams *parent);
bool update(const vehicle_status_s &vehicle_status);
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
uint8_t getStatus() const { return _status; }
private:
bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
void updateAttitudeStatus();
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);