From 1ad66e606b47a29c1bd182e6e11bcf530b967f88 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 14 Dec 2021 15:48:58 +0100 Subject: [PATCH] ControlAllocation: introduce convention for control surface deflection sign - horizontal control surfaces: up=positive deflection - vertical control surfaces (rudder): right=positive deflection - mixed (V-Tail): up=positive deflection Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/1030_plane | 34 ++++---- .../init.d-posix/airframes/1040_standard_vtol | 15 ++-- .../init.d-posix/airframes/1041_tailsitter | 13 ++-- .../init.d-posix/airframes/1042_tiltrotor | 12 +-- .../ActuatorEffectivenessControlSurfaces.cpp | 15 ++-- .../ActuatorEffectivenessControlSurfaces.hpp | 20 ++--- src/modules/control_allocator/module.yaml | 78 +++++++++++++------ 7 files changed, 109 insertions(+), 78 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 2d30fa33d0..873555d7b5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -50,26 +50,24 @@ param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 param set-default CA_SV_CS_COUNT 6 -param set-default CA_SV_CS0_TRQ_Y 1.0000 -param set-default CA_SV_CS0_TYPE 2 -param set-default CA_SV_CS1_TYPE 7 -param set-default CA_SV_CS2_TRQ_P -1.0000 -param set-default CA_SV_CS2_TRQ_R 1.0000 -param set-default CA_SV_CS2_TYPE 9 -param set-default CA_SV_CS3_TRQ_P 1.0000 -param set-default CA_SV_CS3_TRQ_R 1.0000 -param set-default CA_SV_CS3_TYPE 10 -param set-default CA_SV_CS4_TRQ_P 1.0000 -param set-default CA_SV_CS5_TYPE 8 - -param set-default PWM_MAIN_FUNC3 201 -param set-default PWM_MAIN_FUNC4 202 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TYPE 4 +param set-default CA_SV_CS4_TYPE 9 +param set-default CA_SV_CS5_TYPE 10 +param set-default PWM_MAIN_FUNC3 204 +param set-default PWM_MAIN_FUNC4 205 param set-default PWM_MAIN_FUNC5 101 -param set-default PWM_MAIN_FUNC6 203 -param set-default PWM_MAIN_FUNC7 204 -param set-default PWM_MAIN_FUNC8 205 +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 -param set-default PWM_MAIN_REV 288 +param set-default PWM_MAIN_REV 256 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol index 0206e9d191..32650fca24 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -7,7 +7,7 @@ . ${R}etc/init.d/rc.vtol_defaults -#param set-default SYS_CTRL_ALLOC 1 +# param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 4 @@ -25,11 +25,12 @@ param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TRQ_R 1.0000 -param set-default CA_SV_CS0_TYPE 9 -param set-default CA_SV_CS1_TRQ_R 1.0000 -param set-default CA_SV_CS1_TYPE 10 -param set-default CA_SV_CS2_TRQ_P 1.0000 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS2_TRQ_P 1.0 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 @@ -38,8 +39,6 @@ param set-default PWM_MAIN_FUNC5 105 param set-default PWM_MAIN_FUNC6 201 param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 -param set-default PWM_MAIN_REV 32 - param set-default FW_L1_PERIOD 12 param set-default FW_MAN_P_MAX 30 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter index 723d8f9f0a..68cd393073 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter @@ -25,12 +25,12 @@ param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default CA_SV_CS_COUNT 2 -param set-default CA_SV_CS0_TYPE 9 -param set-default CA_SV_CS0_TRQ_P -1 -param set-default CA_SV_CS0_TRQ_Y -1 -param set-default CA_SV_CS1_TYPE 10 -param set-default CA_SV_CS1_TRQ_P -1 -param set-default CA_SV_CS1_TRQ_Y 1 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_Y -0.5 +param set-default CA_SV_CS1_TYPE 6 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_Y 0.5 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 @@ -39,6 +39,7 @@ param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 param set-default PWM_MAIN_FUNC6 201 param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_REV 96 # invert both elevons param set-default FW_L1_PERIOD 12 param set-default FW_MAN_P_MAX 30 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor index f2fe9de009..7f43eddf6c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor @@ -28,11 +28,12 @@ param set-default CA_ROTOR0_TILT 1 param set-default CA_ROTOR1_TILT 2 param set-default CA_ROTOR2_TILT 3 param set-default CA_ROTOR3_TILT 4 -param set-default CA_SV_CS0_TRQ_R 1.0000 -param set-default CA_SV_CS0_TYPE 9 -param set-default CA_SV_CS1_TRQ_R 1.0000 -param set-default CA_SV_CS1_TYPE 10 -param set-default CA_SV_CS2_TRQ_P 1.0000 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TYPE 3 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_TL0_CT 0 param set-default CA_SV_TL1_CT 0 @@ -51,7 +52,6 @@ param set-default PWM_MAIN_FUNC8 207 param set-default PWM_MAIN_FUNC9 201 param set-default PWM_MAIN_FUNC10 202 param set-default PWM_MAIN_FUNC11 203 -param set-default PWM_MAIN_REV 256 param set-default FW_L1_PERIOD 12 param set-default FW_MAN_P_MAX 30 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 7b849178b9..04acb7dfee 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -82,8 +82,15 @@ void ActuatorEffectivenessControlSurfaces::updateParams() param_get(_param_handles[i].trim, &_params[i].trim); - // TODO: enforce limits? + // TODO: enforce limits (note that tailsitter uses different limits)? switch (_params[i].type) { + + case Type::LeftAileron: + break; + + case Type::RightAileron: + break; + case Type::Elevator: break; @@ -107,12 +114,6 @@ void ActuatorEffectivenessControlSurfaces::updateParams() torque.setZero(); break; - case Type::LeftAileron: - break; - - case Type::RightAileron: - break; - case Type::Airbrakes: torque.setZero(); break; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index cad017ad87..7fa0c98d85 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -45,16 +45,16 @@ public: enum class Type : int32_t { // This matches with the parameter - Elevator = 1, - Rudder = 2, - LeftElevon = 3, - RightElevon = 4, - LeftVTail = 5, - RightVTail = 6, - LeftFlaps = 7, - RightFlaps = 8, - LeftAileron = 9, - RightAileron = 10, + LeftAileron = 1, + RightAileron = 2, + Elevator = 3, + Rudder = 4, + LeftElevon = 5, + RightElevon = 6, + LeftVTail = 7, + RightVTail = 8, + LeftFlaps = 9, + RightFlaps = 10, Airbrakes = 11, Custom = 12, }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index efd8cf7fb6..153d49096a 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -246,16 +246,16 @@ parameters: short: Control Surface ${i} type type: enum values: - 1: Elevator - 2: Rudder - 3: Left Elevon - 4: Right Elevon - 5: Left V-Tail - 6: Right V-Tail - 7: Left Flaps - 8: Right Flaps - 9: Left Aileron - 10: Right Aileron + 1: Left Aileron + 2: Right Aileron + 3: Elevator + 4: Rudder + 5: Left Elevon + 6: Right Elevon + 7: Left V-Tail + 8: Right V-Tail + 9: Left Flap + 10: Right Flap 11: Airbrakes 12: Custom num_instances: *max_num_servos @@ -423,23 +423,55 @@ mixer: - select_identifier: 'servo-type' # restrict torque based on servo type apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw'] items: - # TODO - 1: # Elevator + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # By default the scale is set to 1/N, where N is the amount of actuators with an effect on + # the corresponding axis. With that we are using all the available servo moving range. + 1: # Left Aileron + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + 2: # Right Aileron + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + 3: # Elevator - { 'hidden': True, 'default': 0.0 } # roll - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch - { 'hidden': True, 'default': 0.0 } # yaw - 2: # Rudder + 4: # Rudder - { 'hidden': True, 'default': 0.0 } # roll - { 'hidden': True, 'default': 0.0 } # pitch - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw - 3: # Left elevon - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll - - { 'min': -1.0, 'max': 0.0, 'default': -1.0 } # pitch + 5: # Left Elevon + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch - { 'hidden': True, 'default': 0.0 } # yaw - 4: # Right elevon - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch + 6: # Right Elevon + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch - { 'hidden': True, 'default': 0.0 } # yaw + 7: # Left V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + 8: # Right V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + + - select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters + apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter'] + items: + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # No rules for control surfaces on tailsitter beside elevons. + 5: # Left Elevon (Tailsitter --> wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + 6: # Right Elevon (Tailsitter -->wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw config: param: CA_AIRFRAME @@ -604,16 +636,16 @@ mixer: - name: 'CA_SV_CS${i}_TYPE' label: 'Type' function: 'type' - identifier: 'servo-type' + identifier: 'servo-type-tailsitter' - name: 'CA_SV_CS${i}_TRQ_R' label: 'Roll Scale' - identifier: 'servo-torque-roll' + identifier: 'servo-torque-roll-tailsitter' - name: 'CA_SV_CS${i}_TRQ_P' label: 'Pitch Scale' - identifier: 'servo-torque-pitch' + identifier: 'servo-torque-pitch-tailsitter' - name: 'CA_SV_CS${i}_TRQ_Y' label: 'Yaw Scale' - identifier: 'servo-torque-yaw' + identifier: 'servo-torque-yaw-tailsitter' - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' parameters: