From fb096d19105bd337ef184927258aa91508f5a162 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 19 Feb 2025 11:03:53 +0100 Subject: [PATCH] preflight check: optional arguments rather than members we now use optional args to give ActuatorEffectivenessTiltrotorVTOL::updateSetpoint the necessary info about the preflight check. All the other ActuatorEffectiveness*::updateSetpoint functions can be used exactly like before, by ignoring the optional arguments. Rather than the previous solution of setting a member variable only when _actuator_effectiveness is an instance of ActuatorEffectivenessTiltrotorVTOL, this also allows us to lose the ugly dynamic_cast (poor cpp man's instanceof). the other dynamic_cast is replaced by checking _effectiveness_source_id which should be equivalent. --- .../ActuatorEffectiveness.hpp | 3 +- .../control_allocator/ControlAllocator.cpp | 32 +++---------------- .../ActuatorEffectivenessCustom.cpp | 3 +- .../ActuatorEffectivenessCustom.hpp | 3 +- .../ActuatorEffectivenessFixedWing.cpp | 3 +- .../ActuatorEffectivenessFixedWing.hpp | 3 +- .../ActuatorEffectivenessHelicopter.cpp | 3 +- .../ActuatorEffectivenessHelicopter.hpp | 3 +- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 3 +- ...ActuatorEffectivenessHelicopterCoaxial.hpp | 3 +- .../ActuatorEffectivenessMCTilt.cpp | 3 +- .../ActuatorEffectivenessMCTilt.hpp | 3 +- .../ActuatorEffectivenessRoverAckermann.cpp | 3 +- .../ActuatorEffectivenessRoverAckermann.hpp | 3 +- .../ActuatorEffectivenessStandardVTOL.cpp | 3 +- .../ActuatorEffectivenessStandardVTOL.hpp | 3 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 3 +- .../ActuatorEffectivenessTailsitterVTOL.hpp | 3 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 7 ++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 6 ++-- .../ActuatorEffectivenessUUV.cpp | 3 +- .../ActuatorEffectivenessUUV.hpp | 3 +- 22 files changed, 48 insertions(+), 54 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index fd73fbd938..8c26d2ea79 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -198,7 +198,8 @@ public: */ virtual void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) {} + const matrix::Vector &actuator_max, bool preflight_check_running = false, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) {} /** * Get a bitmask of motors to be stopped diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index f5c1f168b4..50d93b407a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -450,23 +450,12 @@ ControlAllocator::Run() // Do allocation _control_allocation[i]->allocate(); - if (_preflight_check_running) { - - // alternative: specify these member variables in the general ActuatorEffectiveness, - // and just don't use them anywhere except TiltrotorVTOL - auto actuator_effectiveness_tiltrotor_vtol = dynamic_cast(_actuator_effectiveness); - - if (actuator_effectiveness_tiltrotor_vtol) { - float collective_tilt_sp = preflight_check_get_tilt_control(); - actuator_effectiveness_tiltrotor_vtol->_collective_tilt_normalized_setpoint = collective_tilt_sp; - actuator_effectiveness_tiltrotor_vtol->_collective_thrust_normalized_setpoint = 0.0f; - } - } + float preflight_check_tilt_sp = preflight_check_get_tilt_control(); _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(), - _preflight_check_running); + _preflight_check_running, preflight_check_tilt_sp, 0.0f); if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); @@ -494,23 +483,10 @@ ControlAllocator::Run() perf_end(_loop_perf); } -// void ControlAllocator::test_individual_control_surfaces() { - // goal here: modify actuation at the servo level. - - // in here: small state machine cycling through servos (or taking info - // from outside about which servo to actuate) - // if test running: if enough time passed: go to next thing - // if last thing: test = not running - - // elsewhere (probably Run()...) - // set test running if right message received - // if test running, - -// } - void ControlAllocator::preflight_check_update_state() { - bool tiltrotor = dynamic_cast(_actuator_effectiveness) != nullptr; + // bool tiltrotor = dynamic_cast(_actuator_effectiveness) != nullptr; + bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; // cycle through roll, pitch, yaw, and for each one inject positive and // negative torque setpoints. diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index 433e3024c4..67ea5ec861 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -61,7 +61,8 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index e471a403fa..6cfe9313cf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -47,7 +47,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; const char *name() const override { return "Custom"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index b0bac0a1d4..bd97193e4d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -63,7 +63,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 19125bf6c4..4f93e266cc 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -53,7 +53,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index d430dc6463..37ce255c63 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -130,7 +130,8 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index da3b613765..b2582cd252 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -80,7 +80,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index a2b17ae849..ca2ad1fa18 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -104,7 +104,8 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index 16b145206d..f635828f88 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -71,7 +71,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 788fe02874..ccd9e2fab9 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -78,7 +78,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { actuator_sp += _tilt_offsets; // TODO: dynamic matrix update diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index b1c2cd26ad..4f8c706dac 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -57,7 +57,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; const char *name() const override { return "MC Tilt"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 685a5c239d..b42dee86c0 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -51,7 +51,8 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 5e94366af2..730ba6b5f1 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -45,7 +45,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; const char *name() const override { return "Rover (Ackermann)"; } private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 30de4b2981..f0c067bdd6 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -85,7 +85,8 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { if (matrix_index == 0) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index b06105f37d..b9308c32bf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -77,7 +77,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 450a1cf2f7..31933df76b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -90,7 +90,8 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { if (matrix_index == 0) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 797569c047..3a9ce7f8d4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -74,7 +74,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index bfb3742ab5..285e3d8803 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -126,7 +126,8 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { // apply tilt @@ -147,8 +148,8 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; const char *name() const override { return "VTOL Tiltrotor"; } void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; - float _collective_tilt_normalized_setpoint{0.5f}; - float _collective_thrust_normalized_setpoint{0.0f}; - protected: bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp index ac55ca5fbf..cf45d8c281 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -57,7 +57,8 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) + const matrix::Vector &actuator_max, bool preflight_check_running, + float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 4a85f91e59..9ac1ed78e4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -56,7 +56,8 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max, bool preflight_check_running) override; + const matrix::Vector &actuator_max, bool preflight_check_running = true, + float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override; const char *name() const override { return "UUV"; }