diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 1d887a5f3f..29d3d54b5c 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -95,15 +95,15 @@ ActuatorEffectiveness::ActuatorBitmask ActuatorEffectiveness::getStoppedMotors() // additionally be stopped in the ControlAllocator, due to motor // failure. - // a) they are turned off because they are generally not used in the given flight phase. + // a) because they are generally not used in the given flight phase. ActuatorEffectiveness::ActuatorBitmask stopped_motors_mask = _stopped_motors_mask_due_to_flight_phase; - // b) they are stopped because the thrust setpoint in a given direction is NaN - if (_forwards_motors_stopped_by_thrust) { stopped_motors_mask |= _forwards_motors_mask; } + // b) because the thrust setpoint in a given direction is NaN + if (_longitudinal_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.longitudinal; } - if (_upwards_motors_stopped_by_thrust) { stopped_motors_mask |= _upwards_motors_mask; } + if (_lateral_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.lateral; } - if (_sideways_motors_stopped_by_thrust) { stopped_motors_mask |= _sideways_motors_mask; } + if (_vertical_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.vertical; } return stopped_motors_mask; } diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 867da079cf..3634ce490b 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -220,9 +220,9 @@ public: */ void stopMotorsBasedOnThrustSetpoint(const matrix::Vector3f &thrust_sp) { - _forwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0)); - _sideways_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1)); - _upwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2)); + _longitudinal_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0)); + _lateral_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1)); + _vertical_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2)); } protected: @@ -231,15 +231,17 @@ protected: // ActuatorEffectivenessRotors::get{Upwards,Forwards,Sideways}Motors() // They can alternatively be set to zero to completely disable stopping motors based on NaN thrust - ActuatorBitmask _upwards_motors_mask{}; - ActuatorBitmask _forwards_motors_mask{}; - ActuatorBitmask _sideways_motors_mask{}; + struct MotorDirectionBitmasks { + ActuatorBitmask longitudinal{}; + ActuatorBitmask lateral{}; + ActuatorBitmask vertical{}; + } _motor_direction_bitmasks; FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ActuatorBitmask _stopped_motors_mask_due_to_flight_phase{}; - bool _forwards_motors_stopped_by_thrust{false}; - bool _upwards_motors_stopped_by_thrust{false}; - bool _sideways_motors_stopped_by_thrust{false}; + bool _longitudinal_motors_stopped_by_thrust{false}; + bool _vertical_motors_stopped_by_thrust{false}; + bool _lateral_motors_stopped_by_thrust{false}; }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index be1c27b04a..5b93350d33 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -52,7 +52,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat // Motors _rotors.enablePropellerTorque(false); const bool rotors_added_successfully = _rotors.addActuators(configuration); - _forwards_motors_mask = _rotors.getForwardsMotors(); + _rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); // Control Surfaces _first_control_surface_idx = configuration.num_actuators_matrix[0]; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index f9f3c5948f..7199eb1981 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -59,9 +59,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration _tilts.updateTorqueSign(_mc_rotors.geometry()); const bool tilts_added_successfully = _tilts.addActuators(configuration); - _forwards_motors_mask = _mc_rotors.getForwardsMotors(); - _sideways_motors_mask = _mc_rotors.getSidewaysMotors(); - _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + _mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); // Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle). // Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index 2ead33b5c8..2e7fc74897 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -51,10 +51,7 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configura // Motors const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); - - _forwards_motors_mask = _mc_rotors.getForwardsMotors(); - _sideways_motors_mask = _mc_rotors.getSidewaysMotors(); - _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + _mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); return rotors_added_successfully; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 4596d0f361..00aad721e6 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -43,8 +43,6 @@ #include "ActuatorEffectivenessTilts.hpp" -using namespace matrix; - ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config, bool tilt_support) : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) @@ -267,55 +265,38 @@ ActuatorBitmask ActuatorEffectivenessRotors::getMotors() const return motors; } -// These three allow an angle deviation of 8.1 deg. Motor tilt for yaw actuation -// could reasonably be more. Alternatively, we could classify the motors such -// that every rotor points in a defined direction, putting the decision -// boundaries at 45 deg. TODO revisit - -ActuatorBitmask ActuatorEffectivenessRotors::getUpwardsMotors() const +// Helper to check if a vector is primarily aligned with a specific axis index +bool ActuatorEffectivenessRotors::isAlignedWithAxis(const Vector3f &axis_abs, int primary_idx) { - ActuatorBitmask upwards_motors = 0; + for (int i = 0; i < 3; ++i) { + if (i == primary_idx) { + if (axis_abs(i) <= 0.1f) { return false; } - 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; + } else { + if (axis_abs(i) >= 0.5f) { return false; } } } - return upwards_motors; + return true; } -ActuatorBitmask ActuatorEffectivenessRotors::getForwardsMotors() const +void ActuatorEffectivenessRotors::setMotorDirectionBitmasks(ActuatorEffectiveness::MotorDirectionBitmasks &masks) { - ActuatorBitmask forward_motors = 0; + masks.longitudinal = masks.lateral = masks.vertical = 0; for (int i = 0; i < _geometry.num_rotors; ++i) { - const Vector3f &axis = _geometry.rotors[i].axis; + const Vector3f &axis_abs = _geometry.rotors[i].axis.abs(); - if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { - forward_motors |= 1u << i; + if (isAlignedWithAxis(axis_abs, 0)) { // X-axis + masks.longitudinal |= 1 << i; + + } else if (isAlignedWithAxis(axis_abs, 1)) { // Y-axis + masks.lateral |= 1 << i; + + } else if (isAlignedWithAxis(axis_abs, 2)) { // Z-axis + masks.vertical |= 1 << i; } } - - return forward_motors; -} - -ActuatorBitmask ActuatorEffectivenessRotors::getSidewaysMotors() const -{ - ActuatorBitmask sideways_motors = 0; - - for (int i = 0; i < _geometry.num_rotors; ++i) { - const Vector3f &axis = _geometry.rotors[i].axis; - - // This includes both left and right pointing motors, in contrast to the other two directions... - if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) > 0.5f && fabsf(axis(2)) < 0.1f) { - sideways_motors |= 1u << i; - } - } - - return sideways_motors; } bool diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 7aad2a62cd..aaefd18171 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -50,9 +50,13 @@ class ActuatorEffectivenessTilts; using namespace time_literals; +using namespace matrix; using ActuatorBitmask = ActuatorEffectiveness::ActuatorBitmask; +static constexpr float MIN_AXIS_DOMINANT = 0.5f; +static constexpr float MAX_AXIS_NEGLIGIBLE = 0.5f; + class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness { public: @@ -130,15 +134,15 @@ public: ActuatorBitmask getMotors() const; - ActuatorBitmask getUpwardsMotors() const; - ActuatorBitmask getForwardsMotors() const; - ActuatorBitmask getSidewaysMotors() const; + void setMotorDirectionBitmasks(ActuatorEffectiveness::MotorDirectionBitmasks &masks); private: void updateParams() override; const AxisConfiguration _axis_config; const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + bool isAlignedWithAxis(const Vector3f &axis_abs, int primary_idx); + struct ParamHandles { param_t position_x; param_t position_y; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 35eca10ed8..3f6e5fc960 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -52,8 +52,7 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu configuration.selected_matrix = 0; _rotors.enablePropellerTorqueNonUpwards(false); const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); - _upwards_motors_mask = _rotors.getUpwardsMotors(); - _forwards_motors_mask = _rotors.getForwardsMotors(); + _rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); // Control Surfaces configuration.selected_matrix = 1; @@ -93,7 +92,7 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask_due_to_flight_phase = _upwards_motors_mask; + _stopped_motors_mask_due_to_flight_phase = _motor_direction_bitmasks.vertical; break; case FlightPhase::HOVER_FLIGHT: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index a7f1293809..f9c4b51c92 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -106,19 +106,22 @@ void ActuatorEffectivenessTailsitterVTOL::updateMotorMasks() switch (_flight_phase) { case FlightPhase::FORWARD_FLIGHT: - // TODO only set this mask if the motors are not needed for rate control, watching one or both of: - // - _mc_rotors.geometry().num_rotors > 3 (used in getEffectivenessMatrix to enable MC yaw control) - // - VT_FW_DIFTHR_EN (the general switch for this -- are the two equivalent?) + _mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); + + // Exchange forwards and upwards bitmasks - allocation frame + // upwards direction now points forwards + { + const ActuatorBitmask tmp = _motor_direction_bitmasks.longitudinal; + _motor_direction_bitmasks.longitudinal = _motor_direction_bitmasks.vertical; + _motor_direction_bitmasks.vertical = tmp; + } - _forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards - _upwards_motors_mask = 0; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _forwards_motors_mask = 0; - _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + _mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); break; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index d59a93458f..14e951f379 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -83,8 +83,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config << configuration.num_actuators[(int)ActuatorType::MOTORS]; const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); - _forwards_motors_mask = _mc_rotors.getForwardsMotors(); - _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + _mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks); _motors = _mc_rotors.getMotors(); // Control Surfaces