mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
control_allocator: replace ModuleParams with generated parameter strings as needed
This commit is contained in:
Daniel Agar
parent
51abb804ac
commit
eb362f2f63
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 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
|
||||
@ -54,78 +54,33 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
|
||||
_updated = false;
|
||||
|
||||
// Get multirotor geometry
|
||||
MultirotorGeometry geometry = {};
|
||||
geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
|
||||
geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
|
||||
geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
|
||||
geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
|
||||
geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
|
||||
geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
|
||||
geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
|
||||
geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
|
||||
MultirotorGeometry geometry{};
|
||||
|
||||
geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
|
||||
geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
|
||||
geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
|
||||
geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
|
||||
geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
|
||||
geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
|
||||
geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
|
||||
geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
|
||||
for (int n = 0; n < NUM_ROTORS_MAX; n++) {
|
||||
char buffer[17];
|
||||
|
||||
geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
|
||||
geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
|
||||
geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
|
||||
geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
|
||||
geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
|
||||
geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
|
||||
geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
|
||||
geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
|
||||
for (int i = 0; i < 3; i++) {
|
||||
char axis_char = 'X' + i;
|
||||
|
||||
geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
|
||||
geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
|
||||
geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
|
||||
geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
|
||||
geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
|
||||
geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
|
||||
geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
|
||||
geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
|
||||
// CA_MC_Rn_P{X,Y,Z}
|
||||
sprintf(buffer, "CA_MC_R%u_P%c", n, axis_char);
|
||||
param_get(param_find(buffer), &geometry.rotors[n].position(i));
|
||||
|
||||
geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
|
||||
geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
|
||||
geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
|
||||
geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
|
||||
geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
|
||||
geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
|
||||
geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
|
||||
geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
|
||||
// CA_MC_Rn_A{X,Y,Z}
|
||||
sprintf(buffer, "CA_MC_R%u_A%c", n, axis_char);
|
||||
param_get(param_find(buffer), &geometry.rotors[n].axis(i));
|
||||
}
|
||||
|
||||
geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
|
||||
geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
|
||||
geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
|
||||
geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
|
||||
geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
|
||||
geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
|
||||
geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
|
||||
geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
|
||||
geometry.rotors[n].axis.normalize();
|
||||
|
||||
geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
|
||||
geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
|
||||
geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
|
||||
geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
|
||||
geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
|
||||
geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
|
||||
geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
|
||||
geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
|
||||
// CA_MC_Rn_CT
|
||||
sprintf(buffer, "CA_MC_R%u_CT", n);
|
||||
param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);
|
||||
|
||||
geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
|
||||
geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
|
||||
geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
|
||||
geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
|
||||
geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
|
||||
geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
|
||||
geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
|
||||
geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
|
||||
// CA_MC_Rn_KM
|
||||
sprintf(buffer, "CA_MC_R%u_KM", n);
|
||||
param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
|
||||
}
|
||||
|
||||
_num_actuators = computeEffectivenessMatrix(geometry, matrix);
|
||||
return true;
|
||||
@ -145,11 +100,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
|
||||
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
|
||||
|
||||
// Get rotor axis
|
||||
matrix::Vector3f axis(
|
||||
geometry.rotors[i].axis_x,
|
||||
geometry.rotors[i].axis_y,
|
||||
geometry.rotors[i].axis_z
|
||||
);
|
||||
matrix::Vector3f axis{geometry.rotors[i].axis};
|
||||
|
||||
// Normalize axis
|
||||
float axis_norm = axis.norm();
|
||||
@ -163,11 +114,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
|
||||
}
|
||||
|
||||
// Get rotor position
|
||||
matrix::Vector3f position(
|
||||
geometry.rotors[i].position_x,
|
||||
geometry.rotors[i].position_y,
|
||||
geometry.rotors[i].position_z
|
||||
);
|
||||
const matrix::Vector3f position{geometry.rotors[i].position};
|
||||
|
||||
// Get coefficients
|
||||
float ct = geometry.rotors[i].thrust_coef;
|
||||
@ -178,10 +125,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
|
||||
}
|
||||
|
||||
// Compute thrust generated by this rotor
|
||||
matrix::Vector3f thrust = ct * axis;
|
||||
const matrix::Vector3f thrust{ct * axis};
|
||||
|
||||
// Compute moment generated by this rotor
|
||||
matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
|
||||
const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};
|
||||
|
||||
// Fill corresponding items in effectiveness matrix
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 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
|
||||
@ -57,20 +57,16 @@ public:
|
||||
|
||||
static constexpr int NUM_ROTORS_MAX = 8;
|
||||
|
||||
typedef struct {
|
||||
float position_x;
|
||||
float position_y;
|
||||
float position_z;
|
||||
float axis_x;
|
||||
float axis_y;
|
||||
float axis_z;
|
||||
float thrust_coef;
|
||||
float moment_ratio;
|
||||
} RotorGeometry;
|
||||
struct RotorGeometry {
|
||||
matrix::Vector3f position{};
|
||||
matrix::Vector3f axis{};
|
||||
float thrust_coef{0.f};
|
||||
float moment_ratio{0.f};
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
struct MultirotorGeometry {
|
||||
RotorGeometry rotors[NUM_ROTORS_MAX];
|
||||
} MultirotorGeometry;
|
||||
};
|
||||
|
||||
static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
|
||||
@ -81,78 +77,4 @@ public:
|
||||
private:
|
||||
bool _updated{true};
|
||||
int _num_actuators{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
|
||||
(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
|
||||
(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
|
||||
(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
|
||||
(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
|
||||
(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
|
||||
(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
|
||||
(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
|
||||
(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
|
||||
(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
|
||||
(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
|
||||
(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
|
||||
(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
|
||||
(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
|
||||
(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
|
||||
(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
|
||||
(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
|
||||
(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
|
||||
(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
|
||||
(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
|
||||
(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
|
||||
(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
|
||||
(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
|
||||
(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
|
||||
(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
|
||||
|
||||
(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
|
||||
(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
|
||||
(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
|
||||
(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
|
||||
(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
|
||||
)
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user