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 32650fca24..aa5c45dfab 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -10,7 +10,7 @@ # param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 2 -param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR_COUNT 5 param set-default CA_ROTOR0_PX 0.1515 param set-default CA_ROTOR0_PY 0.245 param set-default CA_ROTOR0_KM 0.05 @@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.1515 param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS0_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index d8a757adc5..bc399db45a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -62,7 +62,7 @@ param set-default VT_FW_MOT_OFFID 1234 param set-default VT_TYPE 2 param set-default CA_AIRFRAME 2 -param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR_COUNT 5 param set-default CA_ROTOR0_PX 0.15 param set-default CA_ROTOR0_PY 0.15 param set-default CA_ROTOR1_PX -0.15 @@ -73,6 +73,9 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.15 param set-default CA_ROTOR3_PY 0.15 param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS1_TRQ_R 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard index e41fc87d6b..b95fc758a3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard +++ b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard @@ -25,7 +25,7 @@ . ${R}etc/init.d/rc.vtol_defaults param set-default CA_AIRFRAME 2 -param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR_COUNT 5 param set-default CA_ROTOR0_PX 0.15 param set-default CA_ROTOR0_PY 0.15 param set-default CA_ROTOR1_PX -0.15 @@ -36,6 +36,9 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.15 param set-default CA_ROTOR3_PY 0.15 param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 4 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS0_TRQ_R -0.5 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index 083ad05fe0..ca991b5424 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -49,7 +49,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration } // motors - _motors.enableYawControl(false); + _motors.enablePropellerTorque(false); const bool motors_added_successfully = _motors.addActuators(configuration); // Torque diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 9bc52bd5d0..bddc95e913 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -51,7 +51,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat } // Motors - _rotors.enableYawControl(false); + _rotors.enablePropellerTorque(false); const bool rotors_added_successfully = _rotors.addActuators(configuration); // Control Surfaces diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 33a02269f2..da1ebdb9b6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -51,7 +51,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration } // MC motors - _mc_rotors.enableYawControl(!_tilts.hasYawControl()); + _mc_rotors.enablePropellerTorque(!_tilts.hasYawControl()); const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index e6ef1a1773..3833bf5459 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -185,10 +185,18 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry float ct = geometry.rotors[i].thrust_coef; float km = geometry.rotors[i].moment_ratio; - if (geometry.yaw_disabled) { + if (geometry.propeller_torque_disabled) { km = 0.f; } + if (geometry.propeller_torque_disabled_non_upwards) { + bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; + + if (!upwards) { + km = 0.f; + } + } + if (fabsf(ct) < FLT_EPSILON) { continue; } @@ -239,3 +247,18 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di Vector3f axis{0.f, 0.f, -1.f}; return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; } + +uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const +{ + uint32_t upwards_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { + upwards_motors |= 1u << i; + } + } + + return upwards_motors; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 0824c4fe6b..1572a1826d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -73,7 +73,8 @@ public: struct Geometry { RotorGeometry rotors[NUM_ROTORS_MAX]; int num_rotors{0}; - bool yaw_disabled{false}; + bool propeller_torque_disabled{false}; + bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors }; ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, @@ -113,7 +114,11 @@ public: */ static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction); - void enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; } + void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } + + void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } + + uint32_t getUpwardsMotors() const; private: void updateParams() override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 0bce8d5cb0..692f76e53c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -37,7 +37,7 @@ using namespace matrix; ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent) - : ModuleParams(parent), _mc_rotors(this), _control_surfaces(this) + : ModuleParams(parent), _rotors(this), _control_surfaces(this) { } @@ -49,17 +49,11 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu return false; } - // MC motors + // Motors configuration.selected_matrix = 0; - const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); - _mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1; - - // Pusher/Puller - configuration.selected_matrix = 1; - - for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) { - configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f}); - } + _rotors.enablePropellerTorqueNonUpwards(false); + const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); + _mc_motors_mask = _rotors.getUpwardsMotors(); // Control Surfaces configuration.selected_matrix = 1; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 4051791a9a..7eddbba718 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -80,7 +80,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } private: - ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight) @@ -90,8 +90,4 @@ private: uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)}; - DEFINE_PARAMETERS( - (ParamInt) _param_ca_stdvtol_n_p ///< number of pushers - ) - }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 2d37eb82ab..9710db8465 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -56,7 +56,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi // MC motors configuration.selected_matrix = 0; - _mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors + _mc_rotors.enablePropellerTorque(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); // Control Surfaces diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 3504913102..c92ccedaf6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -60,7 +60,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // MC motors configuration.selected_matrix = 0; - _mc_rotors.enableYawControl(!_tilts.hasYawControl()); + _mc_rotors.enablePropellerTorque(!_tilts.hasYawControl()); // Update matrix with tilts in vertical position when update is triggered by a manual // configuration (parameter) change. This is to make sure the normalization diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d58ea18715..83e4021d6e 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -370,18 +370,6 @@ parameters: instance_start: 0 default: 0 - # Standard VTOL - CA_STDVTOL_N_P: - description: - short: Number of fixed wing (pusher/puller) motors - type: enum - values: - 1: '1' - 2: '2' - 3: '3' - 4: '4' - default: 1 - # Mixer mixer: @@ -525,7 +513,7 @@ mixer: title: 'Standard VTOL' actuators: - actuator_type: 'motor' - group_label: 'MC Motors' + group_label: 'Motors' count: 'CA_ROTOR_COUNT' per_item_parameters: standard: @@ -535,10 +523,15 @@ mixer: label: 'Direction CCW' function: 'spin-dir' show_as: 'true-if-positive' - - actuator_type: 'motor' - group_label: 'FW Motors' - count: 'CA_STDVTOL_N_P' - item_label_prefix: 'FW Motor ${i}' + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' - actuator_type: 'servo' group_label: 'Control Surfaces' count: 'CA_SV_CS_COUNT' diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index ea1a9628ee..7c413d33b1 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -427,13 +427,13 @@ void Standard::fill_actuator_outputs() _thrust_setpoint_0->timestamp = hrt_absolute_time(); _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _thrust_setpoint_0->xyz[0] = 0.f; + _thrust_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE]; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE]; _thrust_setpoint_1->timestamp = hrt_absolute_time(); _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _thrust_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; _thrust_setpoint_1->xyz[2] = 0.f;