Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Control Allocator: Metric #24199

Merged
merged 7 commits into from
Jan 20, 2025
Merged

Control Allocator: Metric #24199

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
efe070d
add: metric allocation
Pedro-Roque Jan 15, 2025
0f7f340
add: actual files
Pedro-Roque Jan 15, 2025
6d3db2a
rft: moved metric allocation to pseudo-inverse via flag with public m…
Pedro-Roque Jan 16, 2025
038c5fb
del: removed metric allocation test and added test in pseudo-inverse …
Pedro-Roque Jan 16, 2025
837b5e6
rft: deleted extra newline at the end of pseudo inverse test file
Pedro-Roque Jan 16, 2025
42e3293
feat: removed unnecessary log include
Pedro-Roque Jan 17, 2025
8c25724
Merge branch 'main' into pr-metric-allocator-library
Pedro-Roque Jan 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 12 additions & 4 deletions src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
}
}
Expand Down
2 changes: 2 additions & 0 deletions src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,13 @@ class ControlAllocationPseudoInverse: public ControlAllocation
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.
Expand Down
24 changes: 24 additions & 0 deletions src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Loading