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

Spacecraft build and bare control allocator (#24221)

Browse Source
This commit is contained in:
Pedro Roque
2025-02-07 05:54:24 +01:00
committed by GitHub
parent de1ade8eb8
commit e7e76e2e21
21 changed files with 1037 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+8
View File
@@ -270,6 +270,14 @@ ControlAllocator::update_effectiveness_source()
tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
break;
case EffectivenessSource::SPACECRAFT_2D:
// spacecraft_allocation does allocation and publishes directly to actuator_motors topic
break;
case EffectivenessSource::SPACECRAFT_3D:
// spacecraft_allocation does allocation and publishes directly to actuator_motors topic
break;
default:
PX4_ERR("Unknown airframe");
break;
src/modules/control_allocator/ControlAllocator.hpp
+2
View File
@@ -158,6 +158,8 @@ private:
HELICOPTER_TAIL_ESC = 10,
HELICOPTER_TAIL_SERVO = 11,
HELICOPTER_COAXIAL = 12,
SPACECRAFT_2D = 13,
SPACECRAFT_3D = 14,
};
enum class FailureMode {
src/modules/control_allocator/module.yaml
+2
View File
@@ -31,6 +31,8 @@ parameters:
11: Helicopter (tail Servo)
12: Helicopter (Coaxial)
13: Rover (Mecanum)
14: Spacecraft 2D
15: Spacecraft 3D
default: 0
CA_METHOD:
src/modules/spacecraft/CMakeLists.txt
+51
View File
@@ -0,0 +1,51 @@
############################################################################
#
# 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.
#
############################################################################
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
px4_add_module(
MODULE modules__spacecraft
MAIN spacecraft
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
STACK_MAIN
3000
SRCS
SpacecraftHandler.cpp
SpacecraftHandler.hpp
MODULE_CONFIG
module.yaml
DEPENDS
mathlib
px4_work_queue
)
src/modules/spacecraft/Kconfig
+12
View File
@@ -0,0 +1,12 @@
menuconfig MODULES_SPACECRAFT
bool "SPACECRAFT"
default n
---help---
Enable support for spacecraft
menuconfig USER_SPACECRAFT
bool "spacecraft running as userspace module"
default y
depends on BOARD_PROTECTED && MODULES_SPACECRAFT
---help---
Put SPACECRAFT in userspace memory
src/modules/spacecraft/SpacecraftHandler.cpp
+91
View File
@@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2013-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 SpacecraftHandler.cpp
*
* Control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "SpacecraftHandler.hpp"
int SpacecraftHandler::task_spawn(int argc, char *argv[])
{
return 0;
}
int SpacecraftHandler::print_status()
{
PX4_INFO("Running");
return 0;
}
int SpacecraftHandler::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int SpacecraftHandler::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements control allocation for spacecraft vehicles.
It takes torque and thrust setpoints as inputs and outputs
actuator setpoint messages.
)DESCR_STR"
);
PRINT_MODULE_USAGE_NAME("spacecraft", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
/**
* Control Allocator app start / stop handling function
*/
extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]);
int spacecraft_main(int argc, char *argv[])
{
return SpacecraftHandler::main(argc, argv);
}
src/modules/spacecraft/SpacecraftHandler.hpp
+87
View File
@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2013-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 ControlAllocator.hpp
*
* Control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/actuator_servos_trim.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/failure_detector_status.h>
class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
SpacecraftHandler();
virtual ~SpacecraftHandler();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
private: /**< loop duration performance counter */
};
src/modules/spacecraft/module.yaml
+235
View File
@@ -0,0 +1,235 @@
__max_num_mc_motors: &max_num_mc_motors 12
__max_num_thrusters: &max_num_thrusters 12
__max_num_servos: &max_num_servos 8
__max_num_tilts: &max_num_tilts 4
module_name: Control Allocation
parameters:
- group: Geometry
definitions:
CA_AIRFRAME:
description:
short: Airframe selection
long: |
Defines which mixer implementation to use.
Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.
'Custom' should only be used if noting else can be used.
type: enum
values:
0: Multirotor
1: Fixed-wing
2: Standard VTOL
3: Tiltrotor VTOL
4: Tailsitter VTOL
5: Rover (Ackermann)
6: Rover (Differential)
7: Motors (6DOF)
8: Multirotor with Tilt
9: Custom
10: Helicopter (tail ESC)
11: Helicopter (tail Servo)
12: Helicopter (Coaxial)
13: Rover (Mecanum)
14: Spacecraft 2D
15: Spacecraft 3D
default: 14
CA_METHOD:
description:
short: Control allocation method
long: |
Selects the algorithm and desaturation method.
If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
type: enum
values:
0: Pseudo-inverse with output clipping
1: Pseudo-inverse with metric allocation
2: Automatic
default: 1
CA_R_REV:
description:
short: Bidirectional/Reversible motors
long: |
Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well.
type: bitmask
bit:
0: Motor 1
1: Motor 2
2: Motor 3
3: Motor 4
4: Motor 5
5: Motor 6
6: Motor 7
7: Motor 8
8: Motor 9
9: Motor 10
10: Motor 11
11: Motor 12
default: 0
# (SC) Thrusters
CA_THRUSTER_CNT:
description:
short: Total number of thrusters
type: enum
values:
0: '0'
1: '1'
2: '2'
3: '3'
4: '4'
5: '5'
6: '6'
7: '7'
8: '8'
9: '9'
10: '10'
11: '11'
12: '12'
default: 0
CA_THRUSTER${i}_PX:
description:
short: Position of thruster ${i} along X body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_thrusters
min: -100
max: 100
default: 0.0
CA_THRUSTER${i}_PY:
description:
short: Position of thruster ${i} along Y body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_thrusters
min: -100
max: 100
default: 0.0
CA_THRUSTER${i}_PZ:
description:
short: Position of thruster ${i} along Z body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_thrusters
min: -100
max: 100
default: 0.0
CA_THRUSTER${i}_AX:
description:
short: Axis of thruster ${i} thrust vector, X body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_thrusters
min: -100
max: 100
default: 0.0
CA_THRUSTER${i}_AY:
description:
short: Axis of thruster ${i} thrust vector, Y body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_thrusters
min: -100
max: 100
default: 0.0
CA_THRUSTER${i}_AZ:
description:
short: Axis of thruster ${i} thrust vector, Z body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_thrusters
min: -100
max: 100
default: -1.0
CA_THRUSTER${i}_CT:
description:
short: Thrust coefficient of rotor ${i}
long: |
The thrust coefficient if defined as Thrust = CT * u^2,
where u (with value between actuator minimum and maximum)
is the output signal sent to the motor controller.
type: float
decimal: 1
increment: 1
num_instances: *max_num_thrusters
min: 0
max: 100
default: 6.5
# Mixer
mixer:
actuator_types:
motor:
functions: 'Motor'
actuator_testing_values:
min: 0
max: 1
default_is_nan: true
DEFAULT:
actuator_testing_values:
min: -1
max: 1
default: -1
config:
param: CA_AIRFRAME
types:
14: # Spacecraft 2D
title: 'Spacecraft 2D'
actuators:
- actuator_type: 'motor'
count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size
per_item_parameters:
standard:
position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ]
extra:
- name: 'CA_THRUSTER${i}_AX'
label: 'Axis X'
function: 'axisx'
advanced: true
- name: 'CA_THRUSTER${i}_AY'
label: 'Axis Y'
function: 'axisy'
advanced: true
- name: 'CA_THRUSTER${i}_AZ'
label: 'Axis Z'
function: 'axisz'
advanced: true
15: # Sapcecraft 3D
title: 'Spacecraft 3D'
actuators:
- actuator_type: 'motor'
count: 'CA_THRUSTER_CNT'
per_item_parameters:
standard:
position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ]
extra:
- name: 'CA_THRUSTER${i}_AX'
label: 'Axis X'
function: 'axisx'
advanced: true
- name: 'CA_THRUSTER${i}_AY'
label: 'Axis Y'
function: 'axisy'
advanced: true
- name: 'CA_THRUSTER${i}_AZ'
label: 'Axis Z'
function: 'axisz'
advanced: true