From c28d015d9c0898f4ac95ea7422d0571401e450ee Mon Sep 17 00:00:00 2001 From: Balduin Date: Tue, 18 Feb 2025 15:42:28 +0100 Subject: [PATCH] preflight check: replace uOrb msg with argument Previously, the approach to modify collective tilt control was to send the corresponding tiltrotor_extra_control uOrb message from ControlAllocator, which then influences ActuatorEffectivenessTiltrotorVTOL with minimal changes. This was a bit hacky and introduced potentially conflicting uOrb messages. So, with this new approach we pass the same information via argument. Specifically, the class ActuatorEffectiveness now declares updateSetpoint with an extra argument, preflight_check_running. It is only used in ActuatorEffectivenessTiltrotorVTOL, but has to be included as a "dummy" in all other classes inheriting from ActuatorEffectiveness. The argument can be used to bypass the collective tilt/thrust setpoints, instead replacing them with values from public class member variables which can be set from outside just before calling updateSetpoints. Also, slight refactor in ControlAllocator by splitting up the functions related to the preflight check into smaller parts --- .../ActuatorEffectiveness.hpp | 2 +- .../control_allocator/ControlAllocator.cpp | 69 ++++++++++--------- .../control_allocator/ControlAllocator.hpp | 5 +- .../ActuatorEffectivenessCustom.cpp | 2 +- .../ActuatorEffectivenessCustom.hpp | 2 +- .../ActuatorEffectivenessFixedWing.cpp | 2 +- .../ActuatorEffectivenessFixedWing.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 2 +- .../ActuatorEffectivenessHelicopter.hpp | 2 +- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 2 +- ...ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- .../ActuatorEffectivenessMCTilt.cpp | 2 +- .../ActuatorEffectivenessMCTilt.hpp | 2 +- .../ActuatorEffectivenessRoverAckermann.cpp | 2 +- .../ActuatorEffectivenessRoverAckermann.hpp | 2 +- .../ActuatorEffectivenessStandardVTOL.cpp | 2 +- .../ActuatorEffectivenessStandardVTOL.hpp | 2 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 2 +- .../ActuatorEffectivenessTailsitterVTOL.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 24 +++++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 5 +- .../ActuatorEffectivenessUUV.cpp | 2 +- .../ActuatorEffectivenessUUV.hpp | 2 +- 23 files changed, 81 insertions(+), 60 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 0ddd4988f3..fd73fbd938 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -198,7 +198,7 @@ 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) {} + const matrix::Vector &actuator_max, bool preflight_check_running) {} /** * 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 631f6429a2..f5c1f168b4 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -61,8 +61,6 @@ ControlAllocator::ControlAllocator() : _actuator_servos_pub.advertise(); _actuator_servos_trim_pub.advertise(); - _tiltrotor_extra_controls_pub.advertise(); - for (int i = 0; i < MAX_NUM_MOTORS; ++i) { char buffer[17]; snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); @@ -347,7 +345,6 @@ ControlAllocator::Run() if (_vehicle_status_sub.update(&vehicle_status)) { _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; - _preflight_check_running = vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK; ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; @@ -442,25 +439,34 @@ ControlAllocator::Run() } if (_preflight_check_running) { + preflight_check_update_state(); preflight_check_overwrite_torque_sp(c); } for (int i = 0; i < _num_control_allocation; ++i) { - ActuatorEffectivenessTiltrotorVTOL *casted = dynamic_cast - (_actuator_effectiveness); - - if (casted != nullptr) { - casted->_preflight_check_running = _preflight_check_running; - } - _control_allocation[i]->setControlSetpoint(c[i]); // 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; + } + } + _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()); + _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(), + _preflight_check_running); if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); @@ -502,15 +508,13 @@ ControlAllocator::Run() // } -void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) { +void ControlAllocator::preflight_check_update_state() { + + bool tiltrotor = dynamic_cast(_actuator_effectiveness) != nullptr; // cycle through roll, pitch, yaw, and for each one inject positive and // negative torque setpoints. - // is this the proper way to do it? - // bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; - bool tiltrotor = dynamic_cast(_actuator_effectiveness) != nullptr; - int n_axes = 3; if (tiltrotor) { n_axes = 4; @@ -524,12 +528,13 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) { int axis = _preflight_check_phase / 2; int negative = _preflight_check_phase % 2; - float modified_tilt_control = 0.5f; - if (axis < 3) { c[0](0) = 0.; c[0](1) = 0.; @@ -543,24 +548,24 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]); + void preflight_check_update_state(); + float preflight_check_get_tilt_control(); AllocationMethod _allocation_method_id{AllocationMethod::NONE}; ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations @@ -181,7 +183,6 @@ private: uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1}; /**< vehicle torque setpoint subscription (2. instance) */ uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */ - uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; // Outputs uORB::PublicationMulti _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)}; @@ -190,8 +191,6 @@ private: uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)}; - uORB::Publication _tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index ead2043682..433e3024c4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -61,7 +61,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index b7906669f2..e471a403fa 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -47,7 +47,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 0a4516ba56..b0bac0a1d4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -63,7 +63,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { 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 f0b095709e..19125bf6c4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -53,7 +53,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 643b218c47..d430dc6463 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -130,7 +130,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 93083fa066..da3b613765 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -80,7 +80,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 0c06f5963f..a2b17ae849 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -104,7 +104,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index a507aee2dd..16b145206d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -71,7 +71,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 a8effa8cb3..788fe02874 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -78,7 +78,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { 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 0b12482781..b1c2cd26ad 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -57,7 +57,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 e9eda4c538..685a5c239d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -51,7 +51,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index f6108b4baf..5e94366af2 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -45,7 +45,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 f15624dd67..30de4b2981 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -85,7 +85,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { 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 9f945a6cd8..b06105f37d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -77,7 +77,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 559101c106..450a1cf2f7 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -90,7 +90,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { 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 708f104faa..797569c047 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -74,7 +74,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) 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 92f7119161..bfb3742ab5 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -126,14 +126,30 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { // apply tilt + + // if preflight_check_running, we alter the behaviour in these two ways: + // - collective tilt and thrust setpoints are NOT taken from uOrb + // message, but from class member variable, which we can arbitrarily set + // before calling this function + // - collective tilt is added to actuator_sp even if + // (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) + // evaluates to false + if (matrix_index == 0) { tiltrotor_extra_controls_s tiltrotor_extra_controls; - if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) { + if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || preflight_check_running) { + float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f; + float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint; + + if (preflight_check_running) { + control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f; + control_collective_thrust = _collective_thrust_normalized_setpoint; + } // 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; @@ -166,7 +182,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) override; const char *name() const override { return "VTOL Tiltrotor"; } void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; - bool _preflight_check_running{false}; + float _collective_tilt_normalized_setpoint{0.5f}; + float _collective_thrust_normalized_setpoint{0.0f}; protected: bool _collective_tilt_updated{true}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp index e4c861a9d0..ac55ca5fbf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -57,7 +57,7 @@ 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) + const matrix::Vector &actuator_max, bool preflight_check_running) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 1b89da8d9b..4a85f91e59 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -56,7 +56,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, bool preflight_check_running) override; const char *name() const override { return "UUV"; }