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

FilaureDetector: publish failure_detector_status and just call that from commander

Browse Source
This commit is contained in:
Matthias Grob
2025-05-15 18:15:22 +02:00
parent 8eb57d4f5b
commit db18501b40
4 changed files with 26 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+1 -13
View File
@@ -1921,19 +1921,7 @@ void Commander::run()
_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.timestamp = hrt_absolute_time();
_failure_detector_status_pub.publish(fd_status);
_failure_detector.publishStatus();
}
checkWorkerThread();
src/modules/commander/Commander.hpp
-2
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>
@@ -312,7 +311,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)};
src/modules/commander/failure_detector/FailureDetector.cpp
+17
View File
@@ -206,6 +206,23 @@ 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.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;
src/modules/commander/failure_detector/FailureDetector.hpp
+8 -6
View File
@@ -50,9 +50,12 @@
#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>
@@ -61,8 +64,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>
union failure_detector_status_u {
struct {
@@ -102,9 +103,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; }
void publishStatus();
private:
void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
@@ -139,6 +139,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(