Skip to content

Commit

Permalink
initial control allocation support
Browse files Browse the repository at this point in the history 
 - control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See #13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>
  • Loading branch information
@jlecoeur @sfuhrer
@RomanBapst @dagar
3 people authored and dagar committed Jan 18, 2021
1 parent fc6b61d commit 2c670ff
Show file tree
Hide file tree
Showing 68 changed files with 6,129 additions and 17 deletions.
52 changes: 52 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
#!/bin/sh
#
# @name 3DR Iris Quadrotor SITL
#
# @type Quadrotor Wide
#
# @maintainer Julian Oes <julian@oes.ch>
#

sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc

if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0

param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05

param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0

param set CA_MC_R0_PX 0.1515
param set CA_MC_R0_PY 0.245
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM 0.05
param set CA_MC_R1_PX -0.1515
param set CA_MC_R1_PY -0.1875
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.1515
param set CA_MC_R2_PY -0.245
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.1515
param set CA_MC_R3_PY 0.1875
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM -0.05

fi

set MIXER direct
75 changes: 75 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
#!/bin/sh
#
# @name Typhoon H480 SITL
#
# @type Hexarotor x
#

sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc

if [ $AUTOCNF = yes ]
then
param set MPC_XY_VEL_I_ACC 4
param set MPC_XY_VEL_P_ACC 3

param set RTL_DESCEND_ALT 10
param set RTL_LAND_DELAY 0

param set TRIG_INTERFACE 3
param set TRIG_MODE 4
param set MNT_MODE_IN 0
param set MAV_PROTO_VER 2

param set MPC_USE_HTE 0

param set VM_MASS 2.66
param set VM_INERTIA_XX 0.06
param set VM_INERTIA_YY 0.06
param set VM_INERTIA_ZZ 0.10

param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT4_MIN 0.0
param set CA_ACT5_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_ACT4_MAX 1.0
param set CA_ACT5_MAX 1.0

param set CA_MC_R0_PX 0.0
param set CA_MC_R0_PY 1.0
param set CA_MC_R0_CT 9.5
param set CA_MC_R0_KM -0.05
param set CA_MC_R1_PX 0.0
param set CA_MC_R1_PY -1.0
param set CA_MC_R1_CT 9.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.866025
param set CA_MC_R2_PY -0.5
param set CA_MC_R2_CT 9.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.866025
param set CA_MC_R3_PY 0.5
param set CA_MC_R3_CT 9.5
param set CA_MC_R3_KM 0.05
param set CA_MC_R4_PX 0.866025
param set CA_MC_R4_PY 0.5
param set CA_MC_R4_CT 9.5
param set CA_MC_R4_KM 0.05
param set CA_MC_R5_PX -0.866025
param set CA_MC_R5_PY -0.5
param set CA_MC_R5_CT 9.5
param set CA_MC_R5_KM -0.05
fi

set MAV_TYPE 13

# set MIXER hexa_x
set MIXER direct
10 changes: 10 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@

mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix

mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530

# shellcheck disable=SC2154
mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
# shellcheck disable=SC2154
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,4 +54,5 @@ px4_add_romfs_files(
rc.vehicle_setup
rc.vtol_apps
rc.vtol_defaults
rc.ctrlalloc
)
57 changes: 57 additions & 0 deletions ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
#!/bin/sh
#
# @name S500 with control allocation
#
# @type Quadrotor x
# @class Copter
#
# @maintainer Silvan Fuhrer
#

sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc

if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0

param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05

param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0

param set CA_MC_R0_PX 0.177
param set CA_MC_R0_PY 0.177
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM 0.05
param set CA_MC_R1_PX -0.177
param set CA_MC_R1_PY -0.177
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.177
param set CA_MC_R2_PY -0.177
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.177
param set CA_MC_R3_PY 0.177
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM -0.05

fi

set MIXER direct
set PWM_OUT 1234

set MIXER_AUX direct_aux
set PWM_AUX_OUT 1234
75 changes: 75 additions & 0 deletions ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
#!/bin/sh
#
# @name Hex X with control allocation
#
# @type Hexarotor x
# @class Copter
#
# @maintainer Silvan Fuhrer
#

sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc

if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0

param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05

param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT4_MIN 0.0
param set CA_ACT5_MIN 0.0

param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_ACT4_MAX 1.0
param set CA_ACT5_MAX 1.0

param set CA_MC_R0_PX 0.0
param set CA_MC_R0_PY 0.275
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM -0.05

param set CA_MC_R1_PX 0.0
param set CA_MC_R1_PY -0.275
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05

param set CA_MC_R2_PX 0.238
param set CA_MC_R2_PY -0.1375
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05

param set CA_MC_R3_PX -0.238
param set CA_MC_R3_PY 0.1375
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM 0.05

param set CA_MC_R4_PX 0.238
param set CA_MC_R4_PY 0.1375
param set CA_MC_R4_CT 6.5
param set CA_MC_R4_KM 0.05

param set CA_MC_R5_PX -0.238
param set CA_MC_R5_PY -0.1375
param set CA_MC_R5_CT 6.5
param set CA_MC_R5_KM -0.05

fi

set MIXER direct
set PWM_OUT 123456

set MIXER_AUX direct_aux
set PWM_AUX_OUT 123456
26 changes: 26 additions & 0 deletions ROMFS/px4fmu_common/init.d/rc.ctrlalloc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
#!/bin/sh
#
# Standard apps for new control allocation and controllers
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#

#
# Start angular velocity controller
#
angular_velocity_controller start
mc_rate_control stop

#
# Start Control Allocator
#
control_allocator start

#
# Disable hover thrust estimator and prearming
# These features are currently incompatible with control allocation
#
# TODO: fix
#
param set MPC_USE_HTE 0
param set COM_PREARM_MODE 0
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,4 +43,5 @@ px4_add_romfs_files(
tiltrotor_sitl.main.mix
uuv_x_sitl.main.mix
vectored6dof_sitl.main.mix
tiltrotor_sitl_direct.main.mix
)
14 changes: 14 additions & 0 deletions ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
Mixer for quad tiltrotor (x motor configuration)
================================================

A: 0
A: 1
A: 2
A: 3
A: 4
A: 5
A: 6
A: 7
A: 8
A: 9
A: 10
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/mixers/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ px4_add_romfs_files(
coax.main.mix
delta.main.mix
deltaquad.main.mix
direct.main.mix
dodeca_bottom_cox.aux.mix
dodeca_top_cox.main.mix
firefly6.aux.mix
Expand Down
10 changes: 10 additions & 0 deletions ROMFS/px4fmu_common/mixers/direct.main.mix
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
# Direct mixer

A: 0
A: 1
A: 2
A: 3
A: 4
A: 5
A: 6
A: 7
2 changes: 1 addition & 1 deletion Tools/px_romfs_pruner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ def main():
# handle mixer files differently than startup files
if file_path.endswith(".mix"):
if line.startswith(("Z:", "M:", "R: ", "O:", "S:",
"H:", "T:", "P:")):
"H:", "T:", "P:", "A:")):
# reduce multiple consecutive spaces into a
# single space
line_reduced = re.sub(' +', ' ', line)
Expand Down
Loading

0 comments on commit 2c670ff

Please sign in to comment.