fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Beat Küng
2021-11-23 12:40:22 -05:00
committed by Daniel Agar
parent dbc889a2ae
commit f99c2617ad
10 changed files with 152 additions and 588 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
+5 -3
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;
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
+8 -5
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
)
};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c
-560
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);