dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TiltrotorVTOL: allow stopping front tilted motors in fast forward flight

This commit is contained in:
Matthias Grob 2023-04-28 13:33:59 +02:00
parent 66b451e61f
commit 0be6069b83
4 changed files with 22 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
View File

@ -265,6 +265,17 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di
return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
}
uint32_t ActuatorEffectivenessRotors::getMotors() const
{
uint32_t motors = 0;
for (int i = 0; i < _geometry.num_rotors; ++i) {
motors |= 1u << i;
}
return motors;
}
uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
{
uint32_t upwards_motors = 0;

1
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp
View File

@ -126,6 +126,7 @@ public:
void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
uint32_t getMotors() const;
uint32_t getUpwardsMotors() const;
uint32_t getForwardsMotors() const;

13
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -68,10 +68,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
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];
_untiltable_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);
_motors = _mc_rotors.getMotors();
// Control Surfaces
configuration.selected_matrix = 1;
@ -118,7 +119,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
{
// apply tilt
if (matrix_index == 0) {
tiltrotor_extra_controls_s tiltrotor_extra_controls;
if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
@ -145,12 +145,15 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
// 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;
}
}
}
if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
}
}
// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
@ -180,7 +183,7 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
// update stopped motors
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors_mask = _nontilted_motors;
_stopped_motors_mask |= _untiltable_motors;
break;
case FlightPhase::HOVER_FLIGHT:

6
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
View File

@ -84,8 +84,6 @@ public:
const char *name() const override { return "VTOL Tiltrotor"; }
uint32_t getStoppedMotors() const override { return _stopped_motors; }
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
protected:
@ -94,8 +92,8 @@ protected:
ActuatorEffectivenessControlSurfaces _control_surfaces;
ActuatorEffectivenessTilts _tilts;
uint32_t _nontilted_motors{}; ///< motors that are not tiltable
uint32_t _stopped_motors{}; ///< currently stopped motors
uint32_t _motors{};
uint32_t _untiltable_motors{};
int _first_control_surface_idx{0}; ///< applies to matrix 1
int _first_tilt_idx{0}; ///< applies to matrix 0