fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FilaureDetector: publish failure_detector_status in class and call that from commander

Browse Source
This commit is contained in:
Matthias Grob 2025-05-15 18:15:22 +02:00 committed by Silvan Fuhrer
parent 2adc36b2af
commit 540e80eb77
4 changed files with 27 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/commander/Commander.cpp
View File

@ -1956,21 +1956,7 @@ void Commander::run()
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
// failure_detector_status publish
failure_detector_status_s fd_status{};
fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch;
fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
fd_status.fd_ext = _failure_detector.getStatusFlags().ext;
fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
fd_status.motor_stop_mask = _failure_detector.getMotorStopMask();
fd_status.timestamp = hrt_absolute_time();
_failure_detector_status_pub.publish(fd_status);
_failure_detector.publishStatus();
}
checkWorkerThread();

2
src/modules/commander/Commander.hpp
View File

@ -54,7 +54,6 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
@ -313,7 +312,6 @@ private:
// Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};

18
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -89,6 +89,24 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
return _failure_detector_status.value != status_prev.value;
}
void FailureDetector::publishStatus()
{
failure_detector_status_s failure_detector_status{};
failure_detector_status.fd_roll = _failure_detector_status.flags.roll;
failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch;
failure_detector_status.fd_alt = _failure_detector_status.flags.alt;
failure_detector_status.fd_ext = _failure_detector_status.flags.ext;
failure_detector_status.fd_arm_escs = _failure_detector_status.flags.arm_escs;
failure_detector_status.fd_battery = _failure_detector_status.flags.battery;
failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop;
failure_detector_status.fd_motor = _failure_detector_status.flags.motor;
failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState();
failure_detector_status.motor_failure_mask = _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask;
failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask();
failure_detector_status.timestamp = hrt_absolute_time();
_failure_detector_status_pub.publish(failure_detector_status);
}
void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
{
vehicle_attitude_s attitude;

14
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -51,16 +51,18 @@
#include <px4_platform_common/module_params.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/pwm_input.h>
union failure_detector_status_u {
struct {
@ -86,10 +88,8 @@ public:
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
const failure_detector_status_u &getStatus() const { return _failure_detector_status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _failure_detector_status.flags; }
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
uint16_t getMotorStopMask() { return _failure_injector.getMotorStopMask(); }
void publishStatus();
private:
void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
@ -124,6 +124,8 @@ private:
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
FailureInjector _failure_injector;
DEFINE_PARAMETERS(