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

control_allocator: remove min/max/scale params

This commit is contained in:
Beat Küng 2021-11-11 11:39:40 +01:00 committed by Daniel Agar
parent b29d9db7f1
commit 2809dd8944
10 changed files with 24 additions and 463 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc
View File

@ -11,38 +11,21 @@
param set-default SYS_CTRL_ALLOC 1
param set-default MPC_USE_HTE 0
param set-default VM_MASS 1.5
param set-default VM_INERTIA_XX 0.03
param set-default VM_INERTIA_YY 0.03
param set-default VM_INERTIA_ZZ 0.05
param set-default CA_AIRFRAME 0
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0
param set-default CA_ACT3_MIN 0.0
param set-default CA_ACT0_MAX 1.0
param set-default CA_ACT1_MAX 1.0
param set-default CA_ACT2_MAX 1.0
param set-default CA_ACT3_MAX 1.0
param set-default CA_MC_R_COUNT 4
param set-default CA_MC_R0_PX 0.1515
param set-default CA_MC_R0_PY 0.245
param set-default CA_MC_R0_CT 6.5
param set-default CA_MC_R0_KM 0.05
param set-default CA_MC_R1_PX -0.1515
param set-default CA_MC_R1_PY -0.1875
param set-default CA_MC_R1_CT 6.5
param set-default CA_MC_R1_KM 0.05
param set-default CA_MC_R2_PX 0.1515
param set-default CA_MC_R2_PY -0.245
param set-default CA_MC_R2_CT 6.5
param set-default CA_MC_R2_KM -0.05
param set-default CA_MC_R3_PX -0.1515
param set-default CA_MC_R3_PY 0.1875
param set-default CA_MC_R3_CT 6.5
param set-default CA_MC_R3_KM -0.05
param set-default PWM_MAIN_FUNC1 101

26
ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc
View File

@ -29,50 +29,26 @@ param set-default TRIG_MODE 4
param set-default MNT_MODE_IN 0
param set-default MAV_PROTO_VER 2
param set-default MPC_USE_HTE 0
param set-default VM_MASS 2.66
param set-default VM_INERTIA_XX 0.06
param set-default VM_INERTIA_YY 0.06
param set-default VM_INERTIA_ZZ 0.10
param set-default CA_AIRFRAME 0
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0
param set-default CA_ACT3_MIN 0.0
param set-default CA_ACT4_MIN 0.0
param set-default CA_ACT5_MIN 0.0
param set-default CA_ACT0_MAX 1.0
param set-default CA_ACT1_MAX 1.0
param set-default CA_ACT2_MAX 1.0
param set-default CA_ACT3_MAX 1.0
param set-default CA_ACT4_MAX 1.0
param set-default CA_ACT5_MAX 1.0
param set-default CA_MC_R_COUNT 6
param set-default CA_MC_R0_PX 0.0
param set-default CA_MC_R0_PY 1.0
param set-default CA_MC_R0_CT 9.5
param set-default CA_MC_R0_KM -0.05
param set-default CA_MC_R1_PX 0.0
param set-default CA_MC_R1_PY -1.0
param set-default CA_MC_R1_CT 9.5
param set-default CA_MC_R1_KM 0.05
param set-default CA_MC_R2_PX 0.866025
param set-default CA_MC_R2_PY -0.5
param set-default CA_MC_R2_CT 9.5
param set-default CA_MC_R2_KM -0.05
param set-default CA_MC_R3_PX -0.866025
param set-default CA_MC_R3_PY 0.5
param set-default CA_MC_R3_CT 9.5
param set-default CA_MC_R3_KM 0.05
param set-default CA_MC_R4_PX 0.866025
param set-default CA_MC_R4_PY 0.5
param set-default CA_MC_R4_CT 9.5
param set-default CA_MC_R4_KM 0.05
param set-default CA_MC_R5_PX -0.866025
param set-default CA_MC_R5_PY -0.5
param set-default CA_MC_R5_CT 9.5
param set-default CA_MC_R5_KM -0.05
param set-default PWM_MAIN_FUNC1 101

20
ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc
View File

@ -17,38 +17,20 @@ set MIXER_AUX none
param set-default SYS_CTRL_ALLOC 1
param set-default MPC_USE_HTE 0
param set-default VM_MASS 1.5
param set-default VM_INERTIA_XX 0.03
param set-default VM_INERTIA_YY 0.03
param set-default VM_INERTIA_ZZ 0.05
param set-default CA_AIRFRAME 0
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0
param set-default CA_ACT3_MIN 0.0
param set-default CA_ACT0_MAX 1.0
param set-default CA_ACT1_MAX 1.0
param set-default CA_ACT2_MAX 1.0
param set-default CA_ACT3_MAX 1.0
param set-default CA_MC_R_COUNT 4
param set-default CA_MC_R0_PX 0.177
param set-default CA_MC_R0_PY 0.177
param set-default CA_MC_R0_CT 6.5
param set-default CA_MC_R0_KM 0.05
param set-default CA_MC_R1_PX -0.177
param set-default CA_MC_R1_PY -0.177
param set-default CA_MC_R1_CT 6.5
param set-default CA_MC_R1_KM 0.05
param set-default CA_MC_R2_PX 0.177
param set-default CA_MC_R2_PY -0.177
param set-default CA_MC_R2_CT 6.5
param set-default CA_MC_R2_KM -0.05
param set-default CA_MC_R3_PX -0.177
param set-default CA_MC_R3_PY 0.177
param set-default CA_MC_R3_CT 6.5
param set-default CA_MC_R3_KM -0.05
param set-default PWM_MAIN_FUNC1 101

26
ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc
View File

@ -13,56 +13,32 @@
. ${R}etc/init.d/rc.mc_defaults
param set-default SYS_CTRL_ALLOC 1
param set-default MPC_USE_HTE 0
param set-default VM_MASS 1.5
param set-default VM_INERTIA_XX 0.03
param set-default VM_INERTIA_YY 0.03
param set-default VM_INERTIA_ZZ 0.05
param set-default CA_AIRFRAME 0
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0
param set-default CA_ACT3_MIN 0.0
param set-default CA_ACT4_MIN 0.0
param set-default CA_ACT5_MIN 0.0
param set-default CA_ACT0_MAX 1.0
param set-default CA_ACT1_MAX 1.0
param set-default CA_ACT2_MAX 1.0
param set-default CA_ACT3_MAX 1.0
param set-default CA_ACT4_MAX 1.0
param set-default CA_ACT5_MAX 1.0
param set-default CA_MC_R_COUNT 6
param set-default CA_MC_R0_PX 0.0
param set-default CA_MC_R0_PY 0.275
param set-default CA_MC_R0_CT 6.5
param set-default CA_MC_R0_KM -0.05
param set-default CA_MC_R1_PX 0.0
param set-default CA_MC_R1_PY -0.275
param set-default CA_MC_R1_CT 6.5
param set-default CA_MC_R1_KM 0.05
param set-default CA_MC_R2_PX 0.238
param set-default CA_MC_R2_PY -0.1375
param set-default CA_MC_R2_CT 6.5
param set-default CA_MC_R2_KM -0.05
param set-default CA_MC_R3_PX -0.238
param set-default CA_MC_R3_PY 0.1375
param set-default CA_MC_R3_CT 6.5
param set-default CA_MC_R3_KM 0.05
param set-default CA_MC_R4_PX 0.238
param set-default CA_MC_R4_PY 0.1375
param set-default CA_MC_R4_CT 6.5
param set-default CA_MC_R4_KM 0.05
param set-default CA_MC_R5_PX -0.238
param set-default CA_MC_R5_PY -0.1375
param set-default CA_MC_R5_CT 6.5
param set-default CA_MC_R5_KM -0.05
param set-default PWM_MAIN_FUNC1 101

7
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
View File

@ -41,6 +41,13 @@
#include "ControlAllocation.hpp"
ControlAllocation::ControlAllocation()
{
_control_allocation_scale.setAll(1.f);
_actuator_min.setAll(0.f);
_actuator_max.setAll(1.f);
}
void
ControlAllocation::setEffectivenessMatrix(
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,

2
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
View File

@ -74,7 +74,7 @@
class ControlAllocation
{
public:
ControlAllocation() { _control_allocation_scale.setAll(1.f); }
ControlAllocation();
virtual ~ControlAllocation() = default;
static constexpr uint8_t NUM_ACTUATORS = 16;

40
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -93,46 +93,6 @@ ControlAllocator::parameters_updated()
return;
}
// Minimum actuator values
matrix::Vector<float, NUM_ACTUATORS> actuator_min;
actuator_min(0) = _param_ca_act0_min.get();
actuator_min(1) = _param_ca_act1_min.get();
actuator_min(2) = _param_ca_act2_min.get();
actuator_min(3) = _param_ca_act3_min.get();
actuator_min(4) = _param_ca_act4_min.get();
actuator_min(5) = _param_ca_act5_min.get();
actuator_min(6) = _param_ca_act6_min.get();
actuator_min(7) = _param_ca_act7_min.get();
actuator_min(8) = _param_ca_act8_min.get();
actuator_min(9) = _param_ca_act9_min.get();
actuator_min(10) = _param_ca_act10_min.get();
actuator_min(11) = _param_ca_act11_min.get();
actuator_min(12) = _param_ca_act12_min.get();
actuator_min(13) = _param_ca_act13_min.get();
actuator_min(14) = _param_ca_act14_min.get();
actuator_min(15) = _param_ca_act15_min.get();
_control_allocation->setActuatorMin(actuator_min);
// Maximum actuator values
matrix::Vector<float, NUM_ACTUATORS> actuator_max;
actuator_max(0) = _param_ca_act0_max.get();
actuator_max(1) = _param_ca_act1_max.get();
actuator_max(2) = _param_ca_act2_max.get();
actuator_max(3) = _param_ca_act3_max.get();
actuator_max(4) = _param_ca_act4_max.get();
actuator_max(5) = _param_ca_act5_max.get();
actuator_max(6) = _param_ca_act6_max.get();
actuator_max(7) = _param_ca_act7_max.get();
actuator_max(8) = _param_ca_act8_max.get();
actuator_max(9) = _param_ca_act9_max.get();
actuator_max(10) = _param_ca_act10_max.get();
actuator_max(11) = _param_ca_act11_max.get();
actuator_max(12) = _param_ca_act12_max.get();
actuator_max(13) = _param_ca_act13_max.get();
actuator_max(14) = _param_ca_act14_max.get();
actuator_max(15) = _param_ca_act15_max.get();
_control_allocation->setActuatorMax(actuator_max);
_control_allocation->updateParameters();
update_effectiveness_matrix_if_needed(true);
}

36
src/modules/control_allocator/ControlAllocator.hpp
View File

@ -159,41 +159,7 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
(ParamBool<px4::params::CA_BAT_SCALE_EN>) _param_ca_bat_scale_en,
(ParamBool<px4::params::CA_AIR_SCALE_EN>) _param_ca_air_scale_en,
(ParamFloat<px4::params::CA_ACT0_MIN>) _param_ca_act0_min,
(ParamFloat<px4::params::CA_ACT1_MIN>) _param_ca_act1_min,
(ParamFloat<px4::params::CA_ACT2_MIN>) _param_ca_act2_min,
(ParamFloat<px4::params::CA_ACT3_MIN>) _param_ca_act3_min,
(ParamFloat<px4::params::CA_ACT4_MIN>) _param_ca_act4_min,
(ParamFloat<px4::params::CA_ACT5_MIN>) _param_ca_act5_min,
(ParamFloat<px4::params::CA_ACT6_MIN>) _param_ca_act6_min,
(ParamFloat<px4::params::CA_ACT7_MIN>) _param_ca_act7_min,
(ParamFloat<px4::params::CA_ACT8_MIN>) _param_ca_act8_min,
(ParamFloat<px4::params::CA_ACT9_MIN>) _param_ca_act9_min,
(ParamFloat<px4::params::CA_ACT10_MIN>) _param_ca_act10_min,
(ParamFloat<px4::params::CA_ACT11_MIN>) _param_ca_act11_min,
(ParamFloat<px4::params::CA_ACT12_MIN>) _param_ca_act12_min,
(ParamFloat<px4::params::CA_ACT13_MIN>) _param_ca_act13_min,
(ParamFloat<px4::params::CA_ACT14_MIN>) _param_ca_act14_min,
(ParamFloat<px4::params::CA_ACT15_MIN>) _param_ca_act15_min,
(ParamFloat<px4::params::CA_ACT0_MAX>) _param_ca_act0_max,
(ParamFloat<px4::params::CA_ACT1_MAX>) _param_ca_act1_max,
(ParamFloat<px4::params::CA_ACT2_MAX>) _param_ca_act2_max,
(ParamFloat<px4::params::CA_ACT3_MAX>) _param_ca_act3_max,
(ParamFloat<px4::params::CA_ACT4_MAX>) _param_ca_act4_max,
(ParamFloat<px4::params::CA_ACT5_MAX>) _param_ca_act5_max,
(ParamFloat<px4::params::CA_ACT6_MAX>) _param_ca_act6_max,
(ParamFloat<px4::params::CA_ACT7_MAX>) _param_ca_act7_max,
(ParamFloat<px4::params::CA_ACT8_MAX>) _param_ca_act8_max,
(ParamFloat<px4::params::CA_ACT9_MAX>) _param_ca_act9_max,
(ParamFloat<px4::params::CA_ACT10_MAX>) _param_ca_act10_max,
(ParamFloat<px4::params::CA_ACT11_MAX>) _param_ca_act11_max,
(ParamFloat<px4::params::CA_ACT12_MAX>) _param_ca_act12_max,
(ParamFloat<px4::params::CA_ACT13_MAX>) _param_ca_act13_max,
(ParamFloat<px4::params::CA_ACT14_MAX>) _param_ca_act14_max,
(ParamFloat<px4::params::CA_ACT15_MAX>) _param_ca_act15_max
(ParamInt<px4::params::CA_METHOD>) _param_ca_method
)
};

300
src/modules/control_allocator/control_allocator_params.c
View File

@ -1,300 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file control_allocator_params.c
*
* Parameters for the control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Control allocation method
*
* @value 0 Pseudo-inverse with output clipping
* @value 1 Pseudo-inverse with sequential desaturation technique
* @min 0
* @max 1
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_METHOD, 1);
/**
* Battery power level scaler
*
* This compensates for voltage drop of the battery over time by attempting to
* normalize performance across the operating range of the battery. The copter
* should constantly behave as if it was fully charged with reduced max acceleration
* at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
* it will still be 0.5 at 60% battery.
*
* @boolean
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_BAT_SCALE_EN, 0);
/**
* Airspeed scaler
*
* This compensates for the variation of flap effectiveness with airspeed.
*
* @boolean
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIR_SCALE_EN, 0);
/**
* Minimum value for actuator 0
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT0_MIN, 0.0);
/**
* Minimum value for actuator 1
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT1_MIN, 0.0);
/**
* Minimum value for actuator 2
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT2_MIN, 0.0);
/**
* Minimum value for actuator 3
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT3_MIN, 0.0);
/**
* Minimum value for actuator 4
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT4_MIN, 0.0);
/**
* Minimum value for actuator 5
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT5_MIN, 0.0);
/**
* Minimum value for actuator 6
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT6_MIN, 0.0);
/**
* Minimum value for actuator 7
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT7_MIN, 0.0);
/**
* Minimum value for actuator 8
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT8_MIN, 0.0);
/**
* Minimum value for actuator 9
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT9_MIN, 0.0);
/**
* Minimum value for actuator 10
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT10_MIN, 0.0);
/**
* Minimum value for actuator 11
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT11_MIN, 0.0);
/**
* Minimum value for actuator 12
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT12_MIN, 0.0);
/**
* Minimum value for actuator 13
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT13_MIN, 0.0);
/**
* Minimum value for actuator 14
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT14_MIN, 0.0);
/**
* Minimum value for actuator 15
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT15_MIN, 0.0);
/**
* Maximum value for actuator 0
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT0_MAX, 0.0);
/**
* Maximum value for actuator 1
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT1_MAX, 0.0);
/**
* Maximum value for actuator 2
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT2_MAX, 0.0);
/**
* Maximum value for actuator 3
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT3_MAX, 0.0);
/**
* Maximum value for actuator 4
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT4_MAX, 0.0);
/**
* Maximum value for actuator 5
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT5_MAX, 0.0);
/**
* Maximum value for actuator 6
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT6_MAX, 0.0);
/**
* Maximum value for actuator 7
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT7_MAX, 0.0);
/**
* Maximum value for actuator 8
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT8_MAX, 0.0);
/**
* Maximum value for actuator 9
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT9_MAX, 0.0);
/**
* Maximum value for actuator 10
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT10_MAX, 0.0);
/**
* Maximum value for actuator 11
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT11_MAX, 0.0);
/**
* Maximum value for actuator 12
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT12_MAX, 0.0);
/**
* Maximum value for actuator 13
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT13_MAX, 0.0);
/**
* Maximum value for actuator 14
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0);
/**
* Maximum value for actuator 15
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0);

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

@ -18,6 +18,17 @@ parameters:
2: Tiltrotor VTOL (WIP)
default: 0
CA_METHOD:
description:
short: Control allocation method
long: |
Selects the algorithm and desaturation method
type: enum
values:
0: Pseudo-inverse with output clipping
1: Pseudo-inverse with sequential desaturation technique
default: 1
# MC rotors
CA_MC_R_COUNT:
description: