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

control_allocator: generic motor configuration for standard vtols 

This adds the pusher/puller to the standard motors and makes the axis
configurable.
This commit is contained in:
Beat Küng 2022-02-16 11:05:10 +01:00
parent b2dc9ee710
commit 76d8d8cae6
14 changed files with 66 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
View File

@ -10,7 +10,7 @@
# param set-default SYS_CTRL_ALLOC 1
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_PX 0.1515
param set-default CA_ROTOR0_PY 0.245
param set-default CA_ROTOR0_KM 0.05
@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.1515
param set-default CA_ROTOR3_PY 0.1875
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_AX 1.0
param set-default CA_ROTOR4_AZ 0.0
param set-default CA_ROTOR4_PX 0.2
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TYPE 1

5
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
View File

@ -62,7 +62,7 @@ param set-default VT_FW_MOT_OFFID 1234
param set-default VT_TYPE 2
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_PX 0.15
param set-default CA_ROTOR0_PY 0.15
param set-default CA_ROTOR1_PX -0.15
@ -73,6 +73,9 @@ param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_AX 1.0
param set-default CA_ROTOR4_AZ 0.0
param set-default CA_ROTOR4_PX 0.2
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS1_TRQ_R 0.5

5
ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
View File

@ -25,7 +25,7 @@
. ${R}etc/init.d/rc.vtol_defaults
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_PX 0.15
param set-default CA_ROTOR0_PY 0.15
param set-default CA_ROTOR1_PX -0.15
@ -36,6 +36,9 @@ param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_AX 1.0
param set-default CA_ROTOR4_AZ 0.0
param set-default CA_ROTOR4_PX 0.2
param set-default CA_SV_CS_COUNT 4
param set-default CA_SV_CS0_TYPE 1
param set-default CA_SV_CS0_TRQ_R -0.5

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
View File

@ -49,7 +49,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
}
// motors
_motors.enableYawControl(false);
_motors.enablePropellerTorque(false);
const bool motors_added_successfully = _motors.addActuators(configuration);
// Torque

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
View File

@ -51,7 +51,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
}
// Motors
_rotors.enableYawControl(false);
_rotors.enablePropellerTorque(false);
const bool rotors_added_successfully = _rotors.addActuators(configuration);
// Control Surfaces

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
View File

@ -51,7 +51,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
}
// MC motors
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
// Tilts

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

@ -185,10 +185,18 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
float ct = geometry.rotors[i].thrust_coef;
float km = geometry.rotors[i].moment_ratio;
if (geometry.yaw_disabled) {
if (geometry.propeller_torque_disabled) {
km = 0.f;
}
if (geometry.propeller_torque_disabled_non_upwards) {
bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f;
if (!upwards) {
km = 0.f;
}
}
if (fabsf(ct) < FLT_EPSILON) {
continue;
}
@ -239,3 +247,18 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di
Vector3f axis{0.f, 0.f, -1.f};
return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
}
uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
{
uint32_t upwards_motors = 0;
for (int i = 0; i < _geometry.num_rotors; ++i) {
const Vector3f &axis = _geometry.rotors[i].axis;
if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
upwards_motors |= 1u << i;
}
}
return upwards_motors;
}

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

@ -73,7 +73,8 @@ public:
struct Geometry {
RotorGeometry rotors[NUM_ROTORS_MAX];
int num_rotors{0};
bool yaw_disabled{false};
bool propeller_torque_disabled{false};
bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
};
ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
@ -113,7 +114,11 @@ public:
*/
static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
void enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; }
void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; }
void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
uint32_t getUpwardsMotors() const;
private:
void updateParams() override;

16
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
View File

@ -37,7 +37,7 @@
using namespace matrix;
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent)
: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
: ModuleParams(parent), _rotors(this), _control_surfaces(this)
{
}
@ -49,17 +49,11 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
return false;
}
// MC motors
// Motors
configuration.selected_matrix = 0;
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
// Pusher/Puller
configuration.selected_matrix = 1;
for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) {
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
}
_rotors.enablePropellerTorqueNonUpwards(false);
const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
_mc_motors_mask = _rotors.getUpwardsMotors();
// Control Surfaces
configuration.selected_matrix = 1;

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

@ -80,7 +80,7 @@ public:
uint32_t getStoppedMotors() const override { return _stopped_motors; }
private:
ActuatorEffectivenessRotors _mc_rotors;
ActuatorEffectivenessRotors _rotors;
ActuatorEffectivenessControlSurfaces _control_surfaces;
uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
@ -90,8 +90,4 @@ private:
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_STDVTOL_N_P>) _param_ca_stdvtol_n_p ///< number of pushers
)
};

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
View File

@ -56,7 +56,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
// MC motors
configuration.selected_matrix = 0;
_mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
_mc_rotors.enablePropellerTorque(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
// Control Surfaces

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

@ -60,7 +60,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
// MC motors
configuration.selected_matrix = 0;
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
// Update matrix with tilts in vertical position when update is triggered by a manual
// configuration (parameter) change. This is to make sure the normalization

27
src/modules/control_allocator/module.yaml
View File

@ -370,18 +370,6 @@ parameters:
instance_start: 0
default: 0
# Standard VTOL
CA_STDVTOL_N_P:
description:
short: Number of fixed wing (pusher/puller) motors
type: enum
values:
1: '1'
2: '2'
3: '3'
4: '4'
default: 1
# Mixer
mixer:
@ -525,7 +513,7 @@ mixer:
title: 'Standard VTOL'
actuators:
- actuator_type: 'motor'
group_label: 'MC Motors'
group_label: 'Motors'
count: 'CA_ROTOR_COUNT'
per_item_parameters:
standard:
@ -535,10 +523,15 @@ mixer:
label: 'Direction CCW'
function: 'spin-dir'
show_as: 'true-if-positive'
- actuator_type: 'motor'
group_label: 'FW Motors'
count: 'CA_STDVTOL_N_P'
item_label_prefix: 'FW Motor ${i}'
- name: 'CA_ROTOR${i}_AX'
label: 'Axis X'
function: 'axisx'
- name: 'CA_ROTOR${i}_AY'
label: 'Axis Y'
function: 'axisy'
- name: 'CA_ROTOR${i}_AZ'
label: 'Axis Z'
function: 'axisz'
- actuator_type: 'servo'
group_label: 'Control Surfaces'
count: 'CA_SV_CS_COUNT'

4
src/modules/vtol_att_control/standard.cpp
View File

@ -427,13 +427,13 @@ void Standard::fill_actuator_outputs()
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;