fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
21 changed files with 1037 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.vscode/settings.json vendored
View File

@ -129,5 +129,4 @@
"yaml.schemas": {
"${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
},
"ros.distro": "humble"
}

155
ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart Normal file
View File

@ -0,0 +1,155 @@
#!/bin/sh
#
# @name 6DoF Spacecraft Model
#
# @type Freeflyer with 8 thrusters
#
# @maintainer Pedro Roque <padr@kth.se>
#
. ${R}etc/init.d/rc.sc_defaults
param set-default CA_AIRFRAME 15
param set-default MAV_TYPE 99
param set-default CA_THRUSTER_CNT 12
param set-default CA_R_REV 0
# param set-default FW_ARSP_MODE 1
# Auto to be provided by Custom Airframe
param set-default CA_METHOD 0
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
# Set proper failsafes
param set-default COM_ACT_FAIL_ACT 0
param set-default COM_LOW_BAT_ACT 0
param set-default NAV_DLL_ACT 0
param set-default GF_ACTION 1
param set-default NAV_RCL_ACT 1
param set-default COM_POSCTL_NAVL 2
# Set thrusters
param set-default CA_THRUSTER0_PX -0.50
param set-default CA_THRUSTER0_PY 0.50
param set-default CA_THRUSTER0_PZ 0.0
param set-default CA_THRUSTER0_CT 0.237
param set-default CA_THRUSTER0_AX 0.0
param set-default CA_THRUSTER0_AY -1.0
param set-default CA_THRUSTER0_AZ 0.0
param set-default CA_THRUSTER1_PX 0.50
param set-default CA_THRUSTER1_PY 0.50
param set-default CA_THRUSTER1_PZ 0.0
param set-default CA_THRUSTER1_CT 0.237
param set-default CA_THRUSTER1_AX 0.0
param set-default CA_THRUSTER1_AY -1.0
param set-default CA_THRUSTER1_AZ 0.0
param set-default CA_THRUSTER2_PX 0.50
param set-default CA_THRUSTER2_PY -0.50
param set-default CA_THRUSTER2_PZ 0.0
param set-default CA_THRUSTER2_CT 0.237
param set-default CA_THRUSTER2_AX 0.0
param set-default CA_THRUSTER2_AY 1.0
param set-default CA_THRUSTER2_AZ 0.0
param set-default CA_THRUSTER3_PX -0.50
param set-default CA_THRUSTER3_PY -0.50
param set-default CA_THRUSTER3_PZ 0.0
param set-default CA_THRUSTER3_CT 0.237
param set-default CA_THRUSTER3_AX 0.0
param set-default CA_THRUSTER3_AY 1.0
param set-default CA_THRUSTER3_AZ 0.0
param set-default CA_THRUSTER4_PX -0.50
param set-default CA_THRUSTER4_PY 0.0
param set-default CA_THRUSTER4_PZ -0.50
param set-default CA_THRUSTER4_CT 0.237
param set-default CA_THRUSTER4_AX 1.0
param set-default CA_THRUSTER4_AY 0.0
param set-default CA_THRUSTER4_AZ 0.0
param set-default CA_THRUSTER5_PX 0.50
param set-default CA_THRUSTER5_PY 0.0
param set-default CA_THRUSTER5_PZ -0.50
param set-default CA_THRUSTER5_CT 0.237
param set-default CA_THRUSTER5_AX -1.0
param set-default CA_THRUSTER5_AY 0.0
param set-default CA_THRUSTER5_AZ 0.0
param set-default CA_THRUSTER6_PX 0.50
param set-default CA_THRUSTER6_PY 0.0
param set-default CA_THRUSTER6_PZ 0.50
param set-default CA_THRUSTER6_CT 0.237
param set-default CA_THRUSTER6_AX -1.0
param set-default CA_THRUSTER6_AY 0.0
param set-default CA_THRUSTER6_AZ 0.0
param set-default CA_THRUSTER7_PX -0.50
param set-default CA_THRUSTER7_PY 0.0
param set-default CA_THRUSTER7_PZ 0.50
param set-default CA_THRUSTER7_CT 0.237
param set-default CA_THRUSTER7_AX 1.0
param set-default CA_THRUSTER7_AY 0.0
param set-default CA_THRUSTER7_AZ 0.0
param set-default CA_THRUSTER8_PX 0.0
param set-default CA_THRUSTER8_PY -0.50
param set-default CA_THRUSTER8_PZ -0.50
param set-default CA_THRUSTER8_CT 0.237
param set-default CA_THRUSTER8_AX 0.0
param set-default CA_THRUSTER8_AY 0.0
param set-default CA_THRUSTER8_AZ 1.0
param set-default CA_THRUSTER9_PX 0.0
param set-default CA_THRUSTER9_PY 0.50
param set-default CA_THRUSTER9_PZ -0.50
param set-default CA_THRUSTER9_CT 0.237
param set-default CA_THRUSTER9_AX 0.0
param set-default CA_THRUSTER9_AY 0.0
param set-default CA_THRUSTER9_AZ 1.0
param set-default CA_THRUSTER10_PX 0.0
param set-default CA_THRUSTER10_PY 0.50
param set-default CA_THRUSTER10_PZ 0.50
param set-default CA_THRUSTER10_CT 0.237
param set-default CA_THRUSTER10_AX 0.0
param set-default CA_THRUSTER10_AY 0.0
param set-default CA_THRUSTER10_AZ -1.0
param set-default CA_THRUSTER11_PX 0.0
param set-default CA_THRUSTER11_PY -0.50
param set-default CA_THRUSTER11_PZ 0.50
param set-default CA_THRUSTER11_CT 0.237
param set-default CA_THRUSTER11_AX 0.0
param set-default CA_THRUSTER11_AY 0.0
param set-default CA_THRUSTER11_AZ -1.0
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default PWM_MAIN_FUNC5 105
param set-default PWM_MAIN_FUNC6 106
param set-default PWM_MAIN_FUNC7 107
param set-default PWM_MAIN_FUNC8 108
param set-default PWM_MAIN_FUNC9 109
param set-default PWM_MAIN_FUNC10 110
param set-default PWM_MAIN_FUNC11 111
param set-default PWM_MAIN_FUNC12 112
# PWM Simulation
param set PWM_SIM_PWM_MAX 10000
param set PWM_SIM_PWM_MIN 0
# Controller Tunings
param set-default SC_ROLLRATE_P 0.14
param set-default SC_PITCHRATE_P 0.14
param set-default SC_ROLLRATE_I 0.3
param set-default SC_PITCHRATE_I 0.3
param set-default SC_ROLLRATE_D 0.004
param set-default SC_PITCHRATE_D 0.004

149
ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d Normal file
View File

@ -0,0 +1,149 @@
#!/bin/sh
#
# @name 3DoF Spacecraft Model
#
# @type 2D Freeflyer with 8 thrusters - Planar motion
#
# @maintainer Pedro Roque <padr@kth.se>
#
. ${R}etc/init.d/rc.sc_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d}
param set-default SIM_GZ_EN 1
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs
param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
param set-default CA_AIRFRAME 14
param set-default MAV_TYPE 99
param set-default CA_THRUSTER_CNT 8
param set-default CA_R_REV 0
# param set-default FW_ARSP_MODE 1
# Auto to be provided by Custom Airframe
param set-default CA_METHOD 0 # 0 is PseudoInverse, 3 is Metric
# Set proper failsafes
param set-default COM_ACT_FAIL_ACT 0
param set-default COM_LOW_BAT_ACT 0
param set-default NAV_DLL_ACT 0
param set-default GF_ACTION 1
param set-default NAV_RCL_ACT 1
param set-default COM_POSCTL_NAVL 2
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default CA_THRUSTER0_PX -0.12
param set-default CA_THRUSTER0_PY -0.12
param set-default CA_THRUSTER0_PZ 0.0
param set-default CA_THRUSTER0_CT 1.4
param set-default CA_THRUSTER0_AX 1.0
param set-default CA_THRUSTER0_AY 0.0
param set-default CA_THRUSTER0_AZ 0.0
param set-default CA_THRUSTER1_PX 0.12
param set-default CA_THRUSTER1_PY -0.12
param set-default CA_THRUSTER1_PZ 0.0
param set-default CA_THRUSTER1_CT 1.4
param set-default CA_THRUSTER1_AX -1.0
param set-default CA_THRUSTER1_AY 0.0
param set-default CA_THRUSTER1_AZ 0.0
param set-default CA_THRUSTER2_PX -0.12
param set-default CA_THRUSTER2_PY 0.12
param set-default CA_THRUSTER2_PZ 0.0
param set-default CA_THRUSTER2_CT 1.4
param set-default CA_THRUSTER2_AX 1.0
param set-default CA_THRUSTER2_AY 0.0
param set-default CA_THRUSTER2_AZ 0.0
param set-default CA_THRUSTER3_PX 0.12
param set-default CA_THRUSTER3_PY 0.12
param set-default CA_THRUSTER3_PZ 0.0
param set-default CA_THRUSTER3_CT 1.4
param set-default CA_THRUSTER3_AX -1.0
param set-default CA_THRUSTER3_AY 0.0
param set-default CA_THRUSTER3_AZ 0.0
param set-default CA_THRUSTER4_PX 0.12
param set-default CA_THRUSTER4_PY -0.12
param set-default CA_THRUSTER4_PZ 0.0
param set-default CA_THRUSTER4_CT 1.4
param set-default CA_THRUSTER4_AX 0.0
param set-default CA_THRUSTER4_AY 1.0
param set-default CA_THRUSTER4_AZ 0.0
param set-default CA_THRUSTER5_PX 0.12
param set-default CA_THRUSTER5_PY 0.12
param set-default CA_THRUSTER5_PZ 0.0
param set-default CA_THRUSTER5_CT 1.4
param set-default CA_THRUSTER5_AX 0.0
param set-default CA_THRUSTER5_AY -1.0
param set-default CA_THRUSTER5_AZ 0.0
param set-default CA_THRUSTER6_PX -0.12
param set-default CA_THRUSTER6_PY -0.12
param set-default CA_THRUSTER6_PZ 0.0
param set-default CA_THRUSTER6_CT 1.4
param set-default CA_THRUSTER6_AX 0.0
param set-default CA_THRUSTER6_AY 1.0
param set-default CA_THRUSTER6_AZ 0.0
param set-default CA_THRUSTER7_PX -0.12
param set-default CA_THRUSTER7_PY 0.12
param set-default CA_THRUSTER7_PZ 0.0
param set-default CA_THRUSTER7_CT 1.4
param set-default CA_THRUSTER7_AX 0.0
param set-default CA_THRUSTER7_AY -1.0
param set-default CA_THRUSTER7_AZ 0.0
param set-default SIM_GZ_TH_FUNC1 101
param set-default SIM_GZ_TH_FUNC2 102
param set-default SIM_GZ_TH_FUNC3 103
param set-default SIM_GZ_TH_FUNC4 104
param set-default SIM_GZ_TH_FUNC5 105
param set-default SIM_GZ_TH_FUNC6 106
param set-default SIM_GZ_TH_FUNC7 107
param set-default SIM_GZ_TH_FUNC8 108
param set-default SIM_GZ_TH_MIN1 0
param set-default SIM_GZ_TH_MIN2 0
param set-default SIM_GZ_TH_MIN3 0
param set-default SIM_GZ_TH_MIN4 0
param set-default SIM_GZ_TH_MIN5 0
param set-default SIM_GZ_TH_MIN6 0
param set-default SIM_GZ_TH_MIN7 0
param set-default SIM_GZ_TH_MIN8 0
param set-default SIM_GZ_TH_MAX1 10000
param set-default SIM_GZ_TH_MAX2 10000
param set-default SIM_GZ_TH_MAX3 10000
param set-default SIM_GZ_TH_MAX4 10000
param set-default SIM_GZ_TH_MAX5 10000
param set-default SIM_GZ_TH_MAX6 10000
param set-default SIM_GZ_TH_MAX7 10000
param set-default SIM_GZ_TH_MAX8 10000
# Controller Tunings
param set SC_YAWRATE_P 3.335
param set SC_YAWRATE_I 0.87
param set SC_YAWRATE_D 0.15
param set SC_YR_INT_LIM 0.2
param set SC_YAW_P 3.0
param set SPC_POS_P 0.20
param set SPC_VEL_P 6.55
param set SPC_VEL_I 0.0
param set SPC_VEL_D 0.0
param set SPC_VEL_MAX 12.0

3
ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
View File

@ -114,5 +114,8 @@ px4_add_romfs_files(
17001_flightgear_tf-g1
17002_flightgear_tf-g2
71001_gazebo-classic_spacecraft_dart
71002_gz_spacecraft_2d
# [22000, 22999] Reserve for custom models
)

7
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
)
endif()
if(CONFIG_MODULES_SPACECRAFT)
px4_add_romfs_files(
rc.sc_apps
rc.sc_defaults
)
endif()
if(CONFIG_MODULES_ROVER_MECANUM)
px4_add_romfs_files(
rc.rover_mecanum_apps

150
ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d Normal file
View File

@ -0,0 +1,150 @@
#!/bin/sh
#
# @name KTH Space Robot
#
# @type Space Robot
# @class 2D Space Robot
#
# @maintainer DISCOWER
#
. ${R}etc/init.d/rc.sc_defaults
param set-default CA_AIRFRAME 14
param set-default MAV_TYPE 99
param set-default CA_THRUSTER_CNT 8
param set-default CA_R_REV 0
# Auto to be provided by Custom Airframe
param set-default CA_METHOD 0
# Set proper failsafes
param set-default COM_ACT_FAIL_ACT 0
param set-default COM_LOW_BAT_ACT 0
param set-default NAV_DLL_ACT 0
param set-default GF_ACTION 1
param set-default NAV_RCL_ACT 1
param set-default COM_POSCTL_NAVL 2
# Set Mocap Vision frame
param set EKF2_EV_CTRL 15
param set EKF2_HGT_REF 3
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default CA_THRUSTER0_PX -0.12
param set-default CA_THRUSTER0_PY -0.12
param set-default CA_THRUSTER0_PZ 0.0
param set-default CA_THRUSTER0_CT 1.4
param set-default CA_THRUSTER0_AX 1.0
param set-default CA_THRUSTER0_AY 0.0
param set-default CA_THRUSTER0_AZ 0.0
param set-default CA_THRUSTER1_PX 0.12
param set-default CA_THRUSTER1_PY -0.12
param set-default CA_THRUSTER1_PZ 0.0
param set-default CA_THRUSTER1_CT 1.4
param set-default CA_THRUSTER1_AX -1.0
param set-default CA_THRUSTER1_AY 0.0
param set-default CA_THRUSTER1_AZ 0.0
param set-default CA_THRUSTER2_PX -0.12
param set-default CA_THRUSTER2_PY 0.12
param set-default CA_THRUSTER2_PZ 0.0
param set-default CA_THRUSTER2_CT 1.4
param set-default CA_THRUSTER2_AX 1.0
param set-default CA_THRUSTER2_AY 0.0
param set-default CA_THRUSTER2_AZ 0.0
param set-default CA_THRUSTER3_PX 0.12
param set-default CA_THRUSTER3_PY 0.12
param set-default CA_THRUSTER3_PZ 0.0
param set-default CA_THRUSTER3_CT 1.4
param set-default CA_THRUSTER3_AX -1.0
param set-default CA_THRUSTER3_AY 0.0
param set-default CA_THRUSTER3_AZ 0.0
param set-default CA_THRUSTER4_PX 0.12
param set-default CA_THRUSTER4_PY -0.12
param set-default CA_THRUSTER4_PZ 0.0
param set-default CA_THRUSTER4_CT 1.4
param set-default CA_THRUSTER4_AX 0.0
param set-default CA_THRUSTER4_AY 1.0
param set-default CA_THRUSTER4_AZ 0.0
param set-default CA_THRUSTER5_PX 0.12
param set-default CA_THRUSTER5_PY 0.12
param set-default CA_THRUSTER5_PZ 0.0
param set-default CA_THRUSTER5_CT 1.4
param set-default CA_THRUSTER5_AX 0.0
param set-default CA_THRUSTER5_AY -1.0
param set-default CA_THRUSTER5_AZ 0.0
param set-default CA_THRUSTER6_PX -0.12
param set-default CA_THRUSTER6_PY -0.12
param set-default CA_THRUSTER6_PZ 0.0
param set-default CA_THRUSTER6_CT 1.4
param set-default CA_THRUSTER6_AX 0.0
param set-default CA_THRUSTER6_AY 1.0
param set-default CA_THRUSTER6_AZ 0.0
param set-default CA_THRUSTER7_PX -0.12
param set-default CA_THRUSTER7_PY 0.12
param set-default CA_THRUSTER7_PZ 0.0
param set-default CA_THRUSTER7_CT 1.4
param set-default CA_THRUSTER7_AX 0.0
param set-default CA_THRUSTER7_AY -1.0
param set-default CA_THRUSTER7_AZ 0.0
param set-default PWM_AUX_TIM0 10
param set-default PWM_AUX_TIM1 10
param set-default PWM_AUX_TIM2 10
param set-default PWM_AUX_FUNC1 101
param set-default PWM_AUX_FUNC2 102
param set-default PWM_AUX_FUNC3 103
param set-default PWM_AUX_FUNC4 104
param set-default PWM_AUX_FUNC5 105
param set-default PWM_AUX_FUNC6 106
param set-default PWM_AUX_FUNC7 107
param set-default PWM_AUX_FUNC8 108
param set-default PWM_AUX_DIS1 0
param set-default PWM_AUX_DIS2 0
param set-default PWM_AUX_DIS3 0
param set-default PWM_AUX_DIS4 0
param set-default PWM_AUX_DIS5 0
param set-default PWM_AUX_DIS6 0
param set-default PWM_AUX_DIS7 0
param set-default PWM_AUX_DIS8 0
param set-default PWM_AUX_MIN1 0
param set-default PWM_AUX_MIN2 0
param set-default PWM_AUX_MIN3 0
param set-default PWM_AUX_MIN4 0
param set-default PWM_AUX_MIN5 0
param set-default PWM_AUX_MIN6 0
param set-default PWM_AUX_MIN7 0
param set-default PWM_AUX_MIN8 0
# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec
param set-default PWM_AUX_MAX1 10000
param set-default PWM_AUX_MAX2 10000
param set-default PWM_AUX_MAX3 10000
param set-default PWM_AUX_MAX4 10000
param set-default PWM_AUX_MAX5 10000
param set-default PWM_AUX_MAX6 10000
param set-default PWM_AUX_MAX7 10000
param set-default PWM_AUX_MAX8 10000
# Controller Tunings
param set-default SC_ROLLRATE_P 0.14
param set-default SC_PITCHRATE_P 0.14
param set-default SC_ROLLRATE_I 0.3
param set-default SC_PITCHRATE_I 0.3
param set-default SC_ROLLRATE_D 0.004
param set-default SC_PITCHRATE_D 0.004

36
ROMFS/px4fmu_common/init.d/rc.sc_apps Normal file
View File

@ -0,0 +1,36 @@
#!/bin/sh
#
# Standard apps for sr. Attitude/Position estimator, Attitude/Position control.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
# Start Spacecraft App
spacecraft start
# Estimator Group Selection
# ekf2 start &
# Start MicroDDS Client
# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2
# uxrce_dds_client start -t udp -p 8888
#
# Start Control Allocator
#
# sc_control_allocator start
#
# Start Spacecraft Rate Controller.
#
# sc_rate_control start
#
# Start Spacecraft Attitude Controller.
#
# sc_att_control start
#
# Start Spacecraft Position Controller.
#
# sc_pos_control start

27
ROMFS/px4fmu_common/init.d/rc.sc_defaults Normal file
View File

@ -0,0 +1,27 @@
#!/bin/sh
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE sc
# MAV_TYPE_QUADROTOR 2
#param set-default MAV_TYPE 12
# Set micro-dds-client to use ethernet and IP-address 192.168.0.1
param set-default UXRCE_DDS_AG_IP -1062731775
# Disable preflight disarm to not interfere with external launching
param set-default COM_DISARM_PRFLT -1
param set-default CBRK_SUPPLY_CHK 894281
param set-default COM_ARM_HFLT_CHK 0
#Missing params
param set-default CP_DIST -1.0
# Default to MoCap fusion
param set-default ATT_EXT_HDG_M 2
param set-default EKF2_EV_CTRL 15
param set-default EKF2_EV_DELAY 5
param set-default EKF2_GPS_CTRL 0
param set-default EKF2_HGT_REF 3

2
Tools/kconfig/allyesconfig.py
View File

@ -53,6 +53,7 @@ exception_list = [
'SYSTEMCMDS_I2C_LAUNCHER', # undefined reference to `system',
'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl
'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors
'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
]
exception_list_sitl = [
@ -73,6 +74,7 @@ exception_list_sitl = [
'SYSTEMCMDS_I2CDETECT', # Not supported in SITL
'SYSTEMCMDS_DMESG', # Not supported in SITL
'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL
'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
]
def main():

2
Tools/simulation/gz

@ -1 +1 @@
Subproject commit 230450cc817dd7675612ed5ec72ee59b6989d367
Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a

1
boards/px4/sitl/default.px4board
View File

@ -80,3 +80,4 @@ CONFIG_EXAMPLES_HELLO=y
CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
CONFIG_EXAMPLES_WORK_ITEM=y
CONFIG_MODULES_SPACECRAFT=n

17
boards/px4/sitl/spacecraft.px4board Normal file
View File

@ -0,0 +1,17 @@
CONFIG_MODULES_AIRSPEED_SELECTOR=n
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
CONFIG_MODULES_FW_ATT_CONTROL=n
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_FW_POS_CONTROL=n
CONFIG_MODULES_FW_RATE_CONTROL=n
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
CONFIG_MODULES_MC_ATT_CONTROL=n
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
CONFIG_MODULES_CONTROL_ALLOCATOR=n
CONFIG_MODULES_SPACECRAFT=y

1
cmake/metadata.cmake
View File

@ -53,6 +53,7 @@ file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml
# avoid param duplicates
list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/")
list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/")
list(FILTER yaml_config_files EXCLUDE REGEX ".*/spacecraft/")
add_custom_target(metadata_parameters
COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs

8
src/modules/control_allocator/ControlAllocator.cpp
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;

2
src/modules/control_allocator/ControlAllocator.hpp
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 {

2
src/modules/control_allocator/module.yaml
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:

51
src/modules/spacecraft/CMakeLists.txt Normal file
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
)

12
src/modules/spacecraft/Kconfig Normal file
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

91
src/modules/spacecraft/SpacecraftHandler.cpp Normal file
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);
}

87
src/modules/spacecraft/SpacecraftHandler.hpp Normal file
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 */
};

235
src/modules/spacecraft/module.yaml Normal file
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