diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 669a4b8af9..bdb6a6d73c 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -77,6 +77,7 @@ uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVL uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal. uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)| +uint16 VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK = 309 # Do control surface preflight check. Axis|value [-1, 1]|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function| uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm. @@ -177,6 +178,12 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 +# param1 in VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK +uint8 AXIS_ROLL = 0 +uint8 AXIS_PITCH = 1 +uint8 AXIS_YAW = 2 +uint8 AXIS_COLLECTIVE_TILT = 3 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index b36da8230f..1f00ac57e7 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -101,3 +101,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo } } } + +void ActuatorEffectiveness::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) +{ + // 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 0ddd4988f3..7e0a229ceb 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -219,6 +219,20 @@ public: */ virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + /** + * If overridden by derived classes, optionally bypass the info usually + * contained in tiltrotor_extra_controls -- normalised collective thrust + * and tilt setpoints. + * + * Base class implementation is empty. + * + * @param bypass Flag indicating whether or not to use the other + * arguments to bypass setpoints + * @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); + protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; uint32_t _stopped_motors_mask{0}; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0e183f4ff9..dcf92b0a51 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -302,6 +302,38 @@ int Commander::custom_command(int argc, char *argv[]) return 0; } + if (!strcmp(argv[0], "cs_check")) { + + // hardcoded values. intended just for testing, remove before PR. + + if (argc > 1) { + if (!strcmp(argv[1], "roll")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_ROLL, 1.0); + + } else if (!strcmp(argv[1], "pitch")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_PITCH, 1.0); + + } else if (!strcmp(argv[1], "yaw")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_YAW, 1.0); + + } else if (!strcmp(argv[1], "tilt")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_COLLECTIVE_TILT, + 1.0); + + } else { + PX4_ERR("argument %s unsupported.", argv[1]); + return 1; + } + + return 0; + + } else { + PX4_ERR("missing argument"); + } + + return 0; + } + if (!strcmp(argv[0], "arm")) { float param2 = 0.f; @@ -1517,6 +1549,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER: case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE: case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION: + case vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK: /* ignore commands that are handled by other parts of the system */ break; @@ -3012,6 +3045,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false); PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false); PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); + PRINT_MODULE_USAGE_COMMAND_DESCR("cs_check", "Run control surface preflight checks"); PRINT_MODULE_USAGE_COMMAND("arm"); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); PRINT_MODULE_USAGE_COMMAND("disarm"); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a7e43a64e3..6c20a910d9 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -46,6 +46,8 @@ #include #include +#include + using namespace matrix; using namespace time_literals; @@ -340,6 +342,10 @@ ControlAllocator::Run() _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_armed) { + preflight_check_abort(); + } + ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; // Check if the current flight phase is HOVER or FIXED_WING @@ -373,6 +379,8 @@ ControlAllocator::Run() } } + preflight_check_handle_command(); + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -424,12 +432,18 @@ ControlAllocator::Run() } } + preflight_check_update_state(); + preflight_check_overwrite_torque_sp(c); + for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setControlSetpoint(c[i]); // Do allocation _control_allocation[i]->allocate(); + + preflight_check_handle_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()); @@ -460,6 +474,171 @@ ControlAllocator::Run() perf_end(_loop_perf); } +void ControlAllocator::preflight_check_handle_command() +{ + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.update(&vehicle_command)) { + + uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK) { + if (!_armed) { + // currently this does not check prearmed status. if not prearmed, it will just do nothing. + // should we output some sort of mild warning in that case? + + preflight_check_start(vehicle_command); + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; + + } else { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + PX4_INFO("Control surface preflight check rejected (armed)"); + } + + preflight_check_send_ack(result); + } + } +} + +void ControlAllocator::preflight_check_start(vehicle_command_s &cmd) +{ + + // If one is running, abort it. Depending on the use case we may prefer + // to instead "silently" overwrite the value. + if (_preflight_check_running) { + preflight_check_abort(); + } + + int axis = (uint8_t) lroundf(cmd.param1); + float input = cmd.param2; + + _preflight_check_running = true; + _preflight_check_axis = axis; + _preflight_check_input = input; + _preflight_check_started = hrt_absolute_time(); + _last_preflight_check_command = cmd; +} + +void ControlAllocator::preflight_check_send_ack(uint8_t result) +{ + + hrt_abstime now = hrt_absolute_time(); + + if (_last_preflight_check_command.from_external) { + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = now; + command_ack.command = _last_preflight_check_command.command; + command_ack.result = result; + command_ack.target_system = _last_preflight_check_command.source_system; + command_ack.target_component = _last_preflight_check_command.source_component; + + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); + } + + _preflight_check_last_ack = now; +} + +void ControlAllocator::preflight_check_abort() +{ + _preflight_check_running = false; + preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED); + +} + +void ControlAllocator::preflight_check_finish() +{ + _preflight_check_running = false; + preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); +} + + +void ControlAllocator::preflight_check_update_state() +{ + static uint32_t PREFLIGHT_CHECK_DURATION = 500_ms; + static uint32_t PREFLIGHT_CHECK_ACK_PERIOD = 1000_ms; + + if (_preflight_check_running) { + hrt_abstime now = hrt_absolute_time(); + + if (now - _preflight_check_started >= PREFLIGHT_CHECK_DURATION) { + // alternative: never stop it here and instead let the GS send a stop message? + preflight_check_finish(); + } + + if (now - _preflight_check_last_ack >= PREFLIGHT_CHECK_ACK_PERIOD) { + preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS); + } + } +} + +void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector + (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) +{ + if (!_preflight_check_running) { return; } + + int axis = -1; + + switch (_preflight_check_axis) { + case vehicle_command_s::AXIS_ROLL: + axis = 0; + break; + + case vehicle_command_s::AXIS_PITCH: + axis = 1; + break; + + case vehicle_command_s::AXIS_YAW: + axis = 2; + break; + + default: + // If none of roll, pitch, yaw, we do nothing here + return; + } + + c[0](0) = 0.; + c[0](1) = 0.; + c[0](2) = 0.; + c[0](axis) = _preflight_check_input; + + if (_num_control_allocation > 1) { + c[1](0) = 0.; + c[1](1) = 0.; + c[1](2) = 0.; + c[1](axis) = _preflight_check_input; + } +} + +void ControlAllocator::preflight_check_handle_tilt_control() +{ + bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; + + if (_preflight_check_running) { + + if (_preflight_check_axis == vehicle_command_s::AXIS_COLLECTIVE_TILT) { + + 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); + + } else { + // Commanded collective tilt axis but the vehicle is not a tiltrotor. Abort + _preflight_check_running = false; + preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); + + } + } + + } else { + // 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); + } +} + void ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index ce92c5036b..dee5c8e25a 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -77,6 +77,8 @@ #include #include #include +#include +#include #include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -138,6 +140,15 @@ private: void publish_actuator_controls(); + void preflight_check_overwrite_torque_sp(matrix::Vector (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]); + void preflight_check_handle_command(); + void preflight_check_update_state(); + void preflight_check_handle_tilt_control(); + void preflight_check_start(vehicle_command_s &cmd); + void preflight_check_send_ack(uint8_t result); + void preflight_check_abort(); + void preflight_check_finish(); + AllocationMethod _allocation_method_id{AllocationMethod::NONE}; ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations int _num_control_allocation{0}; @@ -180,6 +191,8 @@ 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 _vehicle_command_sub{ORB_ID(vehicle_command)}; + // Outputs uORB::PublicationMulti _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)}; @@ -201,6 +214,14 @@ private: // For example, the system might report two motor failures, but only the first one is handled by CA uint16_t _handled_motor_failure_bitmask{0}; + bool _preflight_check_running{false}; + + uint8_t _preflight_check_axis{0}; + float _preflight_check_input{0.0f}; + vehicle_command_s _last_preflight_check_command{0}; + hrt_abstime _preflight_check_started{0}; + hrt_abstime _preflight_check_last_ack{0}; + perf_counter_t _loop_perf; /**< loop duration performance counter */ bool _armed{false}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 93ac6783d6..ea99ce4232 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -124,88 +124,121 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt } +void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt, + float collective_thrust) +{ + // if _bypass_tiltrotor_controls (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 + // - collective tilt is added to actuator_sp even if + // (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) + // evaluates to false + + _bypass_tiltrotor_controls = bypass; + _collective_tilt_normalized_setpoint = collective_tilt; + _collective_thrust_normalized_setpoint = collective_thrust; +} + +void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp, + const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) + +{ + tiltrotor_extra_controls_s tiltrotor_extra_controls; + + if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _bypass_tiltrotor_controls) { + + 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) { + 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; + control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt; + + // initialize _last_collective_tilt_control + if (!PX4_ISFINITE(_last_collective_tilt_control)) { + _last_collective_tilt_control = control_collective_tilt; + + } else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) { + _collective_tilt_updated = true; + _last_collective_tilt_control = control_collective_tilt; + } + + // During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness + // of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max. + // Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid + // a thrust spike when the transition is initiated (as then the tilt is fully forward). + if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) { + _last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f); + + } else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) { + _last_collective_tilt_control = -1.f; + } + + bool yaw_saturated_positive = true; + bool yaw_saturated_negative = true; + + for (int i = 0; i < _tilts.count(); ++i) { + 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) { + actuator_sp(i + _first_tilt_idx) += control_collective_tilt; + + } else { + actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position + } + } + + // custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit + if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; + + // in FW directly use throttle sp + if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { + for (int i = 0; i < _first_tilt_idx; ++i) { + actuator_sp(i) = control_collective_thrust; + } + } + } +} + void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) { - // apply tilt if (matrix_index == 0) { - tiltrotor_extra_controls_s tiltrotor_extra_controls; - if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) { - float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f; - - // 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_collective_tilt_control - if (!PX4_ISFINITE(_last_collective_tilt_control)) { - _last_collective_tilt_control = control_collective_tilt; - - } else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) { - _collective_tilt_updated = true; - _last_collective_tilt_control = control_collective_tilt; - } - - // During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness - // of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max. - // Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid - // a thrust spike when the transition is initiated (as then the tilt is fully forward). - if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) { - _last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f); - - } else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) { - _last_collective_tilt_control = -1.f; - } - - bool yaw_saturated_positive = true; - bool yaw_saturated_negative = true; - - for (int i = 0; i < _tilts.count(); ++i) { - 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) { - actuator_sp(i + _first_tilt_idx) += control_collective_tilt; - - } else { - actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position - } - } - - // custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit - if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) { - - if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { - yaw_saturated_positive = false; - } - - if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { - yaw_saturated_negative = false; - } - - } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { - if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { - yaw_saturated_negative = false; - } - - if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { - yaw_saturated_positive = false; - } - } - } - - _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; - _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; - - // in FW directly use throttle sp - if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { - for (int i = 0; i < _first_tilt_idx; ++i) { - actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint; - } - } - } + processTiltrotorControls(actuator_sp, actuator_min, actuator_max); if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 512bd41f41..3e3a899991 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -88,6 +88,13 @@ public: void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) override; + + void processTiltrotorControls(ActuatorVector &actuator_sp, + const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max); + + protected: bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; @@ -114,6 +121,10 @@ protected: uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; + bool _bypass_tiltrotor_controls{false}; + float _collective_tilt_normalized_setpoint{0.5f}; + float _collective_thrust_normalized_setpoint{0.0f}; + private: void updateParams() override;