fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 01:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

FixedwingRateControl: rework VTOL differential thrust saturation

Browse Source 
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Matthias Grob
2023-06-19 10:59:20 +02:00
committed by Silvan Fuhrer
co-authored by Silvan Fuhrer
parent 53b9e66c66
commit 69aebe650b
2 changed files with 39 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+30 -15
View File
@@ -278,26 +278,41 @@ void FixedwingRateControl::Run()
_rate_control.resetIntegral(); _rate_control.resetIntegral();
} }
// update saturation status from control allocation feedback // Update saturation status from control allocation feedback
// TODO: send the unallocated value directly for better anti-windup
Vector3<bool> diffthr_enabled(
_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
);
if (_vehicle_status.is_vtol_tailsitter) {
// Swap roll and yaw
diffthr_enabled.swapRows(0, 2);
}
// saturation handling for axis controlled by differential thrust (VTOL only)
control_allocator_status_s control_allocator_status; control_allocator_status_s control_allocator_status;
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { // Set saturation flags for VTOL differential thrust feature
Vector<bool, 3> saturation_positive; // If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors.
Vector<bool, 3> saturation_negative; if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
for (size_t i = 0; i < 3; i++) {
if (!control_allocator_status.torque_setpoint_achieved) { if (diffthr_enabled(i)) {
for (size_t i = 0; i < 3; i++) { _rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { _rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
} }
} }
}
// TODO: send the unallocated value directly for better anti-windup // Set saturation flags for control surface controlled axes
_rate_control.setSaturationStatus(saturation_positive, saturation_negative); if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
for (size_t i = 0; i < 3; i++) {
if (!diffthr_enabled(i)) {
_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
}
}
} }
/* bi-linear interpolation over airspeed for actuator trim scheduling */ /* bi-linear interpolation over airspeed for actuator trim scheduling */
src/modules/fw_rate_control/FixedwingRateControl.hpp
+9 -1
View File
@@ -144,6 +144,13 @@ private:
bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
// enum for bitmask of VT_FW_DIFTHR_EN parameter options
enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t {
YAW_BIT = (1 << 0),
ROLL_BIT = (1 << 1),
PITCH_BIT = (1 << 2),
};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max, (ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max, (ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
@@ -194,7 +201,8 @@ private:
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll, (ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw, (ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man (ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en
) )
RateControl _rate_control; ///< class for rate control calculations RateControl _rate_control; ///< class for rate control calculations