fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Control Allocation: fixes in yaw saturation detection for vehicles with tilt-for-yaw (#21994)

Browse Source 
* ActuatorEffectiveness: base yaw saturation on tilt actuator limits (VTOL Tiltrotor)

* ActuatorEffectiveness: add custom yaw saturation for MC Tilt


---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-08-24 13:58:58 +02:00
committed by GitHub
parent 0eb276f273
commit 410206aa5d
4 changed files with 88 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
+48 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -55,7 +55,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
// Tilts
int first_tilt_idx = configuration.num_actuators_matrix[0];
_first_tilt_idx = configuration.num_actuators_matrix[0];
_tilts.updateTorqueSign(_mc_rotors.geometry());
const bool tilts_added_successfully = _tilts.addActuators(configuration);
@@ -69,7 +69,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
if (delta_angle > FLT_EPSILON) {
float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle;
_tilt_offsets(first_tilt_idx + i) = trim;
_tilt_offsets(_first_tilt_idx + i) = trim;
}
}
@@ -82,4 +82,49 @@ void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM
{
actuator_sp += _tilt_offsets;
// TODO: dynamic matrix update
bool yaw_saturated_positive = true;
bool yaw_saturated_negative = true;
for (int i = 0; i < _tilts.count(); ++i) {
// 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;
}
void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
{
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude is not used.
if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
status.unallocated_torque[2] = 1.f;
} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
status.unallocated_torque[2] = -1.f;
} else {
status.unallocated_torque[2] = 0.f;
}
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
+11 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -61,8 +61,18 @@ public:
const char *name() const override { return "MC Tilt"; }
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
protected:
ActuatorVector _tilt_offsets;
ActuatorEffectivenessRotors _mc_rotors;
ActuatorEffectivenessTilts _tilts;
int _first_tilt_idx{0};
struct YawTiltSaturationFlags {
bool tilt_yaw_pos;
bool tilt_yaw_neg;
};
YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+27 -15
View File
@@ -148,6 +148,9 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
_last_collective_tilt_control = control_collective_tilt;
}
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) {
@@ -159,8 +162,32 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
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) {
@@ -173,21 +200,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
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
// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
if (matrix_index == 0 && _tilts.hasYawControl()) {
_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
if (control_sp(2) < -1.f) {
_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
} else if (control_sp(2) > 1.f) {
_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
}
}
}
void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp
+2
View File
@@ -79,6 +79,8 @@ public:
bool hasYawControl() const;
float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); }
private:
void updateParams() override;