mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 14:53:33 +08:00
Make control allocation and actuator effectiveness a non-module-specific library (#24196)
* Remove more circular dependencies with ActuatorEffectiveness * Separate vehicle specific actuator effectiveness Keep actuator effectivenss in control allocator * Remove test dependency for now * Group library directories Fix * Change directory names * Rebase fix
This commit is contained in:
Jaeyoung Lim
@@ -1,103 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <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](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,225 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#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};
|
||||
};
|
||||
@@ -1,78 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectiveness.cpp
|
||||
ActuatorEffectiveness.hpp
|
||||
ActuatorEffectivenessUUV.cpp
|
||||
ActuatorEffectivenessUUV.hpp
|
||||
ActuatorEffectivenessControlSurfaces.cpp
|
||||
ActuatorEffectivenessControlSurfaces.hpp
|
||||
ActuatorEffectivenessCustom.cpp
|
||||
ActuatorEffectivenessCustom.hpp
|
||||
ActuatorEffectivenessFixedWing.cpp
|
||||
ActuatorEffectivenessFixedWing.hpp
|
||||
ActuatorEffectivenessHelicopter.cpp
|
||||
ActuatorEffectivenessHelicopter.hpp
|
||||
ActuatorEffectivenessHelicopterCoaxial.cpp
|
||||
ActuatorEffectivenessHelicopterCoaxial.hpp
|
||||
ActuatorEffectivenessMCTilt.cpp
|
||||
ActuatorEffectivenessMCTilt.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
ActuatorEffectivenessMultirotor.hpp
|
||||
ActuatorEffectivenessTilts.cpp
|
||||
ActuatorEffectivenessTilts.hpp
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
ActuatorEffectivenessRotors.hpp
|
||||
ActuatorEffectivenessStandardVTOL.cpp
|
||||
ActuatorEffectivenessStandardVTOL.hpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.cpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.hpp
|
||||
ActuatorEffectivenessTailsitterVTOL.cpp
|
||||
ActuatorEffectivenessTailsitterVTOL.hpp
|
||||
ActuatorEffectivenessRoverAckermann.hpp
|
||||
ActuatorEffectivenessRoverAckermann.cpp
|
||||
RpmControl.cpp
|
||||
RpmControl.hpp
|
||||
)
|
||||
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
PID
|
||||
)
|
||||
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
@@ -32,8 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
add_subdirectory(ActuatorEffectiveness)
|
||||
add_subdirectory(ControlAllocation)
|
||||
add_subdirectory(VehicleActuatorEffectiveness)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__control_allocator
|
||||
@@ -50,6 +49,7 @@ px4_add_module(
|
||||
DEPENDS
|
||||
mathlib
|
||||
ActuatorEffectiveness
|
||||
VehicleActuatorEffectiveness
|
||||
ControlAllocation
|
||||
px4_work_queue
|
||||
SlewRate
|
||||
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(ControlAllocation
|
||||
ControlAllocation.cpp
|
||||
ControlAllocation.hpp
|
||||
ControlAllocationPseudoInverse.cpp
|
||||
ControlAllocationPseudoInverse.hpp
|
||||
ControlAllocationSequentialDesaturation.cpp
|
||||
ControlAllocationSequentialDesaturation.hpp
|
||||
)
|
||||
target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ControlAllocation PRIVATE mathlib)
|
||||
|
||||
px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
|
||||
px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
|
||||
@@ -1,126 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocation.cpp
|
||||
*
|
||||
* Interface for Control Allocation Algorithms
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ControlAllocation.hpp"
|
||||
|
||||
ControlAllocation::ControlAllocation()
|
||||
{
|
||||
_control_allocation_scale.setAll(1.f);
|
||||
_actuator_min.setAll(0.f);
|
||||
_actuator_max.setAll(1.f);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::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)
|
||||
{
|
||||
_effectiveness = effectiveness;
|
||||
ActuatorVector linearization_point_clipped = linearization_point;
|
||||
clipActuatorSetpoint(linearization_point_clipped);
|
||||
_actuator_trim = actuator_trim + linearization_point_clipped;
|
||||
clipActuatorSetpoint(_actuator_trim);
|
||||
_num_actuators = num_actuators;
|
||||
_control_trim = _effectiveness * linearization_point_clipped;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::setActuatorSetpoint(
|
||||
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_sp)
|
||||
{
|
||||
// Set actuator setpoint
|
||||
_actuator_sp = actuator_sp;
|
||||
|
||||
// Clip
|
||||
clipActuatorSetpoint(_actuator_sp);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
|
||||
{
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_max(i) < _actuator_min(i)) {
|
||||
actuator(i) = _actuator_trim(i);
|
||||
|
||||
} else if (actuator(i) < _actuator_min(i)) {
|
||||
actuator(i) = _actuator_min(i);
|
||||
|
||||
} else if (actuator(i) > _actuator_max(i)) {
|
||||
actuator(i) = _actuator_max(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
|
||||
ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
|
||||
const
|
||||
{
|
||||
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_normalized;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_min(i) < _actuator_max(i)) {
|
||||
actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
|
||||
|
||||
} else {
|
||||
actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
|
||||
}
|
||||
}
|
||||
|
||||
return actuator_normalized;
|
||||
}
|
||||
|
||||
void ControlAllocation::applySlewRateLimit(float dt)
|
||||
{
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_slew_rate_limit(i) > FLT_EPSILON) {
|
||||
float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i);
|
||||
float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i);
|
||||
|
||||
if (delta_sp > delta_sp_max) {
|
||||
_actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max;
|
||||
|
||||
} else if (delta_sp < -delta_sp_max) {
|
||||
_actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,247 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocation.hpp
|
||||
*
|
||||
* Interface for Control Allocation Algorithms
|
||||
*
|
||||
* Implementers of this interface are expected to update the members
|
||||
* of this base class in the `allocate` method.
|
||||
*
|
||||
* Example usage:
|
||||
* ```
|
||||
* [...]
|
||||
* // Initialization
|
||||
* ControlAllocationMethodImpl alloc();
|
||||
* alloc.setEffectivenessMatrix(effectiveness, actuator_trim);
|
||||
* alloc.setActuatorMin(actuator_min);
|
||||
* alloc.setActuatorMin(actuator_max);
|
||||
*
|
||||
* while (1) {
|
||||
* [...]
|
||||
*
|
||||
* // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint
|
||||
* alloc.setControlSetpoint(control_sp);
|
||||
* alloc.allocate();
|
||||
* actuator_sp = alloc.getActuatorSetpoint();
|
||||
*
|
||||
* // Check if the control setpoint was fully allocated
|
||||
* unallocated_control = control_sp - alloc.getAllocatedControl()
|
||||
*
|
||||
* [...]
|
||||
* }
|
||||
* ```
|
||||
*
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
class ControlAllocation
|
||||
{
|
||||
public:
|
||||
ControlAllocation();
|
||||
virtual ~ControlAllocation() = default;
|
||||
|
||||
static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
|
||||
static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
|
||||
|
||||
typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
|
||||
|
||||
enum ControlAxis {
|
||||
ROLL = 0,
|
||||
PITCH,
|
||||
YAW,
|
||||
THRUST_X,
|
||||
THRUST_Y,
|
||||
THRUST_Z
|
||||
};
|
||||
|
||||
/**
|
||||
* Allocate control setpoint to actuators
|
||||
*/
|
||||
virtual void allocate() = 0;
|
||||
|
||||
/**
|
||||
* Set actuator failure flag
|
||||
* This prevents a change of the scaling in the matrix normalization step
|
||||
* in case of a motor failure.
|
||||
*
|
||||
* @param failure Motor failure flag
|
||||
*/
|
||||
void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
|
||||
|
||||
/**
|
||||
* Set the control effectiveness matrix
|
||||
*
|
||||
* @param B Effectiveness matrix
|
||||
*/
|
||||
virtual 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);
|
||||
|
||||
/**
|
||||
* Get the allocated actuator vector
|
||||
*
|
||||
* @return Actuator vector
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const { return _actuator_sp; }
|
||||
|
||||
/**
|
||||
* Set the desired control vector
|
||||
*
|
||||
* @param Control vector
|
||||
*/
|
||||
void setControlSetpoint(const matrix::Vector<float, NUM_AXES> &control) { _control_sp = control; }
|
||||
|
||||
/**
|
||||
* Get the desired control vector
|
||||
*
|
||||
* @return Control vector
|
||||
*/
|
||||
const matrix::Vector<float, NUM_AXES> &getControlSetpoint() const { return _control_sp; }
|
||||
|
||||
/**
|
||||
* Get the allocated control vector
|
||||
*
|
||||
* @return Control vector
|
||||
*/
|
||||
matrix::Vector<float, NUM_AXES> getAllocatedControl() const
|
||||
{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
|
||||
|
||||
/**
|
||||
* Get the control effectiveness matrix
|
||||
*
|
||||
* @return Effectiveness matrix
|
||||
*/
|
||||
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const { return _effectiveness; }
|
||||
|
||||
/**
|
||||
* Set the minimum actuator values
|
||||
*
|
||||
* @param actuator_min Minimum actuator values
|
||||
*/
|
||||
void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min) { _actuator_min = actuator_min; }
|
||||
|
||||
/**
|
||||
* Get the minimum actuator values
|
||||
*
|
||||
* @return Minimum actuator values
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const { return _actuator_min; }
|
||||
|
||||
/**
|
||||
* Set the maximum actuator values
|
||||
*
|
||||
* @param actuator_max Maximum actuator values
|
||||
*/
|
||||
void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) { _actuator_max = actuator_max; }
|
||||
|
||||
/**
|
||||
* Get the maximum actuator values
|
||||
*
|
||||
* @return Maximum actuator values
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const { return _actuator_max; }
|
||||
|
||||
/**
|
||||
* Set the current actuator setpoint.
|
||||
*
|
||||
* Use this when a new allocation method is started to initialize it properly.
|
||||
* In most cases, it is not needed to call this method before `allocate()`.
|
||||
* Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`.
|
||||
*
|
||||
* @param actuator_sp Actuator setpoint
|
||||
*/
|
||||
void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
|
||||
|
||||
void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
|
||||
{ _actuator_slew_rate_limit = slew_rate_limit; }
|
||||
|
||||
/**
|
||||
* Apply slew rate to current actuator setpoint
|
||||
*/
|
||||
void applySlewRateLimit(float dt);
|
||||
|
||||
/**
|
||||
* Clip the actuator setpoint between minimum and maximum values.
|
||||
*
|
||||
* The output is in the range [min; max]
|
||||
*
|
||||
* @param actuator Actuator vector to clip
|
||||
*/
|
||||
void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
|
||||
|
||||
void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
|
||||
|
||||
/**
|
||||
* Normalize the actuator setpoint between minimum and maximum values.
|
||||
*
|
||||
* The output is in the range [-1; +1]
|
||||
*
|
||||
* @param actuator Actuator vector to normalize
|
||||
*
|
||||
* @return Clipped actuator setpoint
|
||||
*/
|
||||
matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
|
||||
const;
|
||||
|
||||
virtual void updateParameters() {}
|
||||
|
||||
int numConfiguredActuators() const { return _num_actuators; }
|
||||
|
||||
void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; }
|
||||
|
||||
protected:
|
||||
friend class ControlAllocator; // for _actuator_sp
|
||||
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; ///< Effectiveness matrix
|
||||
matrix::Vector<float, NUM_AXES> _control_allocation_scale; ///< Scaling applied during allocation
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; ///< Neutral actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; ///< Minimum actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; ///< Maximum actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; ///< Slew rate limit
|
||||
matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp; ///< Previous actuator setpoint
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_sp; ///< Actuator setpoint
|
||||
matrix::Vector<float, NUM_AXES> _control_sp; ///< Control setpoint
|
||||
matrix::Vector<float, NUM_AXES> _control_trim; ///< Control at trim actuator values
|
||||
int _num_actuators{0};
|
||||
bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns
|
||||
bool _had_actuator_failure{false};
|
||||
};
|
||||
@@ -1,181 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocationPseudoInverse.cpp
|
||||
*
|
||||
* Simple Control Allocation Algorithm
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ControlAllocationPseudoInverse.hpp"
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::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,
|
||||
update_normalization_scale);
|
||||
_mix_update_needed = true;
|
||||
_normalization_needs_update = update_normalization_scale;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::updatePseudoInverse()
|
||||
{
|
||||
if (_mix_update_needed) {
|
||||
matrix::geninv(_effectiveness, _mix);
|
||||
|
||||
if (_normalization_needs_update && !_had_actuator_failure) {
|
||||
updateControlAllocationMatrixScale();
|
||||
_normalization_needs_update = false;
|
||||
}
|
||||
|
||||
normalizeControlAllocationMatrix();
|
||||
_mix_update_needed = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
|
||||
{
|
||||
// Same scale on roll and pitch
|
||||
if (_normalize_rpy) {
|
||||
|
||||
int num_non_zero_roll_torque = 0;
|
||||
int num_non_zero_pitch_torque = 0;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
|
||||
if (fabsf(_mix(i, 0)) > 1e-3f) {
|
||||
++num_non_zero_roll_torque;
|
||||
}
|
||||
|
||||
if (fabsf(_mix(i, 1)) > 1e-3f) {
|
||||
++num_non_zero_pitch_torque;
|
||||
}
|
||||
}
|
||||
|
||||
float roll_norm_scale = 1.f;
|
||||
|
||||
if (num_non_zero_roll_torque > 0) {
|
||||
roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f));
|
||||
}
|
||||
|
||||
float pitch_norm_scale = 1.f;
|
||||
|
||||
if (num_non_zero_pitch_torque > 0) {
|
||||
pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f));
|
||||
}
|
||||
|
||||
_control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale);
|
||||
_control_allocation_scale(1) = _control_allocation_scale(0);
|
||||
|
||||
// Scale yaw separately
|
||||
_control_allocation_scale(2) = _mix.col(2).max();
|
||||
|
||||
} else {
|
||||
_control_allocation_scale(0) = 1.f;
|
||||
_control_allocation_scale(1) = 1.f;
|
||||
_control_allocation_scale(2) = 1.f;
|
||||
}
|
||||
|
||||
// Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators
|
||||
// (for tilted actuators)
|
||||
_control_allocation_scale(THRUST_Z) = 1.f;
|
||||
|
||||
for (int axis_idx = 2; axis_idx >= 0; --axis_idx) {
|
||||
int num_non_zero_thrust = 0;
|
||||
float norm_sum = 0.f;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
float norm = fabsf(_mix(i, 3 + axis_idx));
|
||||
norm_sum += norm;
|
||||
|
||||
if (norm > FLT_EPSILON) {
|
||||
++num_non_zero_thrust;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_non_zero_thrust > 0) {
|
||||
_control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust;
|
||||
|
||||
} else {
|
||||
_control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
|
||||
{
|
||||
if (_control_allocation_scale(0) > FLT_EPSILON) {
|
||||
_mix.col(0) /= _control_allocation_scale(0);
|
||||
_mix.col(1) /= _control_allocation_scale(1);
|
||||
}
|
||||
|
||||
if (_control_allocation_scale(2) > FLT_EPSILON) {
|
||||
_mix.col(2) /= _control_allocation_scale(2);
|
||||
}
|
||||
|
||||
if (_control_allocation_scale(3) > FLT_EPSILON) {
|
||||
_mix.col(3) /= _control_allocation_scale(3);
|
||||
_mix.col(4) /= _control_allocation_scale(4);
|
||||
_mix.col(5) /= _control_allocation_scale(5);
|
||||
}
|
||||
|
||||
// Set all the small elements to 0 to avoid issues
|
||||
// in the control allocation algorithms
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
for (int j = 0; j < NUM_AXES; j++) {
|
||||
if (fabsf(_mix(i, j)) < 1e-3f) {
|
||||
_mix(i, j) = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::allocate()
|
||||
{
|
||||
//Compute new gains if needed
|
||||
updatePseudoInverse();
|
||||
|
||||
_prev_actuator_sp = _actuator_sp;
|
||||
|
||||
// Allocate
|
||||
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocationPseudoInverse.hpp
|
||||
*
|
||||
* Simple Control Allocation Algorithm
|
||||
*
|
||||
* It computes the pseudo-inverse of the effectiveness matrix
|
||||
* Actuator saturation is handled by simple clipping, do not
|
||||
* expect good performance in case of actuator saturation.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ControlAllocation.hpp"
|
||||
|
||||
class ControlAllocationPseudoInverse: public ControlAllocation
|
||||
{
|
||||
public:
|
||||
ControlAllocationPseudoInverse() = default;
|
||||
virtual ~ControlAllocationPseudoInverse() = 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();
|
||||
|
||||
private:
|
||||
void normalizeControlAllocationMatrix();
|
||||
void updateControlAllocationMatrixScale();
|
||||
bool _normalization_needs_update{false};
|
||||
};
|
||||
-69
@@ -1,69 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 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 ControlAllocationPseudoInverseTest.cpp
|
||||
*
|
||||
* Tests for Control Allocation Algorithms
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ControlAllocationPseudoInverse.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(ControlAllocationTest, 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.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);
|
||||
}
|
||||
-234
@@ -1,234 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocationSequentialDesaturation.cpp
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#include "ControlAllocationSequentialDesaturation.hpp"
|
||||
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::allocate()
|
||||
{
|
||||
//Compute new gains if needed
|
||||
updatePseudoInverse();
|
||||
|
||||
_prev_actuator_sp = _actuator_sp;
|
||||
|
||||
switch (_param_mc_airmode.get()) {
|
||||
case 1:
|
||||
mixAirmodeRP();
|
||||
break;
|
||||
|
||||
case 2:
|
||||
mixAirmodeRPY();
|
||||
break;
|
||||
|
||||
default:
|
||||
mixAirmodeDisabled();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ControlAllocationSequentialDesaturation::desaturateActuators(
|
||||
ActuatorVector &actuator_sp,
|
||||
const ActuatorVector &desaturation_vector, bool increase_only)
|
||||
{
|
||||
float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
|
||||
|
||||
if (increase_only && gain < 0.f) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
actuator_sp(i) += gain * desaturation_vector(i);
|
||||
}
|
||||
|
||||
gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
actuator_sp(i) += gain * desaturation_vector(i);
|
||||
}
|
||||
}
|
||||
|
||||
float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
|
||||
const ActuatorVector &actuator_sp)
|
||||
{
|
||||
float k_min = 0.f;
|
||||
float k_max = 0.f;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
|
||||
if (fabsf(desaturation_vector(i)) < 0.2f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (actuator_sp(i) < _actuator_min(i)) {
|
||||
float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i);
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
}
|
||||
|
||||
if (actuator_sp(i) > _actuator_max(i)) {
|
||||
float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i);
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
}
|
||||
}
|
||||
|
||||
// Reduce the saturation as much as possible
|
||||
return k_min + k_max;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeRP()
|
||||
{
|
||||
// Airmode for roll and pitch, but not yaw
|
||||
|
||||
// Mix without yaw
|
||||
ActuatorVector thrust_z;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
}
|
||||
|
||||
desaturateActuators(_actuator_sp, thrust_z);
|
||||
|
||||
// Mix yaw independently
|
||||
mixYaw();
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeRPY()
|
||||
{
|
||||
// Airmode for roll, pitch and yaw
|
||||
|
||||
// Do full mixing
|
||||
ActuatorVector thrust_z;
|
||||
ActuatorVector yaw;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
yaw(i) = _mix(i, ControlAxis::YAW);
|
||||
}
|
||||
|
||||
desaturateActuators(_actuator_sp, thrust_z);
|
||||
|
||||
// Unsaturate yaw (in case upper and lower bounds are exceeded)
|
||||
// to prioritize roll/pitch over yaw.
|
||||
desaturateActuators(_actuator_sp, yaw);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
|
||||
{
|
||||
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
|
||||
|
||||
// Mix without yaw
|
||||
ActuatorVector thrust_z;
|
||||
ActuatorVector roll;
|
||||
ActuatorVector pitch;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
roll(i) = _mix(i, ControlAxis::ROLL);
|
||||
pitch(i) = _mix(i, ControlAxis::PITCH);
|
||||
}
|
||||
|
||||
// only reduce thrust
|
||||
desaturateActuators(_actuator_sp, thrust_z, true);
|
||||
|
||||
// Reduce roll/pitch acceleration if needed to unsaturate
|
||||
desaturateActuators(_actuator_sp, roll);
|
||||
desaturateActuators(_actuator_sp, pitch);
|
||||
|
||||
// Mix yaw independently
|
||||
mixYaw();
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixYaw()
|
||||
{
|
||||
// Add yaw to outputs
|
||||
ActuatorVector yaw;
|
||||
ActuatorVector thrust_z;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
|
||||
yaw(i) = _mix(i, ControlAxis::YAW);
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
}
|
||||
|
||||
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
|
||||
// and allow some yaw response at maximum thrust
|
||||
ActuatorVector max_prev = _actuator_max;
|
||||
_actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN;
|
||||
desaturateActuators(_actuator_sp, yaw);
|
||||
_actuator_max = max_prev;
|
||||
|
||||
// reduce thrust only
|
||||
desaturateActuators(_actuator_sp, thrust_z, true);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::updateParameters()
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
-132
@@ -1,132 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 ControlAllocationSequentialDesaturation.hpp
|
||||
*
|
||||
* Control Allocation Algorithm which sequentially modifies control demands in order to
|
||||
* eliminate the saturation of the actuator setpoint vector.
|
||||
*
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ControlAllocationPseudoInverse.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams
|
||||
{
|
||||
public:
|
||||
|
||||
ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {}
|
||||
virtual ~ControlAllocationSequentialDesaturation() = default;
|
||||
|
||||
void allocate() override;
|
||||
|
||||
void updateParameters() override;
|
||||
|
||||
// This is the minimum actuator yaw granted when the controller is saturated.
|
||||
// In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount.
|
||||
static constexpr float MINIMUM_YAW_MARGIN{0.15f};
|
||||
private:
|
||||
|
||||
/**
|
||||
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
|
||||
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
|
||||
* acceleration on a specific axis.
|
||||
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
|
||||
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
|
||||
* roll/pitch/yaw accelerations.
|
||||
*
|
||||
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
|
||||
* outside of [min_output, max_output].
|
||||
*
|
||||
* @param actuator_sp Actuator setpoint, vector that is modified
|
||||
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
|
||||
* @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector
|
||||
*/
|
||||
void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector,
|
||||
bool increase_only = false);
|
||||
|
||||
/**
|
||||
* Computes the gain k by which desaturation_vector has to be multiplied
|
||||
* in order to unsaturate the output that has the greatest saturation.
|
||||
*
|
||||
* @return desaturation gain
|
||||
*/
|
||||
float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: airmode for roll/pitch:
|
||||
* thrust is increased/decreased as much as required to meet the demanded roll/pitch.
|
||||
* Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
|
||||
*/
|
||||
void mixAirmodeRP();
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: full airmode for roll/pitch/yaw:
|
||||
* thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
|
||||
* while giving priority to roll and pitch over yaw.
|
||||
*/
|
||||
void mixAirmodeRPY();
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
|
||||
* roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
|
||||
* Thrust can be reduced to unsaturate the upper side.
|
||||
* @see mixYaw() for the exact yaw behavior.
|
||||
*/
|
||||
void mixAirmodeDisabled();
|
||||
|
||||
/**
|
||||
* Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust).
|
||||
*
|
||||
* Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
|
||||
* some yaw control on the upper end. On the lower end thrust will never be increased,
|
||||
* but yaw is decreased as much as required.
|
||||
*/
|
||||
void mixYaw();
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode ///< air-mode
|
||||
);
|
||||
};
|
||||
-385
@@ -1,385 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ControlAllocationSequentialDesaturationTest.cpp
|
||||
*
|
||||
* Tests for Control Allocation Sequential Desaturation Algorithms
|
||||
*
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ControlAllocationSequentialDesaturation.hpp>
|
||||
#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// Makes and returns a Geometry object for a "standard" quad-x quadcopter.
|
||||
ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
|
||||
{
|
||||
ActuatorEffectivenessRotors::Geometry geometry = {};
|
||||
geometry.rotors[0].position(0) = 1.0f;
|
||||
geometry.rotors[0].position(1) = 1.0f;
|
||||
geometry.rotors[0].position(2) = 0.0f;
|
||||
geometry.rotors[0].axis(0) = 0.0f;
|
||||
geometry.rotors[0].axis(1) = 0.0f;
|
||||
geometry.rotors[0].axis(2) = -1.0f;
|
||||
geometry.rotors[0].thrust_coef = 1.0f;
|
||||
geometry.rotors[0].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[1].position(0) = -1.0f;
|
||||
geometry.rotors[1].position(1) = -1.0f;
|
||||
geometry.rotors[1].position(2) = 0.0f;
|
||||
geometry.rotors[1].axis(0) = 0.0f;
|
||||
geometry.rotors[1].axis(1) = 0.0f;
|
||||
geometry.rotors[1].axis(2) = -1.0f;
|
||||
geometry.rotors[1].thrust_coef = 1.0f;
|
||||
geometry.rotors[1].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[2].position(0) = 1.0f;
|
||||
geometry.rotors[2].position(1) = -1.0f;
|
||||
geometry.rotors[2].position(2) = 0.0f;
|
||||
geometry.rotors[2].axis(0) = 0.0f;
|
||||
geometry.rotors[2].axis(1) = 0.0f;
|
||||
geometry.rotors[2].axis(2) = -1.0f;
|
||||
geometry.rotors[2].thrust_coef = 1.0f;
|
||||
geometry.rotors[2].moment_ratio = -0.05f;
|
||||
|
||||
geometry.rotors[3].position(0) = -1.0f;
|
||||
geometry.rotors[3].position(1) = 1.0f;
|
||||
geometry.rotors[3].position(2) = 0.0f;
|
||||
geometry.rotors[3].axis(0) = 0.0f;
|
||||
geometry.rotors[3].axis(1) = 0.0f;
|
||||
geometry.rotors[3].axis(2) = -1.0f;
|
||||
geometry.rotors[3].thrust_coef = 1.0f;
|
||||
geometry.rotors[3].moment_ratio = -0.05f;
|
||||
|
||||
geometry.num_rotors = 4;
|
||||
|
||||
return geometry;
|
||||
}
|
||||
|
||||
// Returns an effective matrix for a sample quad-copter configuration.
|
||||
ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
|
||||
{
|
||||
ActuatorEffectiveness::EffectivenessMatrix effectiveness;
|
||||
effectiveness.setZero();
|
||||
const auto geometry = make_quad_x_geometry();
|
||||
ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
|
||||
return effectiveness;
|
||||
}
|
||||
|
||||
// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter.
|
||||
void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator)
|
||||
{
|
||||
const auto effectiveness = make_quad_x_effectiveness();
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> actuator_trim;
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> linearization_point;
|
||||
constexpr bool UPDATE_NORMALIZATION_SCALE{false};
|
||||
allocator.setEffectivenessMatrix(
|
||||
effectiveness,
|
||||
actuator_trim,
|
||||
linearization_point,
|
||||
ActuatorEffectiveness::NUM_ACTUATORS,
|
||||
UPDATE_NORMALIZATION_SCALE
|
||||
);
|
||||
}
|
||||
|
||||
static constexpr float EXPECT_NEAR_TOL{1e-4f};
|
||||
|
||||
} // namespace
|
||||
|
||||
// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator
|
||||
// allocation.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 1.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> zero;
|
||||
EXPECT_EQ(actuator_sp, zero);
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust returns the desired actuator setpoint.
|
||||
// Each motor should have an actuator setpoint that when summed together should be equal to
|
||||
// control setpoint.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT};
|
||||
|
||||
for (int i{0}; i < MOTOR_COUNT; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test does not saturate the yaw response.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is low enough to not saturate the motors.
|
||||
constexpr float YAW_CONTROL_SP{0.02f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
|
||||
// this will need to be changed.
|
||||
constexpr float YAW_EFFECTIVENESS_FACTOR{5.f};
|
||||
constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR};
|
||||
// At yaw condition, there will be 2 different actuator values.
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR};
|
||||
|
||||
for (int i{0}; i < MOTOR_COUNT / 2; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test saturates the yaw response, but does not reduce total thrust.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
|
||||
constexpr float YAW_CONTROL_SP{0.25f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// At max yaw, only 2 motors will carry all of the thrust.
|
||||
constexpr int YAW_MOTORS{2};
|
||||
constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS};
|
||||
|
||||
for (int i{0}; i < YAW_MOTORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
|
||||
// This test does not saturate the pitch response.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is low enough to not saturate the motors.
|
||||
constexpr float PITCH_CONTROL_SP{0.1f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
|
||||
// this will need to be changed.
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT};
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test saturates yaw and demonstrates reduction of thrust for yaw.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f};
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT};
|
||||
// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
|
||||
constexpr float YAW_CONTROL_SP{1.f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// In the case of yaw saturation, thrust per motor will be reduced by the hard-coded
|
||||
// magic-number yaw margin of 0.15f.
|
||||
constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available.
|
||||
constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR};
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
|
||||
// This test saturates the pitch response such that thrust is reduced to (partially) compensate.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f * 4.f};
|
||||
// This is high enough to saturate the pitch control.
|
||||
constexpr float PITCH_CONTROL_SP{2.f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
// The maximum actuator value is
|
||||
// THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT.
|
||||
// The amount over 1 is the amount that each motor is reduced by.
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1};
|
||||
EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f);
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness
|
||||
+1
-1
@@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness
|
||||
{
|
||||
+1
-1
@@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
+1
-1
@@ -39,7 +39,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
+1
-1
@@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
+1
-1
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness
|
||||
@@ -0,0 +1,43 @@
|
||||
px4_add_library(VehicleActuatorEffectiveness
|
||||
ActuatorEffectivenessUUV.cpp
|
||||
ActuatorEffectivenessUUV.hpp
|
||||
ActuatorEffectivenessControlSurfaces.cpp
|
||||
ActuatorEffectivenessControlSurfaces.hpp
|
||||
ActuatorEffectivenessCustom.cpp
|
||||
ActuatorEffectivenessCustom.hpp
|
||||
ActuatorEffectivenessFixedWing.cpp
|
||||
ActuatorEffectivenessFixedWing.hpp
|
||||
ActuatorEffectivenessHelicopter.cpp
|
||||
ActuatorEffectivenessHelicopter.hpp
|
||||
ActuatorEffectivenessHelicopterCoaxial.cpp
|
||||
ActuatorEffectivenessHelicopterCoaxial.hpp
|
||||
ActuatorEffectivenessMCTilt.cpp
|
||||
ActuatorEffectivenessMCTilt.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
ActuatorEffectivenessMultirotor.hpp
|
||||
ActuatorEffectivenessTilts.cpp
|
||||
ActuatorEffectivenessTilts.hpp
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
ActuatorEffectivenessRotors.hpp
|
||||
ActuatorEffectivenessStandardVTOL.cpp
|
||||
ActuatorEffectivenessStandardVTOL.hpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.cpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.hpp
|
||||
ActuatorEffectivenessTailsitterVTOL.cpp
|
||||
ActuatorEffectivenessTailsitterVTOL.hpp
|
||||
ActuatorEffectivenessRoverAckermann.hpp
|
||||
ActuatorEffectivenessRoverAckermann.cpp
|
||||
RpmControl.hpp
|
||||
RpmControl.cpp
|
||||
)
|
||||
|
||||
target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(VehicleActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
ActuatorEffectiveness
|
||||
)
|
||||
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness)
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness)
|
||||
Reference in New Issue
Block a user