diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 65b987aba4..2afb41b002 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry return num_actuators; } -uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control) +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, + float collective_tilt_control) { - if (!PX4_ISFINITE(tilt_control)) { - tilt_control = -1.f; + if (!PX4_ISFINITE(collective_tilt_control)) { + collective_tilt_control = -1.f; } uint32_t nontilted_motors = 0; @@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv } const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); - float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f); - float tilt_direction = math::radians((float)tilt.tilt_direction); + const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); + const float tilt_direction = math::radians((float)tilt.tilt_direction); _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 9379b83786..6f5301a7ef 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -54,7 +54,7 @@ bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { - if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } @@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // Update matrix with tilts in vertical position when update is triggered by a manual // configuration (parameter) change. This is to make sure the normalization // scales are tilt-invariant. Note: configuration updates are only possible when disarmed. - const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f : - _last_tilt_control; - _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied) + const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? + -1.f : _last_collective_tilt_control; + _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied) << configuration.num_actuators[(int)ActuatorType::MOTORS]; const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); @@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // If it was an update coming from a config change, then make sure to update matrix in // the next iteration again with the correct tilt (but without updating the normalization scale). - _combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); + _collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully); } @@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 0.99f ? 1.f : control_tilt; + // set control_collective_tilt to exactly -1 or 1 if close to these end points + control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt; + control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt; - // initialize _last_tilt_control - if (!PX4_ISFINITE(_last_tilt_control)) { - _last_tilt_control = control_tilt; + // initialize _last_collective_tilt_control + if (!PX4_ISFINITE(_last_collective_tilt_control)) { + _last_collective_tilt_control = control_collective_tilt; - } else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) { - _combined_tilt_updated = true; - _last_tilt_control = control_tilt; + } else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) { + _collective_tilt_updated = true; + _last_collective_tilt_control = control_collective_tilt; } for (int i = 0; i < _tilts.count(); ++i) { if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) { - actuator_sp(i + _first_tilt_idx) += control_tilt; + actuator_sp(i + _first_tilt_idx) += control_collective_tilt; } } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 7f242915fe..d7d136727c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -82,7 +82,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: - bool _combined_tilt_updated{true}; + bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; ActuatorEffectivenessTilts _tilts; @@ -93,7 +93,7 @@ protected: int _first_control_surface_idx{0}; ///< applies to matrix 1 int _first_tilt_idx{0}; ///< applies to matrix 0 - float _last_tilt_control{NAN}; + float _last_collective_tilt_control{NAN}; uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};