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

control_allocator: remove failed motor from effectiveness

Browse Source 
- limit to handling only 1 motor failure
- Log which motor failures are handled
- Remove motor from effectiveness matrix without
  recomputing the scale / normalization
This commit is contained in:
Alessandro Simovic
2021-11-26 16:51:27 +01:00
committed by Beat Küng
parent fb71e7587c
commit 20ccfbb719
7 changed files with 133 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.hpp
+14
View File
@@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/failure_detector_status.h>
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
{
@@ -128,6 +129,8 @@ private:
void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
void check_for_motor_failures();
void publish_control_allocator_status();
void publish_actuator_controls();
@@ -152,6 +155,11 @@ private:
HELICOPTER = 10,
};
enum class FailureMode {
IGNORE = 0,
REMOVE_FIRST_FAILING_MOTOR = 1,
};
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness
@@ -175,10 +183,15 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
matrix::Vector3f _torque_sp;
matrix::Vector3f _thrust_sp;
// Reflects motor failures that are currently handled, not motor failures that are reported.
// For example, the system might report two motor failures, but only the first one is handled by CA
uint16_t _handled_motor_failure_bitmask{0};
perf_counter_t _loop_perf; /**< loop duration performance counter */
bool _armed{false};
@@ -193,6 +206,7 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)