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

CA: enable differential thrust on matrix 1 in VTOL FW

Browse Source 
Introduce differential thrust SCALE and WEIGHT params that are used
to configure differential thrust on a VTOL vehicle.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-10 11:16:32 +02:00
parent 1695babe8a
commit 3370b4583a
11 changed files with 118 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert
-1
View File
@@ -21,7 +21,6 @@ param set-default VT_B_TRANS_DUR 5
param set-default VT_ELEV_MC_LOCK 0
param set-default VT_TYPE 0
param set-default VT_FW_DIFTHR_EN 1
param set-default VT_FW_DIFTHR_S_Y 0.3
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter
-1
View File
@@ -70,7 +70,6 @@ param set-default MC_PITCH_P 3
param set-default VT_ARSP_TRANS 10
param set-default VT_B_TRANS_DUR 5
param set-default VT_FW_DIFTHR_EN 1
param set-default VT_FW_DIFTHR_S_Y 1
param set-default VT_F_TRANS_DUR 1.5
param set-default VT_TYPE 0
ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter
-1
View File
@@ -67,7 +67,6 @@ param set-default MC_PITCHRATE_P 0.3
param set-default VT_ARSP_TRANS 15
param set-default VT_B_TRANS_DUR 5
param set-default VT_FW_DIFTHR_EN 7
param set-default VT_FW_DIFTHR_S_Y 1
param set-default VT_F_TRANS_DUR 1.5
param set-default VT_TYPE 0
ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
-1
View File
@@ -25,7 +25,6 @@ param set-default VT_ELEV_MC_LOCK 0
param set-default VT_MOT_COUNT 2
param set-default VT_TYPE 0
param set-default VT_FW_DIFTHR_EN 1
param set-default VT_FW_DIFTHR_S_Y 0.3
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
src/modules/control_allocator/ControlAllocator.cpp
+45 -3
View File
@@ -408,6 +408,17 @@ ControlAllocator::Run()
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
}
float fw_dthr_weight[3] = {_param_ca_fw_dthr_wgt_r.get(), _param_ca_fw_dthr_wgt_p.get(), _param_ca_fw_dthr_wgt_y.get()};
float fw_dthr_scale[3] = {_param_ca_fw_dthr_sc_r.get(), _param_ca_fw_dthr_sc_p.get(), _param_ca_fw_dthr_sc_y.get()};
// VTOL Tailsitters have roll/yaw axis swaped in fixed-wing mode
if (_effectiveness_source_id == EffectivenessSource::TAILSITTER_VTOL) {
fw_dthr_weight[2] = fw_dthr_weight[0];
fw_dthr_weight[0] = fw_dthr_weight[2];
fw_dthr_scale[2] = fw_dthr_scale[0];
fw_dthr_scale[0] = fw_dthr_scale[2];
}
// Set control setpoint vector(s)
matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
c[0](0) = _torque_sp(0);
@@ -418,10 +429,17 @@ ControlAllocator::Run()
c[0](5) = _thrust_sp(2);
if (_num_control_allocation > 1) {
matrix::Vector<float, NUM_AXES> vehicle_torque_setpoint_matrix_1;
if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) {
c[1](0) = vehicle_torque_setpoint.xyz[0];
c[1](1) = vehicle_torque_setpoint.xyz[1];
c[1](2) = vehicle_torque_setpoint.xyz[2];
vehicle_torque_setpoint_matrix_1(0) = vehicle_torque_setpoint.xyz[0];
vehicle_torque_setpoint_matrix_1(1) = vehicle_torque_setpoint.xyz[1];
vehicle_torque_setpoint_matrix_1(2) = vehicle_torque_setpoint.xyz[2];
// reduce outputs on servos (matrix 1) if differential thrust with motors (matrix 0) is enabled
c[1](0) = vehicle_torque_setpoint_matrix_1(0) * (1.f - fw_dthr_weight[0]);
c[1](1) = vehicle_torque_setpoint_matrix_1(1) * (1.f - fw_dthr_weight[1]);
c[1](2) = vehicle_torque_setpoint_matrix_1(2) * (1.f - fw_dthr_weight[2]);
}
if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) {
@@ -429,6 +447,30 @@ ControlAllocator::Run()
c[1](4) = vehicle_thrust_setpoint.xyz[1];
c[1](5) = vehicle_thrust_setpoint.xyz[2];
}
const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON
|| fw_dthr_weight[2] > FLT_EPSILON;
// VTOL differential thrust in FW
if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && has_non_zero_dthr_weight) {
/* CA_FW_DTHR_SC_R etc are scales to tune response of differential thrust around each axis.
The scaling factor is applied to the (normalized) torque setpoint from the rate controller going
to the motors for rate control in fixed-wing flight using differential thrust instead of
aerodynamic control surfaces.
Set this parameter such that when the control surfaces are disabled,
the systems rate tracking is maintained as best as possible through
differential thrust.
*/
c[0](0) = vehicle_torque_setpoint_matrix_1(0) * fw_dthr_scale[0] * fw_dthr_weight[0];
c[0](1) = vehicle_torque_setpoint_matrix_1(1) * fw_dthr_scale[1] * fw_dthr_weight[1];
c[0](2) = vehicle_torque_setpoint_matrix_1(2) * fw_dthr_scale[2] * fw_dthr_weight[2];
_actuator_effectiveness->setEnableAuxiliaryMotors(true);
} else if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && !has_non_zero_dthr_weight) {
_actuator_effectiveness->setEnableAuxiliaryMotors(false);
}
}
for (int i = 0; i < _num_control_allocation; ++i) {
src/modules/control_allocator/ControlAllocator.hpp
+7 -1
View File
@@ -217,7 +217,13 @@ private:
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::CA_FW_DTHR_SC_R>) _param_ca_fw_dthr_sc_r,
(ParamFloat<px4::params::CA_FW_DTHR_SC_P>) _param_ca_fw_dthr_sc_p,
(ParamFloat<px4::params::CA_FW_DTHR_SC_Y>) _param_ca_fw_dthr_sc_y,
(ParamFloat<px4::params::CA_FW_DTHR_WGT_R>) _param_ca_fw_dthr_wgt_r,
(ParamFloat<px4::params::CA_FW_DTHR_WGT_P>) _param_ca_fw_dthr_wgt_p,
(ParamFloat<px4::params::CA_FW_DTHR_WGT_Y>) _param_ca_fw_dthr_wgt_y
)
};
src/modules/control_allocator/module.yaml
+66 -1
View File
@@ -40,7 +40,7 @@ parameters:
If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
type: enum
values:
0: Pseudo-inverse with output clipping
0: Pseudo-inverse without any desaturation
1: Pseudo-inverse with sequential desaturation technique
2: Automatic
default: 2
@@ -541,6 +541,71 @@ parameters:
1: Remove first failed motor from effectiveness
default: 0
CA_FW_DTHR_SC_R:
description:
short: Fixed-wing differential thrust roll scale
type: float
decimal: 2
increment: 0.1
min: 0
max: 10
default: 1
CA_FW_DTHR_SC_P:
description:
short: Fixed-wing differential thrust pitch scale
type: float
decimal: 2
increment: 0.1
min: 0
max: 10
default: 1
CA_FW_DTHR_SC_Y:
description:
short: Fixed-wing differential thrust yaw scale
type: float
decimal: 2
increment: 0.1
min: 0
max: 10
default: 1
CA_FW_DTHR_WGT_R:
description:
short: Fixed-wing differential thrust roll weight
long: |
Weight of torque allocated through differential thrust vs control surface.
type: float
decimal: 2
increment: 0.1
min: 0
max: 1
default: 0
CA_FW_DTHR_WGT_P:
description:
short: Pitch weight of differential thrust in fixed-wing
long: |
Weight of torque allocated through differential thrust vs control surface.
type: float
decimal: 2
increment: 0.1
min: 0
max: 1
default: 0
CA_FW_DTHR_WGT_Y:
description:
short: Yaw weight of differential thrust in fixed-wing
long: |
Weight of torque allocated through differential thrust vs control surface.
type: float
decimal: 2
increment: 0.1
min: 0
max: 1
default: 0
# Mixer
mixer:
actuator_types:
src/modules/vtol_att_control/tailsitter.cpp
-13
View File
@@ -291,19 +291,6 @@ void Tailsitter::fill_actuator_outputs()
_thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0];
/* allow differential thrust if enabled */
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_y.get();
}
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * _param_vt_fw_difthr_s_p.get();
}
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get();
}
// for the short period after switching to FW where there is no thrust published yet from the FW controller,
// keep publishing the last MC thrust to keep the motors running
if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) {
src/modules/vtol_att_control/tiltrotor.cpp
-4
View File
@@ -379,10 +379,6 @@ void Tiltrotor::fill_actuator_outputs()
collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
/* allow differential thrust if enabled */
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ;
}
} else {
collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight;
src/modules/vtol_att_control/vtol_att_control_params.c
-55
View File
@@ -283,61 +283,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_HMAX, 0);
*/
PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
/**
* Differential thrust in forwards flight.
*
* Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode.
* The effectiveness of differential thrust around the corresponding axis can be
* tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y.
*
* @min 0
* @max 7
* @bit 0 Yaw
* @bit 1 Roll
* @bit 2 Pitch
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
/**
* Roll differential thrust factor in forward flight
*
* Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f);
/**
* Pitch differential thrust factor in forward flight
*
* Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f);
/**
* Yaw differential thrust factor in forward flight
*
* Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f);
/**
* Backtransition deceleration setpoint to pitch I gain.
*
src/modules/vtol_att_control/vtol_type.h
-10
View File
@@ -76,12 +76,6 @@ enum VtolForwardActuationMode {
ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND
};
// enum for bitmask of VT_FW_DIFTHR_EN parameter options
enum class VtFwDifthrEnBits : int32_t {
YAW_BIT = (1 << 0),
ROLL_BIT = (1 << 1),
PITCH_BIT = (1 << 2),
};
enum class QuadchuteReason {
None = 0,
@@ -344,10 +338,6 @@ protected:
(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
(ParamFloat<px4::params::VT_FW_DIFTHR_S_Y>) _param_vt_fw_difthr_s_y,
(ParamFloat<px4::params::VT_FW_DIFTHR_S_P>) _param_vt_fw_difthr_s_p,
(ParamFloat<px4::params::VT_FW_DIFTHR_S_R>) _param_vt_fw_difthr_s_r,
(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,