diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 05ffb097f2..1d887a5f3f 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -88,3 +88,22 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } + +ActuatorEffectiveness::ActuatorBitmask ActuatorEffectiveness::getStoppedMotors() const +{ + // Motors can be stopped (here) for two reasons. They can + // 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. + 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; } + + if (_upwards_motors_stopped_by_thrust) { stopped_motors_mask |= _upwards_motors_mask; } + + if (_sideways_motors_stopped_by_thrust) { stopped_motors_mask |= _sideways_motors_mask; } + + 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 30d9b415f0..867da079cf 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -205,7 +205,7 @@ public: /** * Get a bitmask of motors to be stopped */ - virtual ActuatorBitmask getStoppedMotors() const { return _stopped_motors_mask; } + ActuatorBitmask getStoppedMotors() const; /** * Fill in the unallocated torque and thrust, customized by effectiveness type. @@ -213,8 +213,33 @@ public: */ virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + /** + * Record which components of the thrust setpoint are NaN, so that motors in that direction are stopped. + * + * @param thrust_sp The thrust setpoint as received by the allocator (instance zero - multicopter only) + */ + 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)); + } + protected: + + // The vehicle effectiveness classes should set these to + // 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{}; + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; - ActuatorBitmask _stopped_motors_mask{0}; + 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}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 39642bf71d..842b6379c5 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -395,6 +395,9 @@ ControlAllocator::Run() if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + _actuator_effectiveness->stopMotorsBasedOnThrustSetpoint(_thrust_sp); + _thrust_sp.nanToZero(); } if (do_update) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index cc955d1acc..e4448db256 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -64,6 +64,4 @@ private: uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; int _first_control_surface_idx{0}; ///< applies to matrix 1 - - ActuatorBitmask _forwards_motors_mask{}; }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 984ab00f9d..f9f3c5948f 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -59,6 +59,10 @@ 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(); + // 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 // and reduced to 0 with the sequential desaturation method. Instead we add it after. diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index 41ed5cb2c5..2ead33b5c8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -52,5 +52,9 @@ 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(); + return rotors_added_successfully; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 2d3ed43f08..4596d0f361 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -267,6 +267,11 @@ 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 { ActuatorBitmask upwards_motors = 0; @@ -297,6 +302,22 @@ ActuatorBitmask ActuatorEffectivenessRotors::getForwardsMotors() const 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 ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 60d9e11973..7aad2a62cd 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -129,8 +129,10 @@ public: void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } ActuatorBitmask getMotors() const; + ActuatorBitmask getUpwardsMotors() const; ActuatorBitmask getForwardsMotors() const; + ActuatorBitmask getSidewaysMotors() const; private: void updateParams() override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 063dcc00aa..35eca10ed8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -91,16 +91,15 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask |= _upwards_motors_mask; + _stopped_motors_mask_due_to_flight_phase = _upwards_motors_mask; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors_mask &= ~_upwards_motors_mask; + _stopped_motors_mask_due_to_flight_phase = 0; break; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 4cfd663de9..6bbadfd388 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -81,9 +81,6 @@ private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - ActuatorBitmask _upwards_motors_mask{}; - ActuatorBitmask _forwards_motors_mask{}; - int _first_control_surface_idx{0}; ///< applies to matrix 1 uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index e908ab06b8..a7f1293809 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -59,6 +59,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi // enable MC yaw control if more than 3 rotors _mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3); const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + updateMotorMasks(); // Control Surfaces configuration.selected_matrix = 1; @@ -96,17 +97,29 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors - switch (flight_phase) { + updateMotorMasks(); + +} + +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?) + _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; - _stopped_motors_mask = 0; + _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + break; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 0429b11300..6574955516 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -77,11 +77,11 @@ public: const char *name() const override { return "VTOL Tailsitter"; } protected: + void updateMotorMasks(); + ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - ActuatorBitmask _forwards_motors_mask{}; - int _first_control_surface_idx{0}; ///< applies to matrix 1 uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index fb3eb541cb..d59a93458f 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -83,6 +83,8 @@ 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(); _motors = _mc_rotors.getMotors(); // Control Surfaces @@ -216,16 +218,15 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask |= _untiltable_motors; + _stopped_motors_mask_due_to_flight_phase = _untiltable_motors; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors_mask = 0; + _stopped_motors_mask_due_to_flight_phase = 0; break; } }