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

Control Allocator: Add option for metric allocation (skip normalization) (#24199)

Browse Source 
* add: metric allocation

* add: actual files

* rft: moved metric allocation to pseudo-inverse via flag with public method

* del: removed metric allocation test and added test in pseudo-inverse testing

* rft: deleted extra newline at the end of pseudo inverse test file

* feat: removed unnecessary log include
This commit is contained in:
Pedro Roque 2025-01-20 13:30:51 +01:00 committed by GitHub
parent 65a8cc0e0a
commit b09340cc98
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 38 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
View File

@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix(
update_normalization_scale);
_mix_update_needed = true;
_normalization_needs_update = update_normalization_scale;
if (_metric_allocation && update_normalization_scale) {
// adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test
_normalization_needs_update = false;
}
}
void
@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
if (_mix_update_needed) {
matrix::geninv(_effectiveness, _mix);
if (_normalization_needs_update && !_had_actuator_failure) {
updateControlAllocationMatrixScale();
_normalization_needs_update = false;
if (!_metric_allocation) {
if (_normalization_needs_update && !_had_actuator_failure) {
updateControlAllocationMatrixScale();
_normalization_needs_update = false;
}
normalizeControlAllocationMatrix();
}
normalizeControlAllocationMatrix();
_mix_update_needed = false;
}
}

2
src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
View File

@ -57,11 +57,13 @@ public:
void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
bool update_normalization_scale) override;
void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; }
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
bool _mix_update_needed{false};
bool _metric_allocation{false};
/**
* Recalculate pseudo inverse if required.

24
src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp
View File

@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase)
EXPECT_EQ(actuator_sp, actuator_sp_expected);
EXPECT_EQ(control_allocated, control_allocated_expected);
}
TEST(ControlAllocationMetricTest, AllZeroCase)
{
ControlAllocationPseudoInverse method;
matrix::Vector<float, 6> control_sp;
matrix::Vector<float, 6> control_allocated;
matrix::Vector<float, 6> control_allocated_expected;
matrix::Matrix<float, 6, 16> effectiveness;
matrix::Vector<float, 16> actuator_sp;
matrix::Vector<float, 16> actuator_trim;
matrix::Vector<float, 16> linearization_point;
matrix::Vector<float, 16> actuator_sp_expected;
method.setMetricAllocation(true);
method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
method.setControlSetpoint(control_sp);
method.allocate();
actuator_sp = method.getActuatorSetpoint();
control_allocated_expected = method.getAllocatedControl();
EXPECT_EQ(actuator_sp, actuator_sp_expected);
EXPECT_EQ(control_allocated, control_allocated_expected);
}