add: actual files

src/lib/control_allocation/control_allocation/ControlAllocationMetric.cpp

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ControlAllocationMetric.hpp
+ *
+ * Control allocation with metric units.
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#include "ControlAllocationMetric.hpp"
+#include <px4_platform_common/log.h>
+
+/*
+ * ControlAllocationMetric
+ *
+ * Note that the effectiveness matrix is in metric units and that
+ * 'update_normalization_scale' is ignored.
+ */
+void
+ControlAllocationMetric::setEffectivenessMatrix(
+	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
+{
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
+			false);
+	_mix_update_needed = true;
+}
+
+void
+ControlAllocationMetric::updatePseudoInverse()
+{
+	if (_mix_update_needed) {
+		matrix::geninv(_effectiveness, _mix);
+		_mix_update_needed = false;
+	}
+}
+
+void
+ControlAllocationMetric::allocate()
+{
+	//Compute new gains if needed
+	updatePseudoInverse();
+
+	_prev_actuator_sp = _actuator_sp;
+
+	// Allocate
+	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+}

src/lib/control_allocation/control_allocation/ControlAllocationMetric.hpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ControlAllocationMetric.hpp
+ *
+ * Control allocation with metric units.
+ *
+ * The metric allocation takes into account maximum thrust and torque
+ * provided by the actuators to calculate setpoints that allow to allocate
+ * specific body force and torque, keeping the scale of the target setpoints.
+ * No normalization is done in this case. Note that, similarly to the PseudoInverse
+ * allocation, saturation is handled by clipping.
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#pragma once
+
+#include "ControlAllocation.hpp"
+
+class ControlAllocationMetric: public ControlAllocation
+{
+public:
+	ControlAllocationMetric() = default;
+	virtual ~ControlAllocationMetric() = default;
+
+	void allocate() override;
+	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;
+
+protected:
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
+
+	bool _mix_update_needed{false};
+
+	/**
+	 * Recalculate pseudo inverse if required.
+	 *
+	 */
+	void updatePseudoInverse();
+
+};

src/lib/control_allocation/control_allocation/ControlAllocationMetricTest.cpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ControlAllocationTest.cpp
+ *
+ * Tests for Control Allocation metric class
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#include <gtest/gtest.h>
+#include "ControlAllocationMetric.hpp"
+#include <iostream>
+
+using namespace matrix;
+
+TEST(ControlAllocationMetricTest, AllZeroCase)
+{
+	ControlAllocationMetric 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.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	method.clipActuatorSetpoint();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}