diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 1f00ac57e7..03681c55f0 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -102,7 +102,7 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo } } -void ActuatorEffectiveness::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) +void ActuatorEffectiveness::overrideCollectiveTilt(bool do_override, float collective_tilt) { // empty implementation to be overridden if needed. } diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 7e0a229ceb..e3648776f2 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -231,7 +231,7 @@ public: * @param collective_tilt Collective tilt normalized setpoint, in [0, 1]. 0: vertical, 1: horizontal. * @param collective_thrust Collective thrust normalized setpoint, in [0, 1]. */ - virtual void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust); + virtual void overrideCollectiveTilt(bool do_override, float collective_tilt); protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a56e3405cd..c2894dbcc0 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -504,6 +504,7 @@ void ControlAllocator::preflight_check_handle_command(hrt_abstime now) // currently this does not check prearmed status. if not prearmed, it will just do nothing. preflight_check_start(vehicle_command, now); result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; + } else { result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; PX4_WARN("Control surface preflight check rejected (not prearmed)"); @@ -632,7 +633,7 @@ void ControlAllocator::preflight_check_handle_tilt_control(hrt_abstime now) if (is_tiltrotor) { float modified_tilt_control = math::constrain(_preflight_check_input, 0.f, 1.f); - _actuator_effectiveness->setBypassTiltrotorControls(true, modified_tilt_control, 0.0f); + _actuator_effectiveness->overrideCollectiveTilt(true, modified_tilt_control); } else { // Commanded collective tilt axis but the vehicle is not a tiltrotor. Abort @@ -647,7 +648,7 @@ void ControlAllocator::preflight_check_handle_tilt_control(hrt_abstime now) // strictly speaking this is only necessary if is_tiltrotor but // can't hurt to call it always in case other // ActuatorEffectiveness* classes implement similar things - _actuator_effectiveness->setBypassTiltrotorControls(false, 0.0f, 0.0f); + _actuator_effectiveness->overrideCollectiveTilt(false, 0.0f); } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index ea99ce4232..478afc2659 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -124,22 +124,20 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt } -void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt, - float collective_thrust) +void ActuatorEffectivenessTiltrotorVTOL::overrideCollectiveTilt(bool do_override, float collective_tilt) { - // if _bypass_tiltrotor_controls (used for control surface preflight + // if _do_override_collective_tilt (used for control surface preflight // check), we alter the behaviour of processTiltrotorControls 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 using setBypassTiltrotorControls + // before calling this function using overrideCollectiveTilt // - collective tilt is added to actuator_sp even if // (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) // evaluates to false - _bypass_tiltrotor_controls = bypass; + _do_override_collective_tilt = do_override; _collective_tilt_normalized_setpoint = collective_tilt; - _collective_thrust_normalized_setpoint = collective_thrust; } void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp, @@ -149,14 +147,13 @@ void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector { tiltrotor_extra_controls_s tiltrotor_extra_controls; - if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _bypass_tiltrotor_controls) { + if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _do_override_collective_tilt) { 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 (_bypass_tiltrotor_controls) { + if (_do_override_collective_tilt) { 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 @@ -190,7 +187,7 @@ void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) { // as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover) - if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _bypass_tiltrotor_controls) { + if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _do_override_collective_tilt) { actuator_sp(i + _first_tilt_idx) += control_collective_tilt; } else { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 3e3a899991..e8c7f6275b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -88,7 +88,7 @@ public: void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; - void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) override; + void overrideCollectiveTilt(bool do_override, float collective_tilt) override; void processTiltrotorControls(ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, @@ -121,9 +121,8 @@ protected: uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; - bool _bypass_tiltrotor_controls{false}; + bool _do_override_collective_tilt{false}; float _collective_tilt_normalized_setpoint{0.5f}; - float _collective_thrust_normalized_setpoint{0.0f}; private: