From a916f507189852d452e791b03878ab6d30bc39c1 Mon Sep 17 00:00:00 2001 From: Pedro-Roque Date: Thu, 30 Jan 2025 16:30:56 +0100 Subject: [PATCH] feat: addition of functions --- .../71001_gazebo-classic_spacecraft_dart | 2 +- .../airframes/71002_gz_spacecraft_2d | 2 +- ROMFS/px4fmu_common/init.d/rc.sc_defaults | 1 - src/lib/mixer_module/CMakeLists.txt | 1 + src/modules/sc_control_allocator/module.yaml | 412 ++---------------- 5 files changed, 31 insertions(+), 387 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart index 7c3449fca4..f582f5d0f3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart @@ -9,7 +9,7 @@ . ${R}etc/init.d/rc.sc_defaults -param set-default CA_AIRFRAME 14 +param set-default CA_AIRFRAME 15 param set-default MAV_TYPE 99 param set-default CA_THRUSTER_CNT 12 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 342ac7c667..9a320d83dd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -21,7 +21,7 @@ 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 13 +param set-default CA_AIRFRAME 14 param set-default MAV_TYPE 99 param set-default CA_THRUSTER_CNT 8 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 6ce7016844..8dc8867b22 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -25,4 +25,3 @@ 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 - diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..ca7cef027d 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -53,6 +53,7 @@ px4_add_library(mixer_module functions/FunctionMotors.hpp functions/FunctionParachute.hpp functions/FunctionServos.hpp + functions/FunctionThrusters.hpp actuator_test.cpp actuator_test.hpp diff --git a/src/modules/sc_control_allocator/module.yaml b/src/modules/sc_control_allocator/module.yaml index 864485be4a..15ee751edb 100644 --- a/src/modules/sc_control_allocator/module.yaml +++ b/src/modules/sc_control_allocator/module.yaml @@ -1,5 +1,5 @@ __max_num_mc_motors: &max_num_mc_motors 12 -__max_num_sc_thrusters: &max_num_sc_thrusters 12 +__max_num_thrusters: &max_num_thrusters 12 __max_num_servos: &max_num_servos 8 __max_num_tilts: &max_num_tilts 4 @@ -22,199 +22,8 @@ parameters: 2: Automatic default: 1 - # Motor parameters - SC_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_CA_R${i}_SLEW: - description: - short: Motor ${i} slew rate limit - long: | - Minimum time allowed for the motor input signal to pass through - the full output range. A value x means that the motor signal - can only go from 0 to 1 in minimum x seconds (in case of - reversible motors, the range is -1 to 1). - - Zero means that slew rate limiting is disabled. - type: float - decimal: 2 - increment: 0.01 - num_instances: *max_num_mc_motors - min: 0 - max: 10 - default: 0.0 - - # Servo params - SC_CA_SV${i}_SLEW: - description: - short: Servo ${i} slew rate limit - long: | - Minimum time allowed for the servo input signal to pass through - the full output range. A value x means that the servo signal - can only go from -1 to 1 in minimum x seconds. - - Zero means that slew rate limiting is disabled. - type: float - decimal: 2 - increment: 0.05 - num_instances: *max_num_servos - min: 0 - max: 10 - default: 0.0 - - - # Motors for external disturbances - SC_CA_ROTOR_COUNT: - description: - short: Total number of rotors - 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 - SC_CA_ROTOR${i}_PX: - description: - short: Position of rotor ${i} along X body axis relative to center of gravity - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_mc_motors - min: -100 - max: 100 - default: 0.0 - SC_CA_ROTOR${i}_PY: - description: - short: Position of rotor ${i} along Y body axis relative to center of gravity - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_mc_motors - min: -100 - max: 100 - default: 0.0 - SC_CA_ROTOR${i}_PZ: - description: - short: Position of rotor ${i} along Z body axis relative to center of gravity - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_mc_motors - min: -100 - max: 100 - default: 0.0 - - SC_CA_ROTOR${i}_AX: - description: - short: Axis of rotor ${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_mc_motors - min: -100 - max: 100 - default: 0.0 - SC_CA_ROTOR${i}_AY: - description: - short: Axis of rotor ${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_mc_motors - min: -100 - max: 100 - default: 0.0 - SC_CA_ROTOR${i}_AZ: - description: - short: Axis of rotor ${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_mc_motors - min: -100 - max: 100 - default: -1.0 - - SC_CA_ROTOR${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_mc_motors - min: 0 - max: 100 - default: 6.5 - - SC_CA_ROTOR${i}_KM: - description: - short: Moment coefficient of rotor ${i} - long: | - The moment coefficient if defined as Torque = KM * Thrust. - - Use a positive value for a rotor with CCW rotation. - Use a negative value for a rotor with CW rotation. - type: float - decimal: 3 - increment: 0.01 - num_instances: *max_num_mc_motors - min: -1 - max: 1 - default: 0.05 - SC_CA_ROTOR${i}_TILT: - description: - short: Rotor ${i} tilt assignment - long: If not set to None, this motor is tilted by the configured tilt servo. - type: enum - values: - 0: 'None' - 1: 'Tilt 1' - 2: 'Tilt 2' - 3: 'Tilt 3' - 4: 'Tilt 4' - num_instances: *max_num_mc_motors - instance_start: 0 - default: 0 - # (SC) Thrusters - SC_CA_THRUSTER_CNT: + CA_THRUSTER_CNT: description: short: Total number of thrusters type: enum @@ -233,74 +42,74 @@ parameters: 11: '11' 12: '12' default: 0 - SC_CA_THRUSTER${i}_PX: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: 0.0 - SC_CA_THRUSTER${i}_PY: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: 0.0 - SC_CA_THRUSTER${i}_PZ: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: 0.0 - SC_CA_THRUSTER${i}_AX: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: 0.0 - SC_CA_THRUSTER${i}_AY: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: 0.0 - SC_CA_THRUSTER${i}_AZ: + 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_sc_thrusters + num_instances: *max_num_thrusters min: -100 max: 100 default: -1.0 - SC_CA_THRUSTER${i}_CT: + CA_THRUSTER${i}_CT: description: short: Thrust coefficient of rotor ${i} long: | @@ -310,7 +119,7 @@ parameters: type: float decimal: 1 increment: 1 - num_instances: *max_num_sc_thrusters + num_instances: *max_num_thrusters min: 0 max: 100 default: 6.5 @@ -318,226 +127,61 @@ parameters: # Mixer mixer: actuator_types: - motor: - functions: 'Motor' - actuator_testing_values: - min: 0 - max: 1 - default_is_nan: true - per_item_parameters: - - name: 'SC_CA_R_REV' - label: 'Bidirectional' - show_as: 'bitset' - index_offset: 0 - advanced: true - - name: 'CA_R${i}_SLEW' - label: 'Slew Rate' - index_offset: 0 - advanced: true thruster: functions: 'Thruster' actuator_testing_values: min: 0 max: 1 default_is_nan: true - servo: - functions: 'Servo' - actuator_testing_values: - min: -1 - max: 1 - default: 0 - per_item_parameters: - - name: 'SC_CA_SV${i}_SLEW' - label: 'Slew Rate' - index_offset: 0 - advanced: true DEFAULT: actuator_testing_values: min: -1 max: 1 default: -1 - rules: - - select_identifier: 'servo-type' # restrict torque based on servo type - apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler'] - items: - # 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. - 0: # Not set - - { 'disabled': True, 'default': 0.0 } # roll - - { 'disabled': True, 'default': 0.0 } # pitch - - { 'disabled': True, 'default': 0.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 3: # Elevator - - { 'hidden': True, 'default': 0.0 } # roll - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 4: # Rudder - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 9: # Left Flap - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 10: # Right Flap - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 11: # Airbrake - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 12: # Custom - - { 'hidden': False, 'default': 0.0 } # roll - - { 'hidden': False, 'default': 0.0 } # pitch - - { 'hidden': False, 'default': 0.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 13: # Left A 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 14: # Right A 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 - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 15: # Single Channel Aileron - - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 16: # Steering Wheel - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler - 17: # Left Spoiler - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler - 18: # Right Spoiler - - { 'hidden': True, 'default': 0.0 } # roll - - { 'hidden': True, 'default': 0.0 } # pitch - - { 'hidden': True, 'default': 0.0 } # yaw - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap - - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler - - - - 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 types: - 13: # Spacecraft 2D - type: 'spacecraft_2d' + 14: # Spacecraft 2D title: 'Spacecraft 2D' actuators: - actuator_type: 'thruster' - count: 'SC_CA_THRUSTER_CNT' # count would be too long for 16 max size + count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size per_item_parameters: standard: - position: [ 'SC_CA_THRUSTER${i}_PX', 'SC_CA_THRUSTER${i}_PY', 'SC_CA_THRUSTER${i}_PZ' ] + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] extra: - - name: 'SC_CA_THRUSTER${i}_AX' + - name: 'CA_THRUSTER${i}_AX' label: 'Axis X' function: 'axisx' advanced: true - - name: 'SC_CA_THRUSTER${i}_AY' + - name: 'CA_THRUSTER${i}_AY' label: 'Axis Y' function: 'axisy' advanced: true - - name: 'SC_CA_THRUSTER${i}_AZ' + - name: 'CA_THRUSTER${i}_AZ' label: 'Axis Z' function: 'axisz' advanced: true - 14: # Sapcecraft 3D + 15: # Sapcecraft 3D title: 'Spacecraft 3D' actuators: - actuator_type: 'thruster' - count: 'SC_CA_THRUSTER_CNT' + count: 'CA_THRUSTER_CNT' per_item_parameters: standard: - position: [ 'SC_CA_THRUSTER${i}_PX', 'SC_CA_THRUSTER${i}_PY', 'SC_CA_THRUSTER${i}_PZ' ] + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] extra: - - name: 'SC_CA_THRUSTER${i}_AX' + - name: 'CA_THRUSTER${i}_AX' label: 'Axis X' function: 'axisx' advanced: true - - name: 'SC_CA_THRUSTER${i}_AY' + - name: 'CA_THRUSTER${i}_AY' label: 'Axis Y' function: 'axisy' advanced: true - - name: 'SC_CA_THRUSTER${i}_AZ' + - name: 'CA_THRUSTER${i}_AZ' label: 'Axis Z' function: 'axisz' advanced: true