Skip to content

Commit

Permalink
Make control allocation and actuator effectiveness a non-module-speci…
Browse files Browse the repository at this point in the history
…fic 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
  • Loading branch information
Jaeyoung-Lim authored Jan 15, 2025
1 parent 3064a4a commit 974446c
Show file tree
Hide file tree
Showing 48 changed files with 98 additions and 52 deletions.
1 change: 1 addition & 0 deletions src/lib/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL)
add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL)
add_subdirectory(collision_prevention EXCLUDE_FROM_ALL)
add_subdirectory(component_information EXCLUDE_FROM_ALL)
add_subdirectory(control_allocation EXCLUDE_FROM_ALL)
add_subdirectory(controllib EXCLUDE_FROM_ALL)
add_subdirectory(conversion EXCLUDE_FROM_ALL)
add_subdirectory(crc EXCLUDE_FROM_ALL)
Expand Down
35 changes: 35 additions & 0 deletions src/lib/control_allocation/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
############################################################################
#
# Copyright (c) 2025 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.
#
############################################################################

add_subdirectory(control_allocation)
add_subdirectory(actuator_effectiveness)
Original file line number Diff line number Diff line change
Expand Up @@ -34,36 +34,6 @@
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})
Expand All @@ -73,6 +43,3 @@ target_link_libraries(ActuatorEffectiveness
mathlib
PID
)

px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,4 @@ 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)
# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@

#include <matrix/matrix/math.hpp>

#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"

class ControlAllocation
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@

#include <gtest/gtest.h>
#include <ControlAllocationSequentialDesaturation.hpp>
#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp>
#include <actuator_effectiveness/ActuatorEffectivenessRotors.hpp>

using namespace matrix;

Expand Down
4 changes: 2 additions & 2 deletions src/modules/control_allocator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -50,6 +49,7 @@ px4_add_module(
DEPENDS
mathlib
ActuatorEffectiveness
VehicleActuatorEffectiveness
ControlAllocation
px4_work_queue
SlewRate
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessControlSurfaces.hpp"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessControlSurfaces.hpp"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"

#include <px4_platform_common/module_params.h>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"

#include <px4_platform_common/module_params.h>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessTilts.hpp"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"

class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessControlSurfaces.hpp"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

#pragma once

#include "ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
#include "ActuatorEffectivenessRotors.hpp"
#include "ActuatorEffectivenessControlSurfaces.hpp"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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)

0 comments on commit 974446c

Please sign in to comment.