control_allocator: add module.yaml, add CA_MC_R_COUNT param & update defaults

This commit is contained in:
Beat Küng 2021-11-02 16:24:58 +01:00 committed by Daniel Agar
parent dbc889a2ae
commit f99c2617ad
10 changed files with 152 additions and 588 deletions

View File

@ -19,7 +19,6 @@ 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_METHOD 1
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0

View File

@ -37,7 +37,6 @@ 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_METHOD 1
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0

View File

@ -25,7 +25,6 @@ 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_METHOD 1
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0

View File

@ -21,7 +21,6 @@ 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_METHOD 1
param set-default CA_ACT0_MIN 0.0
param set-default CA_ACT1_MIN 0.0
param set-default CA_ACT2_MIN 0.0

View File

@ -127,6 +127,8 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
geometry.num_rotors = _param_ca_mc_r_count.get();
_num_actuators = computeEffectivenessMatrix(geometry, matrix);
return true;
}
@ -142,7 +144,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
effectiveness.setZero();
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
for (int i = 0; i < math::min(NUM_ROTORS_MAX, geometry.num_rotors); i++) {
// Get rotor axis
matrix::Vector3f axis(
@ -188,9 +190,9 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
effectiveness(j, i) = moment(j);
effectiveness(j + 3, i) = thrust(j);
}
num_actuators = i + 1;
}
num_actuators = geometry.num_rotors;
return num_actuators;
}

View File

@ -57,7 +57,7 @@ public:
static constexpr int NUM_ROTORS_MAX = 8;
typedef struct {
struct RotorGeometry {
float position_x;
float position_y;
float position_z;
@ -66,11 +66,12 @@ public:
float axis_z;
float thrust_coef;
float moment_ratio;
} RotorGeometry;
};
typedef struct {
struct MultirotorGeometry {
RotorGeometry rotors[NUM_ROTORS_MAX];
} MultirotorGeometry;
int num_rotors{0};
};
static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@ -153,6 +154,8 @@ private:
(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km
(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km,
(ParamInt<px4::params::CA_MC_R_COUNT>) _param_ca_mc_r_count
)
};

View File

@ -1,560 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessMultirotorParams.c
*
* Parameters for the actuator effectiveness of multirotors.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Position of rotor 0 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PX, 0.0);
/**
* Position of rotor 0 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PY, 0.0);
/**
* Position of rotor 0 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PZ, 0.0);
/**
* Axis of rotor 0 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AX, 0.0);
/**
* Axis of rotor 0 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AY, 0.0);
/**
* Axis of rotor 0 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0);
/**
* Thrust coefficient of rotor 0
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT0_MIN and CA_ACT0_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_CT, 0.0);
/**
* Moment coefficient of rotor 0
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.05);
/**
* Position of rotor 1 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PX, 0.0);
/**
* Position of rotor 1 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PY, 0.0);
/**
* Position of rotor 1 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PZ, 0.0);
/**
* Axis of rotor 1 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AX, 0.0);
/**
* Axis of rotor 1 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AY, 0.0);
/**
* Axis of rotor 1 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0);
/**
* Thrust coefficient of rotor 1
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT1_MIN and CA_ACT1_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_CT, 0.0);
/**
* Moment coefficient of rotor 1
*
* The moment coefficient if defined as Torque = KM * Thrust,
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.05);
/**
* Position of rotor 2 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PX, 0.0);
/**
* Position of rotor 2 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PY, 0.0);
/**
* Position of rotor 2 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PZ, 0.0);
/**
* Axis of rotor 2 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AX, 0.0);
/**
* Axis of rotor 2 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AY, 0.0);
/**
* Axis of rotor 2 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0);
/**
* Thrust coefficient of rotor 2
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT2_MIN and CA_ACT2_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_CT, 0.0);
/**
* Moment coefficient of rotor 2
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.05);
/**
* Position of rotor 3 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PX, 0.0);
/**
* Position of rotor 3 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PY, 0.0);
/**
* Position of rotor 3 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PZ, 0.0);
/**
* Axis of rotor 3 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AX, 0.0);
/**
* Axis of rotor 3 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AY, 0.0);
/**
* Axis of rotor 3 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0);
/**
* Thrust coefficient of rotor 3
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT3_MIN and CA_ACT3_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_CT, 0.0);
/**
* Moment coefficient of rotor 3
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.05);
/**
* Position of rotor 4 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PX, 0.0);
/**
* Position of rotor 4 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PY, 0.0);
/**
* Position of rotor 4 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PZ, 0.0);
/**
* Axis of rotor 4 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AX, 0.0);
/**
* Axis of rotor 4 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AY, 0.0);
/**
* Axis of rotor 4 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0);
/**
* Thrust coefficient of rotor 4
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT4_MIN and CA_ACT4_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_CT, 0.0);
/**
* Moment coefficient of rotor 4
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.05);
/**
* Position of rotor 5 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PX, 0.0);
/**
* Position of rotor 5 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PY, 0.0);
/**
* Position of rotor 5 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PZ, 0.0);
/**
* Axis of rotor 5 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AX, 0.0);
/**
* Axis of rotor 5 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AY, 0.0);
/**
* Axis of rotor 5 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0);
/**
* Thrust coefficient of rotor 5
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT5_MIN and CA_ACT5_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_CT, 0.0);
/**
* Moment coefficient of rotor 5
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.05);
/**
* Position of rotor 6 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PX, 0.0);
/**
* Position of rotor 6 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PY, 0.0);
/**
* Position of rotor 6 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PZ, 0.0);
/**
* Axis of rotor 6 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AX, 0.0);
/**
* Axis of rotor 6 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AY, 0.0);
/**
* Axis of rotor 6 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0);
/**
* Thrust coefficient of rotor 6
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT6_MIN and CA_ACT6_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_CT, 0.0);
/**
* Moment coefficient of rotor 6
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.05);
/**
* Position of rotor 7 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PX, 0.0);
/**
* Position of rotor 7 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PY, 0.0);
/**
* Position of rotor 7 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PZ, 0.0);
/**
* Axis of rotor 7 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AX, 0.0);
/**
* Axis of rotor 7 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AY, 0.0);
/**
* Axis of rotor 7 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0);
/**
* Thrust coefficient of rotor 7
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT7_MIN and CA_ACT7_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_CT, 0.0);
/**
* Moment coefficient of rotor 7
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.05);

View File

@ -43,6 +43,8 @@ px4_add_module(
SRCS
ControlAllocator.cpp
ControlAllocator.hpp
MODULE_CONFIG
module.yaml
DEPENDS
mathlib
ActuatorEffectiveness

View File

@ -40,30 +40,16 @@
*/
/**
* Airframe ID
*
* This is used to retrieve pre-computed control effectiveness matrix
*
* @min 0
* @max 2
* @value 0 Multirotor
* @value 1 Standard VTOL (WIP)
* @value 2 Tiltrotor VTOL (WIP)
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
/**
* Control allocation method
*
* @value 0 Pseudo-inverse with output clipping (default)
* @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, 0);
PARAM_DEFINE_INT32(CA_METHOD, 1);
/**
* Battery power level scaler

View File

@ -0,0 +1,135 @@
__max_num_mc_motors: &max_num_mc_motors 8
module_name: Control Allocation
parameters:
- group: Control Allocation
definitions:
CA_AIRFRAME:
description:
short: Airframe selection
long: |
Defines which mixer implementation to use.
Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.
type: enum
values:
0: Multirotor
1: Standard VTOL (WIP)
2: Tiltrotor VTOL (WIP)
default: 0
# MC rotors
CA_MC_R_COUNT:
description:
short: Total number of rotors
type: enum
values:
0: 0 Motors
1: 1 Motor
2: 2 Motors
3: 3 Motors
4: 4 Motors
5: 5 Motors
6: 6 Motors
7: 7 Motors
8: 8 Motors
default: 0
CA_MC_R${i}_PX:
description:
short: Position of rotor ${i} along X body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_mc_motors
min: -100
max: 100
default: 0.0
CA_MC_R${i}_PY:
description:
short: Position of rotor ${i} along Y body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_mc_motors
min: -100
max: 100
default: 0.0
CA_MC_R${i}_PZ:
description:
short: Position of rotor ${i} along Z body axis
type: float
decimal: 2
increment: 0.1
unit: m
num_instances: *max_num_mc_motors
min: -100
max: 100
default: 0.0
CA_MC_R${i}_AX:
description:
short: Axis of rotor ${i} thrust vector, X body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_mc_motors
min: -100
max: 100
default: 0.0
CA_MC_R${i}_AY:
description:
short: Axis of rotor ${i} thrust vector, Y body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_mc_motors
min: -100
max: 100
default: 0.0
CA_MC_R${i}_AZ:
description:
short: Axis of rotor ${i} thrust vector, Z body axis component
long: Only the direction is considered (the vector is normalized).
type: float
decimal: 2
increment: 0.1
num_instances: *max_num_mc_motors
min: -100
max: 100
default: -1.0
CA_MC_R${i}_CT:
description:
short: Thrust coefficient of rotor ${i}
long: |
The thrust coefficient if defined as Thrust = CT * u^2,
where u (with value between actuator minimum and maximum)
is the output signal sent to the motor controller.
type: float
decimal: 1
increment: 1
num_instances: *max_num_mc_motors
min: 0
max: 100
default: 6.5
CA_MC_R${i}_KM:
description:
short: Moment coefficient of rotor ${i}
long: |
The moment coefficient if defined as Torque = KM * Thrust.
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
type: float
decimal: 3
increment: 0.01
num_instances: *max_num_mc_motors
min: -1
max: 1
default: 0.05