From 0817ee40f8f9f2481ce12c6a025a61087ef7fb1e Mon Sep 17 00:00:00 2001 From: mcsauder Date: Wed, 17 Jul 2019 10:32:21 -0600 Subject: [PATCH] Add generic vtol tailsitter airframe/mixer and incorporate modifications from PR 9849 in 4001_quad_x and 13001_caipirinha_vtol. --- .../init.d/airframes/13001_caipirinha_vtol | 9 ++++- .../airframes/13200_generic_vtol_tailsitter | 30 ++++++++++++++ .../init.d/airframes/4001_quad_x | 2 +- .../init.d/airframes/CMakeLists.txt | 1 + .../mixers/vtol_tailsitter_duo.main.mix | 39 +++++++++++++++++++ 5 files changed, 79 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter create mode 100644 ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix diff --git a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol index f6d8ff30f2ff..4542638645bd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol @@ -17,16 +17,20 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF = yes ] then + param set MAV_TYPE 19 + param set MC_ROLL_P 6 param set MC_ROLLRATE_P 0.12 param set MC_ROLLRATE_I 0.002 param set MC_ROLLRATE_D 0.003 param set MC_ROLLRATE_FF 0 + param set MC_PITCH_P 4.5 param set MC_PITCHRATE_P 0.3 param set MC_PITCHRATE_I 0.002 param set MC_PITCHRATE_D 0.003 param set MC_PITCHRATE_FF 0 + param set MC_YAW_P 3.8 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 @@ -37,10 +41,13 @@ then param set VT_ELEV_MC_LOCK 0 param set VT_MOT_ID 12 param set VT_TYPE 0 + + # Indicate that FW roll direction was fixed in mixer, to be removed in v1.10 + param set V19_VT_ROLLDIR 0 fi set MAV_TYPE 19 -set MIXER caipirinha_vtol +set MIXER vtol_tailsitter_duo set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter new file mode 100644 index 000000000000..232a9bc5073e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter @@ -0,0 +1,30 @@ +#!/bin/sh +# +# @name Generic Tailsitter +# +# @type VTOL Duo Tailsitter +# @class VTOL +# +# @output MAIN1 motor right +# @output MAIN2 motor left +# @output MAIN5 elevon right +# @output MAIN6 elevon left +# +# @maintainer Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF = yes ] +then + param set VT_ELEV_MC_LOCK 0 + param set VT_MOT_COUNT 2 + param set VT_TYPE 0 + + param set MAV_TYPE 19 +fi + +set MAV_TYPE 19 +set MIXER vtol_tailsitter_duo + +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x index 60605cc16db3..709e8197ef5a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic Quadrotor x +# @name Generic Quadcopter # # @type Quadrotor x # @class Copter diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 0dc26be26135..e6cfe75e032a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -132,6 +132,7 @@ px4_add_romfs_files( 13010_claire 13012_convergence 13013_deltaquad + 13200_generic_vtol_tailsitter # [14000, 14999] Tri Y 14001_tri_y_yaw+ diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix new file mode 100644 index 000000000000..b2215a8a5e78 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix @@ -0,0 +1,39 @@ +Tailsitter duo mixer +============================ + +This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle +has two motors in total, one attached to each wing. It also has two elevons which +are located in the slipstream of the propellers. This mixer generates 4 PWM outputs +on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the +elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run +at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. + +Motor mixer +------------ +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. + +R: 2- 10000 10000 10000 0 + +Zero mixer (2x) +--------------- +Channels 3,4 are unused. + +Z: + +Z: + +Elevons mixer +-------------- +Channel 5 connects to the right (starboard) elevon. +Channel 6 connects to the left (port) elevon. + +M: 2 +O: 7500 7500 0 -10000 10000 +S: 1 0 -10000 -10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 7500 7500 0 -10000 10000 +S: 1 0 -10000 -10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000