fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 11:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: add parameter based ActuatorEffectivenessCustom

Browse Source 
- this allows creating a custom actuator effectiveness entirely from
parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}
This commit is contained in:
Daniel Agar
2021-10-27 14:23:59 -04:00
parent 43d7d83a4b
commit a88c6434b6
10 changed files with 278 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
+85
View File
@@ -0,0 +1,85 @@
/****************************************************************************
*
* 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 "ActuatorEffectivenessCustom.hpp"
ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
ModuleParams(parent)
{
}
bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
if (_updated || force) {
_updated = false;
int num_actuators = 0;
for (int n = 0; n < NUM_ACTUATORS; n++) {
// CA_ACTn_TRQ_R
// CA_ACTn_TRQ_P
// CA_ACTn_TRQ_Y
char torque_str[3][17];
sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
// CA_ACTn_THR_X
// CA_ACTn_THR_Y
// CA_ACTn_THR_Z
char thrust_str[3][17];
sprintf(thrust_str[0], "CA_ACT%u_THR_X", n);
sprintf(thrust_str[1], "CA_ACT%u_THR_Y", n);
sprintf(thrust_str[2], "CA_ACT%u_THR_Z", n);
for (int i = 0; i < 3; i++) {
// CA_ACTn_TRQ_{R,P,Y}
param_get(param_find(torque_str[i]), &_matrix(n, i));
// CA_ACTn_THR_{X,Y,Z}
param_get(param_find(thrust_str[i]), &_matrix(n, i + 3));
}
if (_matrix.row(n).longerThan(0.f)) {
num_actuators++;
}
}
_num_actuators = num_actuators;
matrix = _matrix;
return true;
}
return false;
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ActuatorEffectivenessCustom.hpp
*
* Actuator effectiveness computed from rotors position and orientation
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "ActuatorEffectiveness.hpp"
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
using namespace time_literals;
class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
{
public:
ActuatorEffectivenessCustom(ModuleParams *parent);
virtual ~ActuatorEffectivenessCustom() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
int numActuators() const override { return _num_actuators; }
private:
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _matrix{};
int _num_actuators{0};
bool _updated{true};
};
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
-52
View File
@@ -1,52 +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.hpp
ActuatorEffectivenessMultirotor.cpp
ActuatorEffectivenessMultirotor.hpp
ActuatorEffectivenessStandardVTOL.cpp
ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectivenessTiltrotorVTOL.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
ControlAllocation
)
# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
src/modules/control_allocator/CMakeLists.txt
+30 -7
View File
@@ -31,22 +31,45 @@
#
############################################################################
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(ActuatorEffectiveness)
add_subdirectory(ControlAllocation)
px4_add_module(
MODULE modules__control_allocator
MAIN control_allocator
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
INCLUDES
${CMAKE_CURRENT_SOURCE_DIR}
SRCS
ActuatorEffectiveness/ActuatorEffectiveness.hpp
ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
ControlAllocation/ControlAllocation.cpp
ControlAllocation/ControlAllocation.hpp
ControlAllocation/ControlAllocationPseudoInverse.cpp
ControlAllocation/ControlAllocationPseudoInverse.hpp
ControlAllocation/ControlAllocationSequentialDesaturation.cpp
ControlAllocation/ControlAllocationSequentialDesaturation.hpp
ControlAllocator.cpp
ControlAllocator.hpp
DEPENDS
mathlib
ActuatorEffectiveness
ControlAllocation
mixer
px4_work_queue
MODULE_CONFIG
module.yaml
)
px4_add_library(ControlAllocation_testing
ControlAllocation/ControlAllocation.cpp
ControlAllocation/ControlAllocationPseudoInverse.cpp
)
target_link_libraries(ControlAllocation_testing PRIVATE mathlib)
px4_add_unit_gtest(SRC ControlAllocation/ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation_testing)
# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
src/modules/control_allocator/ControlAllocation/CMakeLists.txt
-46
View File
@@ -1,46 +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)
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp
+2 -1
View File
@@ -40,7 +40,8 @@
*/
#include <gtest/gtest.h>
#include <ControlAllocationPseudoInverse.hpp>
#include "ControlAllocationPseudoInverse.hpp"
using namespace matrix;
src/modules/control_allocator/ControlAllocator.cpp
+8
View File
@@ -212,6 +212,10 @@ ControlAllocator::update_effectiveness_source()
tmp = new ActuatorEffectivenessTiltrotorVTOL();
break;
case EffectivenessSource::CUSTOM:
tmp = new ActuatorEffectivenessCustom(this);
break;
default:
PX4_ERR("Unknown airframe");
break;
@@ -484,6 +488,10 @@ int ControlAllocator::print_status()
case EffectivenessSource::TILTROTOR_VTOL:
PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
break;
case EffectivenessSource::CUSTOM:
PX4_INFO("EffectivenessSource: Custom");
break;
}
// Print current effectiveness matrix
src/modules/control_allocator/ControlAllocator.hpp
+12 -10
View File
@@ -41,14 +41,15 @@
#pragma once
#include <ActuatorEffectiveness.hpp>
#include <ActuatorEffectivenessMultirotor.hpp>
#include <ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>
#include <ControlAllocation.hpp>
#include <ControlAllocationPseudoInverse.hpp>
#include <ControlAllocationSequentialDesaturation.hpp>
#include <ControlAllocation/ControlAllocation.hpp>
#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
@@ -123,10 +124,11 @@ private:
ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations
enum class EffectivenessSource {
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
TILTROTOR_VTOL = 2,
CUSTOM = 3,
};
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
src/modules/control_allocator/control_allocator_params.c
+1
View File
@@ -50,6 +50,7 @@
* @value 0 Multirotor
* @value 1 Standard VTOL (WIP)
* @value 2 Tiltrotor VTOL (WIP)
* @value 3 Custom (CA_ACTn* torque and thrust parameters)
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
src/modules/control_allocator/module.yaml
+72
View File
@@ -0,0 +1,72 @@
__max_num_config_instances: &max_num_config_instances 16
module_name: control_allocator
parameters:
- group: Control Allocator
definitions:
CA_ACT${i}_TRQ_R:
description:
short: actuator ${i} roll
long: Actuator ${i} roll torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_TRQ_P:
description:
short: actuator ${i} pitch
long: Actuator ${i} pitch torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_TRQ_Y:
description:
short: actuator ${i} yaw
long: Actuator ${i} yaw torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_X:
description:
short: actuator ${i} thrust x
long: Actuator ${i} thrust x
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_Y:
description:
short: actuator ${i} thrust y
long: Actuator ${i} thrust y
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_Z:
description:
short: actuator ${i} thrust y
long: Actuator ${i} thrust y
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0