Copy control allocator to add metric allocator module

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -8,7 +8,8 @@
 #
 # Start Control Allocator
 #
-control_allocator start
+# control_allocator start
+metric_allocator start
 
 #
 # Start Multicopter Rate Controller.

boards/px4/sitl/default.px4board

@@ -11,7 +11,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=n
+CONFIG_MODULES_METRIC_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y

src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectiveness.hpp"
+#include "../ControlAllocation/ControlAllocation.hpp"
+
+#include <px4_platform_common/log.h>
+
+int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque,
+		const matrix::Vector3f &thrust)
+{
+	int actuator_idx = num_actuators_matrix[selected_matrix];
+
+	if (actuator_idx >= NUM_ACTUATORS) {
+		PX4_ERR("Too many actuators");
+		return -1;
+	}
+
+	if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) {
+		PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)");
+		return -1;
+	}
+
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
+	matrix_selection_indexes[totalNumActuators()] = selected_matrix;
+	++num_actuators[(int)type];
+	return num_actuators_matrix[selected_matrix]++;
+}
+
+void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count)
+{
+	int total_count = totalNumActuators();
+
+	for (int i = 0; i < count; ++i) {
+		matrix_selection_indexes[i + total_count] = selected_matrix;
+	}
+
+	num_actuators[(int)type] += count;
+	num_actuators_matrix[selected_matrix] += count;
+}
+
+int ActuatorEffectiveness::Configuration::totalNumActuators() const
+{
+	int total_count = 0;
+
+	for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+		total_count += num_actuators_matrix[i];
+	}
+
+	return total_count;
+}
+
+void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
+{
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (stoppable_motors_mask & motor_mask) {
+
+			// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
+			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
+				_stopped_motors_mask |= motor_mask;
+
+			} else {
+				_stopped_motors_mask &= ~motor_mask;
+			}
+		}
+	}
+}

src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 ActuatorEffectiveness.hpp
+ *
+ * Interface for Actuator Effectiveness
+ *
+ * @author Julien Lecoeur <[email protected]>
+ */
+
+#pragma once
+
+#include <cstdint>
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/control_allocator_status.h>
+
+enum class AllocationMethod {
+	NONE = -1,
+	PSEUDO_INVERSE = 0,
+	SEQUENTIAL_DESATURATION = 1,
+	AUTO = 2,
+};
+
+enum class ActuatorType {
+	MOTORS = 0,
+	SERVOS,
+
+	COUNT
+};
+
+enum class EffectivenessUpdateReason {
+	NO_EXTERNAL_UPDATE = 0,
+	CONFIGURATION_UPDATE = 1,
+	MOTOR_ACTIVATION_UPDATE = 2,
+};
+
+
+class ActuatorEffectiveness
+{
+public:
+	ActuatorEffectiveness() = default;
+	virtual ~ActuatorEffectiveness() = default;
+
+	static constexpr int NUM_ACTUATORS = 16;
+	static constexpr int NUM_AXES = 6;
+
+	enum ControlAxis {
+		ROLL = 0,
+		PITCH,
+		YAW,
+		THRUST_X,
+		THRUST_Y,
+		THRUST_Z
+	};
+
+	static constexpr int MAX_NUM_MATRICES = 2;
+
+	using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
+	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	struct Configuration {
+		/**
+		 * Add an actuator to the selected matrix, returning the index, or -1 on error
+		 */
+		int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust);
+
+		/**
+		 * Call this after manually adding N actuators to the selected matrix
+		 */
+		void actuatorsAdded(ActuatorType type, int count);
+
+		int totalNumActuators() const;
+
+		/// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos
+		EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES];
+
+		int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
+		ActuatorVector trim[MAX_NUM_MATRICES];
+
+		ActuatorVector linearization_point[MAX_NUM_MATRICES];
+
+		int selected_matrix;
+
+		uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
+		int num_actuators[(int)ActuatorType::COUNT];
+	};
+
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	virtual void setFlightPhase(const FlightPhase &flight_phase)
+	{
+		_flight_phase = flight_phase;
+	}
+
+	/**
+	 * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
+	 * This is expected to stay constant.
+	 */
+	virtual int numMatrices() const { return 1; }
+
+	/**
+	 * Get the desired allocation method(s) for each matrix, if configured as AUTO
+	 */
+	virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const
+	{
+		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+			allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE;
+		}
+	}
+
+	/**
+	 * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
+	 */
+	virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
+	{
+		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+			normalize[i] = false;
+		}
+	}
+
+	/**
+	 * Get the control effectiveness matrix if updated
+	 *
+	 * @return true if updated and matrix is set
+	 */
+	virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
+
+	/**
+	 * Get the current flight phase
+	 *
+	 * @return Flight phase
+	 */
+	const FlightPhase &getFlightPhase() const
+	{
+		return _flight_phase;
+	}
+
+	/**
+	 * Display name
+	 */
+	virtual const char *name() const = 0;
+
+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
+	 *
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
+
+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * This can be used to e.g. add non-linear or external terms.
+	 * It is called after the matrix multiplication and before final clipping.
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
+
+	/**
+	 * Get a bitmask of motors to be stopped
+	 */
+	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
+
+	/**
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
+	 */
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
+
+	/**
+	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
+	 *
+	 * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
+	 * @param actuator_sp outcome of the allocation to determine if the motor should be stopped
+	 */
+	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
+
+protected:
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+	uint32_t _stopped_motors_mask{0};
+};