forked from Archive/PX4-Autopilot
control_allocator: major refactoring & additions
- allow effectiveness matrix to select control allocator method (desaturation algorithm) - add actuator_servos publication - add support for multiple matrices (for vtol) - add updateSetpoint callback method to actuator effectiveness to allow it to manipulate the actuator setpoint after allocation - handle motor stopping & reversal - add control surfaces & tilt servos - handle standard vtol + tiltrotor - rename MC rotors params & class to be more generically usable - fixes and enables ActuatorEffectivenessRotorsTest
This commit is contained in:
Beat Küng
Daniel Agar
parent
a81f11acdd
commit
70e46a194f
|
|
@ -14,19 +14,19 @@ param set-default SYS_CTRL_ALLOC 1
|
|||
|
||||
param set-default CA_AIRFRAME 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_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_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_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_KM -0.05
|
||||
param set-default CA_ROTOR_COUNT 4
|
||||
param set-default CA_ROTOR0_PX 0.1515
|
||||
param set-default CA_ROTOR0_PY 0.245
|
||||
param set-default CA_ROTOR0_KM 0.05
|
||||
param set-default CA_ROTOR1_PX -0.1515
|
||||
param set-default CA_ROTOR1_PY -0.1875
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.1515
|
||||
param set-default CA_ROTOR2_PY -0.245
|
||||
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 PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
|
|
|
|||
|
|
@ -30,26 +30,26 @@ param set-default MNT_MODE_IN 0
|
|||
param set-default MAV_PROTO_VER 2
|
||||
|
||||
param set-default CA_AIRFRAME 0
|
||||
param set-default CA_MC_R_COUNT 6
|
||||
param set-default CA_ROTOR_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_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_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_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_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_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_KM -0.05
|
||||
param set-default CA_ROTOR0_PX 0.0
|
||||
param set-default CA_ROTOR0_PY 1.0
|
||||
param set-default CA_ROTOR0_KM -0.05
|
||||
param set-default CA_ROTOR1_PX 0.0
|
||||
param set-default CA_ROTOR1_PY -1.0
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.866025
|
||||
param set-default CA_ROTOR2_PY -0.5
|
||||
param set-default CA_ROTOR2_KM -0.05
|
||||
param set-default CA_ROTOR3_PX -0.866025
|
||||
param set-default CA_ROTOR3_PY 0.5
|
||||
param set-default CA_ROTOR3_KM 0.05
|
||||
param set-default CA_ROTOR4_PX 0.866025
|
||||
param set-default CA_ROTOR4_PY 0.5
|
||||
param set-default CA_ROTOR4_KM 0.05
|
||||
param set-default CA_ROTOR5_PX -0.866025
|
||||
param set-default CA_ROTOR5_PY -0.5
|
||||
param set-default CA_ROTOR5_KM -0.05
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
|
|
|
|||
|
|
@ -19,19 +19,19 @@ param set-default SYS_CTRL_ALLOC 1
|
|||
|
||||
param set-default CA_AIRFRAME 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_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_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_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_KM -0.05
|
||||
param set-default CA_ROTOR_COUNT 4
|
||||
param set-default CA_ROTOR0_PX 0.177
|
||||
param set-default CA_ROTOR0_PY 0.177
|
||||
param set-default CA_ROTOR0_KM 0.05
|
||||
param set-default CA_ROTOR1_PX -0.177
|
||||
param set-default CA_ROTOR1_PY -0.177
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.177
|
||||
param set-default CA_ROTOR2_PY -0.177
|
||||
param set-default CA_ROTOR2_KM -0.05
|
||||
param set-default CA_ROTOR3_PX -0.177
|
||||
param set-default CA_ROTOR3_PY 0.177
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
|
|
|
|||
|
|
@ -15,31 +15,31 @@
|
|||
param set-default SYS_CTRL_ALLOC 1
|
||||
|
||||
param set-default CA_AIRFRAME 0
|
||||
param set-default CA_MC_R_COUNT 6
|
||||
param set-default CA_ROTOR_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_KM -0.05
|
||||
param set-default CA_ROTOR0_PX 0.0
|
||||
param set-default CA_ROTOR0_PY 0.275
|
||||
param set-default CA_ROTOR0_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_KM 0.05
|
||||
param set-default CA_ROTOR1_PX 0.0
|
||||
param set-default CA_ROTOR1_PY -0.275
|
||||
param set-default CA_ROTOR1_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_KM -0.05
|
||||
param set-default CA_ROTOR2_PX 0.238
|
||||
param set-default CA_ROTOR2_PY -0.1375
|
||||
param set-default CA_ROTOR2_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_KM 0.05
|
||||
param set-default CA_ROTOR3_PX -0.238
|
||||
param set-default CA_ROTOR3_PY 0.1375
|
||||
param set-default CA_ROTOR3_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_KM 0.05
|
||||
param set-default CA_ROTOR4_PX 0.238
|
||||
param set-default CA_ROTOR4_PY 0.1375
|
||||
param set-default CA_ROTOR4_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_KM -0.05
|
||||
param set-default CA_ROTOR5_PX -0.238
|
||||
param set-default CA_ROTOR5_PY -0.1375
|
||||
param set-default CA_ROTOR5_KM -0.05
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
|
|
|
|||
|
|
@ -0,0 +1,86 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "../ControlAllocation/ControlAllocation.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque,
|
||||
const matrix::Vector3f &thrust)
|
||||
{
|
||||
int actuator_idx = num_actuators_matrix[selected_matrix];
|
||||
|
||||
if (actuator_idx >= NUM_ACTUATORS) {
|
||||
PX4_ERR("Too many actuators");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) {
|
||||
PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)");
|
||||
return -1;
|
||||
}
|
||||
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
|
||||
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
|
||||
matrix_selection_indexes[totalNumActuators()] = selected_matrix;
|
||||
++num_actuators[(int)type];
|
||||
return num_actuators_matrix[selected_matrix]++;
|
||||
}
|
||||
|
||||
void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count)
|
||||
{
|
||||
int total_count = totalNumActuators();
|
||||
|
||||
for (int i = 0; i < count; ++i) {
|
||||
matrix_selection_indexes[i + total_count] = selected_matrix;
|
||||
}
|
||||
|
||||
num_actuators[(int)type] += count;
|
||||
num_actuators_matrix[selected_matrix] += count;
|
||||
}
|
||||
|
||||
int ActuatorEffectiveness::Configuration::totalNumActuators() const
|
||||
{
|
||||
int total_count = 0;
|
||||
|
||||
for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
|
||||
total_count += num_actuators_matrix[i];
|
||||
}
|
||||
|
||||
return total_count;
|
||||
}
|
||||
|
|
@ -41,18 +41,36 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <ControlAllocation/ControlAllocation.hpp>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
enum class AllocationMethod {
|
||||
NONE = -1,
|
||||
PSEUDO_INVERSE = 0,
|
||||
SEQUENTIAL_DESATURATION = 1,
|
||||
AUTO = 2,
|
||||
};
|
||||
|
||||
enum class ActuatorType {
|
||||
MOTORS = 0,
|
||||
SERVOS,
|
||||
|
||||
COUNT
|
||||
};
|
||||
|
||||
|
||||
class ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectiveness() = default;
|
||||
virtual ~ActuatorEffectiveness() = default;
|
||||
|
||||
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
|
||||
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
|
||||
static constexpr int NUM_ACTUATORS = 16;
|
||||
static constexpr int NUM_AXES = 6;
|
||||
|
||||
static constexpr int MAX_NUM_MATRICES = 2;
|
||||
|
||||
using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
|
||||
using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
|
||||
|
||||
enum class FlightPhase {
|
||||
HOVER_FLIGHT = 0,
|
||||
|
|
@ -61,6 +79,31 @@ public:
|
|||
TRANSITION_FF_TO_HF = 3
|
||||
};
|
||||
|
||||
struct Configuration {
|
||||
/**
|
||||
* Add an actuator to the selected matrix, returning the index, or -1 on error
|
||||
*/
|
||||
int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust);
|
||||
|
||||
/**
|
||||
* Call this after manually adding N actuators to the selected matrix
|
||||
*/
|
||||
void actuatorsAdded(ActuatorType type, int count);
|
||||
|
||||
int totalNumActuators() const;
|
||||
|
||||
/// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos
|
||||
EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES];
|
||||
|
||||
int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
|
||||
ActuatorVector trim[MAX_NUM_MATRICES];
|
||||
|
||||
int selected_matrix;
|
||||
|
||||
uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
|
||||
int num_actuators[(int)ActuatorType::COUNT];
|
||||
};
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
|
|
@ -71,22 +114,28 @@ public:
|
|||
_flight_phase = flight_phase;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
|
||||
* This is expected to stay constant.
|
||||
*/
|
||||
virtual int numMatrices() const { return 1; }
|
||||
|
||||
/**
|
||||
* Get the desired allocation method(s) for each matrix, if configured as AUTO
|
||||
*/
|
||||
virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
|
||||
allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the control effectiveness matrix if updated
|
||||
*
|
||||
* @return true if updated and matrix is set
|
||||
*/
|
||||
virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) = 0;
|
||||
|
||||
/**
|
||||
* Get the actuator trims
|
||||
*
|
||||
* @return Actuator trims
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
|
||||
{
|
||||
return _trim;
|
||||
}
|
||||
virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0;
|
||||
|
||||
/**
|
||||
* Get the current flight phase
|
||||
|
|
@ -98,17 +147,26 @@ public:
|
|||
return _flight_phase;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the number of actuators
|
||||
*/
|
||||
virtual int numActuators() const = 0;
|
||||
|
||||
/**
|
||||
* Display name
|
||||
*/
|
||||
virtual const char *name() const = 0;
|
||||
|
||||
|
||||
/**
|
||||
* Callback from the control allocation, allowing to manipulate the setpoint.
|
||||
* This can be used to e.g. add non-linear or external terms.
|
||||
* It is called after the matrix multiplication and before final clipping.
|
||||
* @param actuator_sp input & output setpoint
|
||||
*/
|
||||
virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp) {}
|
||||
|
||||
/**
|
||||
* Get a bitmask of motors to be stopped
|
||||
*/
|
||||
virtual uint32_t getStoppedMotors() const { return 0; }
|
||||
|
||||
protected:
|
||||
matrix::Vector<float, NUM_ACTUATORS> _trim; ///< Actuator trim
|
||||
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,159 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(ModuleParams *parent)
|
||||
: ModuleParams(parent)
|
||||
{
|
||||
for (int i = 0; i < MAX_COUNT; ++i) {
|
||||
char buffer[17];
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TYPE", i);
|
||||
_param_handles[i].type = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_R", i);
|
||||
_param_handles[i].torque[0] = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_P", i);
|
||||
_param_handles[i].torque[1] = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i);
|
||||
_param_handles[i].torque[2] = param_find(buffer);
|
||||
}
|
||||
|
||||
_count_handle = param_find("CA_SV_CS_COUNT");
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessControlSurfaces::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
int32_t count = 0;
|
||||
|
||||
if (param_get(_count_handle, &count) != 0) {
|
||||
PX4_ERR("param_get failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_count = count;
|
||||
|
||||
for (int i = 0; i < _count; i++) {
|
||||
param_get(_param_handles[i].type, (int32_t *)&_params[i].type);
|
||||
|
||||
Vector3f &torque = _params[i].torque;
|
||||
|
||||
for (int n = 0; n < 3; ++n) {
|
||||
param_get(_param_handles[i].torque[n], &torque(n));
|
||||
}
|
||||
|
||||
// TODO: enforce limits?
|
||||
switch (_params[i].type) {
|
||||
case Type::Elevator:
|
||||
break;
|
||||
|
||||
case Type::Rudder:
|
||||
break;
|
||||
|
||||
case Type::LeftElevon:
|
||||
break;
|
||||
|
||||
case Type::RightElevon:
|
||||
break;
|
||||
|
||||
case Type::LeftVTail:
|
||||
break;
|
||||
|
||||
case Type::RightVTail:
|
||||
break;
|
||||
|
||||
case Type::LeftFlaps:
|
||||
case Type::RightFlaps:
|
||||
torque.setZero();
|
||||
break;
|
||||
|
||||
case Type::LeftAileron:
|
||||
break;
|
||||
|
||||
case Type::RightAileron:
|
||||
break;
|
||||
|
||||
case Type::Airbrakes:
|
||||
torque.setZero();
|
||||
break;
|
||||
|
||||
case Type::Custom:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
{
|
||||
if (!force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _count; i++) {
|
||||
configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessControlSurfaces::applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control,
|
||||
int first_actuator_idx,
|
||||
ActuatorVector &actuator_sp) const
|
||||
{
|
||||
for (int i = 0; i < _count; ++i) {
|
||||
switch (_params[i].type) {
|
||||
// TODO: check sign
|
||||
case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps:
|
||||
actuator_sp(i + first_actuator_idx) += flaps_control;
|
||||
break;
|
||||
|
||||
case ActuatorEffectivenessControlSurfaces::Type::RightFlaps:
|
||||
actuator_sp(i + first_actuator_idx) -= flaps_control;
|
||||
break;
|
||||
|
||||
case ActuatorEffectivenessControlSurfaces::Type::Airbrakes:
|
||||
actuator_sp(i + first_actuator_idx) += airbrakes_control;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
|
||||
static constexpr int MAX_COUNT = 8;
|
||||
|
||||
enum class Type : int32_t {
|
||||
// This matches with the parameter
|
||||
Elevator = 1,
|
||||
Rudder = 2,
|
||||
LeftElevon = 3,
|
||||
RightElevon = 4,
|
||||
LeftVTail = 5,
|
||||
RightVTail = 6,
|
||||
LeftFlaps = 7,
|
||||
RightFlaps = 8,
|
||||
LeftAileron = 9,
|
||||
RightAileron = 10,
|
||||
Airbrakes = 11,
|
||||
Custom = 12,
|
||||
};
|
||||
|
||||
struct Params {
|
||||
Type type;
|
||||
|
||||
matrix::Vector3f torque;
|
||||
};
|
||||
|
||||
ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessControlSurfaces() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
|
||||
const char *name() const override { return "Control Surfaces"; }
|
||||
|
||||
int count() const { return _count; }
|
||||
|
||||
const Params &config(int idx) const { return _params[idx]; }
|
||||
|
||||
void applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control, int first_actuator_idx,
|
||||
ActuatorVector &actuator_sp) const;
|
||||
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
struct ParamHandles {
|
||||
param_t type;
|
||||
|
||||
param_t torque[3];
|
||||
};
|
||||
ParamHandles _param_handles[MAX_COUNT];
|
||||
param_t _count_handle;
|
||||
|
||||
Params _params[MAX_COUNT] {};
|
||||
int _count{0};
|
||||
};
|
||||
|
|
@ -1,198 +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 ActuatorEffectivenessMultirotor.hpp
|
||||
*
|
||||
* Actuator effectiveness computed from rotors position and orientation
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ActuatorEffectivenessMultirotor.hpp"
|
||||
|
||||
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent):
|
||||
ModuleParams(parent)
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
|
||||
bool force)
|
||||
{
|
||||
if (_updated || force) {
|
||||
_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();
|
||||
|
||||
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();
|
||||
|
||||
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();
|
||||
|
||||
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();
|
||||
|
||||
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();
|
||||
|
||||
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[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();
|
||||
|
||||
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();
|
||||
|
||||
geometry.num_rotors = _param_ca_mc_r_count.get();
|
||||
|
||||
_num_actuators = computeEffectivenessMatrix(geometry, matrix);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int
|
||||
ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
|
||||
{
|
||||
int num_actuators = 0;
|
||||
|
||||
effectiveness.setZero();
|
||||
|
||||
for (int i = 0; i < math::min(NUM_ROTORS_MAX, geometry.num_rotors); i++) {
|
||||
|
||||
// Get rotor axis
|
||||
matrix::Vector3f axis(
|
||||
geometry.rotors[i].axis_x,
|
||||
geometry.rotors[i].axis_y,
|
||||
geometry.rotors[i].axis_z
|
||||
);
|
||||
|
||||
// Normalize axis
|
||||
float axis_norm = axis.norm();
|
||||
|
||||
if (axis_norm > FLT_EPSILON) {
|
||||
axis /= axis_norm;
|
||||
|
||||
} else {
|
||||
// Bad axis definition, ignore this rotor
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get rotor position
|
||||
matrix::Vector3f position(
|
||||
geometry.rotors[i].position_x,
|
||||
geometry.rotors[i].position_y,
|
||||
geometry.rotors[i].position_z
|
||||
);
|
||||
|
||||
// Get coefficients
|
||||
float ct = geometry.rotors[i].thrust_coef;
|
||||
float km = geometry.rotors[i].moment_ratio;
|
||||
|
||||
if (fabsf(ct) < FLT_EPSILON) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute thrust generated by this rotor
|
||||
matrix::Vector3f thrust = ct * axis;
|
||||
|
||||
// Compute moment generated by this rotor
|
||||
matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
|
||||
|
||||
// Fill corresponding items in effectiveness matrix
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
effectiveness(j, i) = moment(j);
|
||||
effectiveness(j + 3, i) = thrust(j);
|
||||
}
|
||||
}
|
||||
|
||||
num_actuators = geometry.num_rotors;
|
||||
|
||||
return num_actuators;
|
||||
}
|
||||
|
|
@ -1,163 +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 ActuatorEffectivenessMultirotor.hpp
|
||||
*
|
||||
* Actuator effectiveness computed from rotors position and orientation
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessMultirotor(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessMultirotor() = default;
|
||||
|
||||
static constexpr int NUM_ROTORS_MAX = 8;
|
||||
|
||||
struct RotorGeometry {
|
||||
float position_x;
|
||||
float position_y;
|
||||
float position_z;
|
||||
float axis_x;
|
||||
float axis_y;
|
||||
float axis_z;
|
||||
float thrust_coef;
|
||||
float moment_ratio;
|
||||
};
|
||||
|
||||
struct MultirotorGeometry {
|
||||
RotorGeometry rotors[NUM_ROTORS_MAX];
|
||||
int num_rotors{0};
|
||||
};
|
||||
|
||||
static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
|
||||
|
||||
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
|
||||
|
||||
int numActuators() const override { return _num_actuators; }
|
||||
|
||||
const char *name() const override { return "Multirotor"; }
|
||||
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,
|
||||
|
||||
(ParamInt<px4::params::CA_MC_R_COUNT>) _param_ca_mc_r_count
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,247 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessRotors.hpp
|
||||
*
|
||||
* Actuator effectiveness computed from rotors position and orientation
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config,
|
||||
bool tilt_support)
|
||||
: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
|
||||
{
|
||||
for (int i = 0; i < NUM_ROTORS_MAX; ++i) {
|
||||
char buffer[17];
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i);
|
||||
_param_handles[i].position_x = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i);
|
||||
_param_handles[i].position_y = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i);
|
||||
_param_handles[i].position_z = param_find(buffer);
|
||||
|
||||
if (_axis_config == AxisConfiguration::Configurable) {
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i);
|
||||
_param_handles[i].axis_x = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i);
|
||||
_param_handles[i].axis_y = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i);
|
||||
_param_handles[i].axis_z = param_find(buffer);
|
||||
}
|
||||
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i);
|
||||
_param_handles[i].thrust_coef = param_find(buffer);
|
||||
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i);
|
||||
_param_handles[i].moment_ratio = param_find(buffer);
|
||||
|
||||
if (_tilt_support) {
|
||||
snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i);
|
||||
_param_handles[i].tilt_index = param_find(buffer);
|
||||
}
|
||||
}
|
||||
|
||||
_count_handle = param_find("CA_ROTOR_COUNT");
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessRotors::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
int32_t count = 0;
|
||||
|
||||
if (param_get(_count_handle, &count) != 0) {
|
||||
PX4_ERR("param_get failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_geometry.num_rotors = count;
|
||||
|
||||
for (int i = 0; i < _geometry.num_rotors; ++i) {
|
||||
Vector3f &position = _geometry.rotors[i].position;
|
||||
param_get(_param_handles[i].position_x, &position(0));
|
||||
param_get(_param_handles[i].position_y, &position(1));
|
||||
param_get(_param_handles[i].position_z, &position(2));
|
||||
|
||||
Vector3f &axis = _geometry.rotors[i].axis;
|
||||
|
||||
switch (_axis_config) {
|
||||
case AxisConfiguration::Configurable:
|
||||
param_get(_param_handles[i].axis_x, &axis(0));
|
||||
param_get(_param_handles[i].axis_y, &axis(1));
|
||||
param_get(_param_handles[i].axis_z, &axis(2));
|
||||
break;
|
||||
|
||||
case AxisConfiguration::FixedForward:
|
||||
axis = Vector3f(1.f, 0.f, 0.f);
|
||||
break;
|
||||
|
||||
case AxisConfiguration::FixedUpwards:
|
||||
axis = Vector3f(0.f, 0.f, -1.f);
|
||||
break;
|
||||
}
|
||||
|
||||
param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef);
|
||||
param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio);
|
||||
|
||||
if (_tilt_support) {
|
||||
int32_t tilt_param{0};
|
||||
param_get(_param_handles[i].tilt_index, &tilt_param);
|
||||
_geometry.rotors[i].tilt_index = tilt_param - 1;
|
||||
|
||||
} else {
|
||||
_geometry.rotors[i].tilt_index = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
{
|
||||
if (_updated || force) {
|
||||
_updated = false;
|
||||
|
||||
if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
|
||||
PX4_ERR("Wrong actuator ordering: servos need to be after motors");
|
||||
return false;
|
||||
}
|
||||
|
||||
int num_actuators = computeEffectivenessMatrix(_geometry,
|
||||
configuration.effectiveness_matrices[configuration.selected_matrix],
|
||||
configuration.num_actuators_matrix[configuration.selected_matrix]);
|
||||
configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int
|
||||
ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry,
|
||||
EffectivenessMatrix &effectiveness, int actuator_start_index)
|
||||
{
|
||||
int num_actuators = 0;
|
||||
|
||||
for (int i = 0; i < math::min(NUM_ROTORS_MAX, geometry.num_rotors); i++) {
|
||||
|
||||
if (i + actuator_start_index >= NUM_ACTUATORS) {
|
||||
break;
|
||||
}
|
||||
|
||||
++num_actuators;
|
||||
|
||||
// Get rotor axis
|
||||
Vector3f axis = geometry.rotors[i].axis;
|
||||
|
||||
// Normalize axis
|
||||
float axis_norm = axis.norm();
|
||||
|
||||
if (axis_norm > FLT_EPSILON) {
|
||||
axis /= axis_norm;
|
||||
|
||||
} else {
|
||||
// Bad axis definition, ignore this rotor
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get rotor position
|
||||
const Vector3f &position = geometry.rotors[i].position;
|
||||
|
||||
// Get coefficients
|
||||
float ct = geometry.rotors[i].thrust_coef;
|
||||
float km = geometry.rotors[i].moment_ratio;
|
||||
|
||||
if (geometry.yaw_disabled) {
|
||||
km = 0.f;
|
||||
}
|
||||
|
||||
if (fabsf(ct) < FLT_EPSILON) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute thrust generated by this rotor
|
||||
matrix::Vector3f thrust = ct * axis;
|
||||
|
||||
// Compute moment generated by this rotor
|
||||
matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
|
||||
|
||||
// Fill corresponding items in effectiveness matrix
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
effectiveness(j, i + actuator_start_index) = moment(j);
|
||||
effectiveness(j + 3, i + actuator_start_index) = thrust(j);
|
||||
}
|
||||
}
|
||||
|
||||
return num_actuators;
|
||||
}
|
||||
|
||||
uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
|
||||
{
|
||||
if (!PX4_ISFINITE(tilt_control)) {
|
||||
tilt_control = -1.f;
|
||||
}
|
||||
|
||||
uint32_t nontilted_motors = 0;
|
||||
|
||||
for (int i = 0; i < _geometry.num_rotors; ++i) {
|
||||
int tilt_index = _geometry.rotors[i].tilt_index;
|
||||
|
||||
if (tilt_index == -1 || tilt_index >= tilts.count()) {
|
||||
nontilted_motors |= 1u << i;
|
||||
continue;
|
||||
}
|
||||
|
||||
const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
|
||||
float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
|
||||
float tilt_direction = math::radians((float)tilt.tilt_direction);
|
||||
_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
|
||||
}
|
||||
|
||||
return nontilted_motors;
|
||||
}
|
||||
|
||||
Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction)
|
||||
{
|
||||
Vector3f axis{0.f, 0.f, -1.f};
|
||||
return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
|
||||
}
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 ActuatorEffectivenessRotors.hpp
|
||||
*
|
||||
* Actuator effectiveness computed from rotors position and orientation
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
|
||||
class ActuatorEffectivenessTilts;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
enum class AxisConfiguration {
|
||||
Configurable, ///< axis can be configured
|
||||
FixedForward, ///< axis is fixed, pointing forwards (positive X)
|
||||
FixedUpwards, ///< axis is fixed, pointing upwards (negative Z)
|
||||
};
|
||||
|
||||
static constexpr int NUM_ROTORS_MAX = 8;
|
||||
|
||||
struct RotorGeometry {
|
||||
matrix::Vector3f position;
|
||||
matrix::Vector3f axis;
|
||||
float thrust_coef;
|
||||
float moment_ratio;
|
||||
int tilt_index;
|
||||
};
|
||||
|
||||
struct Geometry {
|
||||
RotorGeometry rotors[NUM_ROTORS_MAX];
|
||||
int num_rotors{0};
|
||||
bool yaw_disabled{false};
|
||||
};
|
||||
|
||||
ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
|
||||
bool tilt_support = false);
|
||||
virtual ~ActuatorEffectivenessRotors() = default;
|
||||
|
||||
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
|
||||
}
|
||||
|
||||
static int computeEffectivenessMatrix(const Geometry &geometry,
|
||||
EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
|
||||
const char *name() const override { return "Multirotor"; }
|
||||
|
||||
/**
|
||||
* Sets the motor axis from tilt configurations and current tilt control.
|
||||
* @param tilts configured tilt servos
|
||||
* @param tilt_control current tilt control in [-1, 1] (can be NAN)
|
||||
* @return the motors as bitset which are not tiltable
|
||||
*/
|
||||
uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control);
|
||||
|
||||
const Geometry &geometry() const { return _geometry; }
|
||||
|
||||
/**
|
||||
* Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then
|
||||
* rotated by tilt_direction around z.
|
||||
*/
|
||||
static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
|
||||
|
||||
void enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; }
|
||||
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
bool _updated{true};
|
||||
const AxisConfiguration _axis_config;
|
||||
const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
|
||||
|
||||
struct ParamHandles {
|
||||
param_t position_x;
|
||||
param_t position_y;
|
||||
param_t position_z;
|
||||
param_t axis_x;
|
||||
param_t axis_y;
|
||||
param_t axis_z;
|
||||
param_t thrust_coef;
|
||||
param_t moment_ratio;
|
||||
param_t tilt_index;
|
||||
};
|
||||
ParamHandles _param_handles[NUM_ROTORS_MAX];
|
||||
param_t _count_handle;
|
||||
|
||||
Geometry _geometry{};
|
||||
};
|
||||
|
|
@ -32,7 +32,7 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ActuatorEffectivenessMultirotorTest.cpp
|
||||
* @file ActuatorEffectivenessRotors.cpp
|
||||
*
|
||||
* Tests for Control Allocation Algorithms
|
||||
*
|
||||
|
|
@ -40,52 +40,55 @@
|
|||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ActuatorEffectivenessMultirotor.hpp>
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
|
||||
TEST(ActuatorEffectivenessRotors, AllZeroCase)
|
||||
{
|
||||
// Quad wide geometry
|
||||
ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {};
|
||||
geometry.rotors[0].position_x = 1.0f;
|
||||
geometry.rotors[0].position_y = 1.0f;
|
||||
geometry.rotors[0].position_z = 0.0f;
|
||||
geometry.rotors[0].axis_x = 0.0f;
|
||||
geometry.rotors[0].axis_y = 0.0f;
|
||||
geometry.rotors[0].axis_z = -1.0f;
|
||||
ActuatorEffectivenessRotors::Geometry geometry = {};
|
||||
geometry.rotors[0].position(0) = 1.0f;
|
||||
geometry.rotors[0].position(1) = 1.0f;
|
||||
geometry.rotors[0].position(2) = 0.0f;
|
||||
geometry.rotors[0].axis(0) = 0.0f;
|
||||
geometry.rotors[0].axis(1) = 0.0f;
|
||||
geometry.rotors[0].axis(2) = -1.0f;
|
||||
geometry.rotors[0].thrust_coef = 1.0f;
|
||||
geometry.rotors[0].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[1].position_x = -1.0f;
|
||||
geometry.rotors[1].position_y = -1.0f;
|
||||
geometry.rotors[1].position_z = 0.0f;
|
||||
geometry.rotors[1].axis_x = 0.0f;
|
||||
geometry.rotors[1].axis_y = 0.0f;
|
||||
geometry.rotors[1].axis_z = -1.0f;
|
||||
geometry.rotors[1].position(0) = -1.0f;
|
||||
geometry.rotors[1].position(1) = -1.0f;
|
||||
geometry.rotors[1].position(2) = 0.0f;
|
||||
geometry.rotors[1].axis(0) = 0.0f;
|
||||
geometry.rotors[1].axis(1) = 0.0f;
|
||||
geometry.rotors[1].axis(2) = -1.0f;
|
||||
geometry.rotors[1].thrust_coef = 1.0f;
|
||||
geometry.rotors[1].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[2].position_x = 1.0f;
|
||||
geometry.rotors[2].position_y = -1.0f;
|
||||
geometry.rotors[2].position_z = 0.0f;
|
||||
geometry.rotors[2].axis_x = 0.0f;
|
||||
geometry.rotors[2].axis_y = 0.0f;
|
||||
geometry.rotors[2].axis_z = -1.0f;
|
||||
geometry.rotors[2].position(0) = 1.0f;
|
||||
geometry.rotors[2].position(1) = -1.0f;
|
||||
geometry.rotors[2].position(2) = 0.0f;
|
||||
geometry.rotors[2].axis(0) = 0.0f;
|
||||
geometry.rotors[2].axis(1) = 0.0f;
|
||||
geometry.rotors[2].axis(2) = -1.0f;
|
||||
geometry.rotors[2].thrust_coef = 1.0f;
|
||||
geometry.rotors[2].moment_ratio = -0.05f;
|
||||
|
||||
geometry.rotors[3].position_x = -1.0f;
|
||||
geometry.rotors[3].position_y = 1.0f;
|
||||
geometry.rotors[3].position_z = 0.0f;
|
||||
geometry.rotors[3].axis_x = 0.0f;
|
||||
geometry.rotors[3].axis_y = 0.0f;
|
||||
geometry.rotors[3].axis_z = -1.0f;
|
||||
geometry.rotors[3].position(0) = -1.0f;
|
||||
geometry.rotors[3].position(1) = 1.0f;
|
||||
geometry.rotors[3].position(2) = 0.0f;
|
||||
geometry.rotors[3].axis(0) = 0.0f;
|
||||
geometry.rotors[3].axis(1) = 0.0f;
|
||||
geometry.rotors[3].axis(2) = -1.0f;
|
||||
geometry.rotors[3].thrust_coef = 1.0f;
|
||||
geometry.rotors[3].moment_ratio = -0.05f;
|
||||
|
||||
matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness =
|
||||
ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry);
|
||||
geometry.num_rotors = 4;
|
||||
|
||||
ActuatorEffectiveness::EffectivenessMatrix effectiveness;
|
||||
effectiveness.setZero();
|
||||
ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
|
||||
|
||||
const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
|
||||
{-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
|
|
@ -95,8 +98,38 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
|
|||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness_expected(
|
||||
expected);
|
||||
ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
|
||||
|
||||
EXPECT_EQ(effectiveness, effectiveness_expected);
|
||||
}
|
||||
|
||||
TEST(ActuatorEffectivenessRotors, Tilt)
|
||||
{
|
||||
Vector3f axis_expected{0.f, 0.f, -1.f};
|
||||
Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{1.f, 0.f, 0.f};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{-1.f, 0.f, 0.f};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{0.f, 0.f, -1.f};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{0.f, 1.f, 0.f};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
|
||||
axis_expected = Vector3f{0.f, -1.f, 0.f};
|
||||
axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
|
||||
EXPECT_EQ(axis, axis_expected);
|
||||
}
|
||||
|
|
@ -31,79 +31,76 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ActuatorEffectivenessStandardVTOL.hpp
|
||||
*
|
||||
* Actuator effectiveness for standard VTOL
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ActuatorEffectivenessStandardVTOL.hpp"
|
||||
#include <ControlAllocation/ControlAllocation.hpp>
|
||||
|
||||
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent)
|
||||
: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
|
||||
{
|
||||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
|
||||
bool force)
|
||||
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
{
|
||||
if (!(_updated || force)) {
|
||||
if (!force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (_flight_phase) {
|
||||
case FlightPhase::HOVER_FLIGHT: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
// MC motors
|
||||
configuration.selected_matrix = 0;
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
|
||||
|
||||
case FlightPhase::FORWARD_FLIGHT: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
// Pusher/Puller
|
||||
configuration.selected_matrix = 1;
|
||||
|
||||
case FlightPhase::TRANSITION_HF_TO_FF:
|
||||
case FlightPhase::TRANSITION_FF_TO_HF: {
|
||||
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{ -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
|
||||
break;
|
||||
}
|
||||
for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) {
|
||||
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
|
||||
}
|
||||
|
||||
_updated = false;
|
||||
// Control Surfaces
|
||||
configuration.selected_matrix = 1;
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp)
|
||||
{
|
||||
ActuatorEffectiveness::setFlightPhase(flight_phase);
|
||||
_updated = true;
|
||||
// apply flaps
|
||||
if (matrix_index == 1) {
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
float control_flaps = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
|
||||
float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
if (_flight_phase == flight_phase) {
|
||||
return;
|
||||
}
|
||||
|
||||
ActuatorEffectiveness::setFlightPhase(flight_phase);
|
||||
|
||||
// update stopped motors
|
||||
switch (flight_phase) {
|
||||
case FlightPhase::FORWARD_FLIGHT:
|
||||
_stopped_motors = _mc_motors_mask;
|
||||
break;
|
||||
|
||||
case FlightPhase::HOVER_FLIGHT:
|
||||
case FlightPhase::TRANSITION_FF_TO_HF:
|
||||
case FlightPhase::TRANSITION_HF_TO_FF:
|
||||
_stopped_motors = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,25 +42,50 @@
|
|||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessStandardVTOL();
|
||||
ActuatorEffectivenessStandardVTOL(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessStandardVTOL() = default;
|
||||
|
||||
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
* @param Flight phase
|
||||
*/
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
int numActuators() const override { return 7; }
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
|
||||
const char *name() const override { return "Standard VTOL"; }
|
||||
protected:
|
||||
bool _updated{true};
|
||||
|
||||
int numMatrices() const override { return 2; }
|
||||
|
||||
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
|
||||
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
|
||||
allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
|
||||
}
|
||||
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp) override;
|
||||
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
uint32_t getStoppedMotors() const override { return _stopped_motors; }
|
||||
|
||||
private:
|
||||
ActuatorEffectivenessRotors _mc_rotors;
|
||||
ActuatorEffectivenessControlSurfaces _control_surfaces;
|
||||
|
||||
uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
|
||||
uint32_t _stopped_motors{}; ///< currently stopped motors
|
||||
|
||||
int _first_control_surface_idx{0}; ///< applies to matrix 1
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -41,76 +41,98 @@
|
|||
|
||||
#include "ActuatorEffectivenessTiltrotorVTOL.hpp"
|
||||
|
||||
ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent)
|
||||
: ModuleParams(parent),
|
||||
_mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true),
|
||||
_control_surfaces(this), _tilts(this)
|
||||
{
|
||||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
bool
|
||||
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
|
||||
bool force)
|
||||
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
{
|
||||
if (!(_updated || force)) {
|
||||
if (!_updated && !force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Trim
|
||||
float tilt = 0.0f;
|
||||
// MC motors
|
||||
configuration.selected_matrix = 0;
|
||||
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
|
||||
_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, _last_tilt_control)
|
||||
<< configuration.num_actuators[(int)ActuatorType::MOTORS];
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
|
||||
switch (_flight_phase) {
|
||||
case FlightPhase::HOVER_FLIGHT: {
|
||||
tilt = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::FORWARD_FLIGHT: {
|
||||
tilt = 1.5f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FlightPhase::TRANSITION_FF_TO_HF:
|
||||
case FlightPhase::TRANSITION_HF_TO_FF: {
|
||||
tilt = 1.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Trim: half throttle, tilted motors
|
||||
_trim(0) = 0.5f;
|
||||
_trim(1) = 0.5f;
|
||||
_trim(2) = 0.5f;
|
||||
_trim(3) = 0.5f;
|
||||
_trim(4) = tilt;
|
||||
_trim(5) = tilt;
|
||||
_trim(6) = tilt;
|
||||
_trim(7) = tilt;
|
||||
|
||||
// Effectiveness
|
||||
const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = {
|
||||
{-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
|
||||
|
||||
// Temporarily disable a few controls (WIP)
|
||||
for (size_t j = 4; j < 8; j++) {
|
||||
matrix(3, j) = 0.0f;
|
||||
matrix(4, j) = 0.0f;
|
||||
matrix(5, j) = 0.0f;
|
||||
}
|
||||
// Control Surfaces
|
||||
configuration.selected_matrix = 1;
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
|
||||
// Tilts
|
||||
configuration.selected_matrix = 0;
|
||||
_first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_tilts.updateYawSign(_mc_rotors.geometry());
|
||||
_tilts.getEffectivenessMatrix(configuration, true);
|
||||
|
||||
_updated = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp)
|
||||
{
|
||||
// apply flaps
|
||||
if (matrix_index == 1) {
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
float control_flaps = -1.f; // For tilt-rotors INDEX_FLAPS is set as combined tilt. TODO: fix this
|
||||
float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
// apply tilt
|
||||
if (matrix_index == 0) {
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f;
|
||||
|
||||
if (fabsf(control_tilt - _last_tilt_control) > 0.01f && PX4_ISFINITE(_last_tilt_control)) {
|
||||
_updated = true;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _tilts.count(); ++i) {
|
||||
if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
|
||||
actuator_sp(i + _first_tilt_idx) += control_tilt;
|
||||
}
|
||||
}
|
||||
|
||||
_last_tilt_control = control_tilt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
if (_flight_phase == flight_phase) {
|
||||
return;
|
||||
}
|
||||
|
||||
ActuatorEffectiveness::setFlightPhase(flight_phase);
|
||||
|
||||
_updated = true;
|
||||
// update stopped motors
|
||||
switch (flight_phase) {
|
||||
case FlightPhase::FORWARD_FLIGHT:
|
||||
_stopped_motors = _nontilted_motors;
|
||||
break;
|
||||
|
||||
case FlightPhase::HOVER_FLIGHT:
|
||||
case FlightPhase::TRANSITION_FF_TO_HF:
|
||||
case FlightPhase::TRANSITION_HF_TO_FF:
|
||||
_stopped_motors = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,25 +42,51 @@
|
|||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessTiltrotorVTOL();
|
||||
ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
|
||||
|
||||
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
|
||||
int numMatrices() const override { return 2; }
|
||||
|
||||
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
|
||||
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
|
||||
allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
* @param Flight phase
|
||||
*/
|
||||
void setFlightPhase(const FlightPhase &flight_phase) override;
|
||||
|
||||
int numActuators() const override { return 10; }
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp) override;
|
||||
|
||||
const char *name() const override { return "VTOL Tiltrotor"; }
|
||||
|
||||
uint32_t getStoppedMotors() const override { return _stopped_motors; }
|
||||
protected:
|
||||
bool _updated{true};
|
||||
ActuatorEffectivenessRotors _mc_rotors;
|
||||
ActuatorEffectivenessControlSurfaces _control_surfaces;
|
||||
ActuatorEffectivenessTilts _tilts;
|
||||
|
||||
uint32_t _nontilted_motors{}; ///< motors that are not tiltable
|
||||
uint32_t _stopped_motors{}; ///< currently stopped motors
|
||||
|
||||
int _first_control_surface_idx{0}; ///< applies to matrix 1
|
||||
int _first_tilt_idx{0}; ///< applies to matrix 0
|
||||
|
||||
float _last_tilt_control{NAN};
|
||||
|
||||
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,142 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessTilts::ActuatorEffectivenessTilts(ModuleParams *parent)
|
||||
: ModuleParams(parent)
|
||||
{
|
||||
for (int i = 0; i < MAX_COUNT; ++i) {
|
||||
char buffer[17];
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_CT", i);
|
||||
_param_handles[i].control = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MINA", i);
|
||||
_param_handles[i].min_angle = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MAXA", i);
|
||||
_param_handles[i].max_angle = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TD", i);
|
||||
_param_handles[i].tilt_direction = param_find(buffer);
|
||||
}
|
||||
|
||||
_count_handle = param_find("CA_SV_TL_COUNT");
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTilts::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
int32_t count = 0;
|
||||
|
||||
if (param_get(_count_handle, &count) != 0) {
|
||||
PX4_ERR("param_get failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_count = count;
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
param_get(_param_handles[i].control, (int32_t *)&_params[i].control);
|
||||
param_get(_param_handles[i].tilt_direction, (int32_t *)&_params[i].tilt_direction);
|
||||
param_get(_param_handles[i].min_angle, &_params[i].min_angle);
|
||||
param_get(_param_handles[i].max_angle, &_params[i].max_angle);
|
||||
|
||||
_params[i].min_angle = math::radians(_params[i].min_angle);
|
||||
_params[i].max_angle = math::radians(_params[i].max_angle);
|
||||
|
||||
_yaw_torque[i] = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
{
|
||||
if (!force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _count; i++) {
|
||||
Vector3f torque{};
|
||||
|
||||
if (_params[i].control == Control::Yaw) {
|
||||
torque(2) = _yaw_torque[i];
|
||||
}
|
||||
|
||||
configuration.addActuator(ActuatorType::SERVOS, torque, Vector3f{});
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTilts::updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry)
|
||||
{
|
||||
for (int i = 0; i < geometry.num_rotors; ++i) {
|
||||
int tilt_index = geometry.rotors[i].tilt_index;
|
||||
|
||||
if (tilt_index == -1 || tilt_index >= _count) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (_params[tilt_index].control != Control::Yaw) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Find the yaw torque sign by checking the motor position and tilt direction.
|
||||
// Rotate position by -tilt_direction around z, then check the sign of y pos
|
||||
float tilt_direction = math::radians((float)_params[tilt_index].tilt_direction);
|
||||
Vector3f rotated_pos = Dcmf{Eulerf{0.f, 0.f, -tilt_direction}} * geometry.rotors[i].position;
|
||||
|
||||
if (rotated_pos(1) < -0.01f) { // add minimal margin
|
||||
_yaw_torque[tilt_index] = 1.f;
|
||||
|
||||
} else if (rotated_pos(1) > 0.01f) {
|
||||
_yaw_torque[tilt_index] = -1.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ActuatorEffectivenessTilts::hasYawControl() const
|
||||
{
|
||||
for (int i = 0; i < _count; i++) {
|
||||
if (_params[i].control == Control::Yaw) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
|
||||
static constexpr int MAX_COUNT = 4;
|
||||
|
||||
enum class Control : int32_t {
|
||||
// This matches with the parameter
|
||||
None = 0,
|
||||
Yaw = 1,
|
||||
};
|
||||
enum class TiltDirection : int32_t {
|
||||
// This matches with the parameter
|
||||
TowardsFront = 0,
|
||||
TowardsRight = 90,
|
||||
};
|
||||
|
||||
struct Params {
|
||||
Control control;
|
||||
float min_angle;
|
||||
float max_angle;
|
||||
TiltDirection tilt_direction;
|
||||
};
|
||||
|
||||
ActuatorEffectivenessTilts(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessTilts() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
|
||||
const char *name() const override { return "Tilts"; }
|
||||
|
||||
int count() const { return _count; }
|
||||
|
||||
const Params &config(int idx) const { return _params[idx]; }
|
||||
|
||||
void updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry);
|
||||
|
||||
bool hasYawControl() const;
|
||||
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
struct ParamHandles {
|
||||
param_t control;
|
||||
param_t min_angle;
|
||||
param_t max_angle;
|
||||
param_t tilt_direction;
|
||||
};
|
||||
|
||||
ParamHandles _param_handles[MAX_COUNT];
|
||||
param_t _count_handle;
|
||||
|
||||
Params _params[MAX_COUNT] {};
|
||||
int _count{0};
|
||||
|
||||
float _yaw_torque[MAX_COUNT] {};
|
||||
};
|
||||
|
|
@ -32,9 +32,14 @@
|
|||
############################################################################
|
||||
|
||||
px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectiveness.cpp
|
||||
ActuatorEffectiveness.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
ActuatorEffectivenessMultirotor.hpp
|
||||
ActuatorEffectivenessControlSurfaces.cpp
|
||||
ActuatorEffectivenessControlSurfaces.hpp
|
||||
ActuatorEffectivenessTilts.cpp
|
||||
ActuatorEffectivenessTilts.hpp
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
ActuatorEffectivenessRotors.hpp
|
||||
ActuatorEffectivenessStandardVTOL.cpp
|
||||
ActuatorEffectivenessStandardVTOL.hpp
|
||||
ActuatorEffectivenessTiltrotorVTOL.cpp
|
||||
|
|
@ -46,7 +51,6 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
|
|||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
ControlAllocation
|
||||
)
|
||||
|
||||
# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
|
|
|
|||
|
|
@ -71,14 +71,16 @@
|
|||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
class ControlAllocation
|
||||
{
|
||||
public:
|
||||
ControlAllocation();
|
||||
virtual ~ControlAllocation() = default;
|
||||
|
||||
static constexpr uint8_t NUM_ACTUATORS = 16;
|
||||
static constexpr uint8_t NUM_AXES = 6;
|
||||
static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
|
||||
static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
|
||||
|
||||
typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
|
||||
|
||||
|
|
@ -93,8 +95,6 @@ public:
|
|||
|
||||
/**
|
||||
* Allocate control setpoint to actuators
|
||||
*
|
||||
* @param control_setpoint Desired control setpoint vector (input)
|
||||
*/
|
||||
virtual void allocate() = 0;
|
||||
|
||||
|
|
@ -190,6 +190,8 @@ public:
|
|||
*/
|
||||
void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
|
||||
|
||||
void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
|
||||
|
||||
/**
|
||||
* Normalize the actuator setpoint between minimum and maximum values.
|
||||
*
|
||||
|
|
@ -207,6 +209,8 @@ public:
|
|||
int numConfiguredActuators() const { return _num_actuators; }
|
||||
|
||||
protected:
|
||||
friend class ControlAllocator; // for _actuator_sp
|
||||
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; //< Effectiveness matrix
|
||||
matrix::Vector<float, NUM_AXES> _control_allocation_scale; //< Scaling applied during allocation
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; //< Neutral actuator values
|
||||
|
|
|
|||
|
|
@ -122,7 +122,4 @@ ControlAllocationPseudoInverse::allocate()
|
|||
|
||||
// Allocate
|
||||
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
|
||||
|
||||
// Clip
|
||||
clipActuatorSetpoint(_actuator_sp);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,9 +53,9 @@ public:
|
|||
ControlAllocationPseudoInverse() = default;
|
||||
virtual ~ControlAllocationPseudoInverse() = default;
|
||||
|
||||
virtual void allocate() override;
|
||||
virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators) override;
|
||||
void allocate() override;
|
||||
void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators) override;
|
||||
|
||||
protected:
|
||||
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ TEST(ControlAllocationTest, AllZeroCase)
|
|||
method.setEffectivenessMatrix(effectiveness, actuator_trim, 16);
|
||||
method.setControlSetpoint(control_sp);
|
||||
method.allocate();
|
||||
method.clipActuatorSetpoint();
|
||||
actuator_sp = method.getActuatorSetpoint();
|
||||
control_allocated_expected = method.getAllocatedControl();
|
||||
|
||||
|
|
|
|||
|
|
@ -60,9 +60,6 @@ ControlAllocationSequentialDesaturation::allocate()
|
|||
mixAirmodeDisabled();
|
||||
break;
|
||||
}
|
||||
|
||||
// Clip
|
||||
clipActuatorSetpoint(_actuator_sp);
|
||||
}
|
||||
|
||||
void ControlAllocationSequentialDesaturation::desaturateActuators(
|
||||
|
|
|
|||
|
|
@ -59,7 +59,10 @@ ControlAllocator::ControlAllocator() :
|
|||
|
||||
ControlAllocator::~ControlAllocator()
|
||||
{
|
||||
delete _control_allocation;
|
||||
for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
|
||||
delete _control_allocation[i];
|
||||
}
|
||||
|
||||
delete _actuator_effectiveness;
|
||||
|
||||
perf_free(_loop_perf);
|
||||
|
|
@ -86,69 +89,85 @@ ControlAllocator::parameters_updated()
|
|||
{
|
||||
// Allocation method & effectiveness source
|
||||
// Do this first: in case a new method is loaded, it will be configured below
|
||||
update_effectiveness_source();
|
||||
update_allocation_method();
|
||||
bool updated = update_effectiveness_source();
|
||||
update_allocation_method(updated); // must be called after update_effectiveness_source()
|
||||
|
||||
if (_control_allocation == nullptr) {
|
||||
if (_num_control_allocation == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
_control_allocation->updateParameters();
|
||||
for (int i = 0; i < _num_control_allocation; ++i) {
|
||||
_control_allocation[i]->updateParameters();
|
||||
}
|
||||
|
||||
update_effectiveness_matrix_if_needed(true);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocator::update_allocation_method()
|
||||
ControlAllocator::update_allocation_method(bool force)
|
||||
{
|
||||
AllocationMethod method = (AllocationMethod)_param_ca_method.get();
|
||||
AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get();
|
||||
|
||||
if (_allocation_method_id != method) {
|
||||
if (!_actuator_effectiveness) {
|
||||
PX4_ERR("_actuator_effectiveness null");
|
||||
return;
|
||||
}
|
||||
|
||||
// Save current state
|
||||
matrix::Vector<float, NUM_ACTUATORS> actuator_sp;
|
||||
if (_allocation_method_id != configured_method || force) {
|
||||
|
||||
if (_control_allocation != nullptr) {
|
||||
actuator_sp = _control_allocation->getActuatorSetpoint();
|
||||
matrix::Vector<float, NUM_ACTUATORS> actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
|
||||
// Cleanup first
|
||||
for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
|
||||
// Save current state
|
||||
if (_control_allocation[i] != nullptr) {
|
||||
actuator_sp[i] = _control_allocation[i]->getActuatorSetpoint();
|
||||
}
|
||||
|
||||
delete _control_allocation[i];
|
||||
_control_allocation[i] = nullptr;
|
||||
}
|
||||
|
||||
// try to instanciate new allocation method
|
||||
ControlAllocation *tmp = nullptr;
|
||||
_num_control_allocation = _actuator_effectiveness->numMatrices();
|
||||
|
||||
switch (method) {
|
||||
case AllocationMethod::PSEUDO_INVERSE:
|
||||
tmp = new ControlAllocationPseudoInverse();
|
||||
break;
|
||||
AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
_actuator_effectiveness->getDesiredAllocationMethod(desired_methods);
|
||||
|
||||
case AllocationMethod::SEQUENTIAL_DESATURATION:
|
||||
tmp = new ControlAllocationSequentialDesaturation();
|
||||
break;
|
||||
for (int i = 0; i < _num_control_allocation; ++i) {
|
||||
AllocationMethod method = configured_method;
|
||||
|
||||
default:
|
||||
PX4_ERR("Unknown allocation method");
|
||||
break;
|
||||
if (configured_method == AllocationMethod::AUTO) {
|
||||
method = desired_methods[i];
|
||||
}
|
||||
|
||||
switch (method) {
|
||||
case AllocationMethod::PSEUDO_INVERSE:
|
||||
_control_allocation[i] = new ControlAllocationPseudoInverse();
|
||||
break;
|
||||
|
||||
case AllocationMethod::SEQUENTIAL_DESATURATION:
|
||||
_control_allocation[i] = new ControlAllocationSequentialDesaturation();
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Unknown allocation method");
|
||||
break;
|
||||
}
|
||||
|
||||
if (_control_allocation[i] == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
_num_control_allocation = 0;
|
||||
|
||||
} else {
|
||||
_control_allocation[i]->setActuatorSetpoint(actuator_sp[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Replace previous method with new one
|
||||
if (tmp == nullptr) {
|
||||
// It did not work, forget about it
|
||||
PX4_ERR("Control allocation init failed");
|
||||
_param_ca_method.set((int)_allocation_method_id);
|
||||
|
||||
} else {
|
||||
// Swap allocation methods
|
||||
delete _control_allocation;
|
||||
_control_allocation = tmp;
|
||||
|
||||
// Save method id
|
||||
_allocation_method_id = method;
|
||||
|
||||
// Configure new allocation method
|
||||
_control_allocation->setActuatorSetpoint(actuator_sp);
|
||||
}
|
||||
_allocation_method_id = configured_method;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
bool
|
||||
ControlAllocator::update_effectiveness_source()
|
||||
{
|
||||
EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
|
||||
|
|
@ -161,15 +180,15 @@ ControlAllocator::update_effectiveness_source()
|
|||
switch (source) {
|
||||
case EffectivenessSource::NONE:
|
||||
case EffectivenessSource::MULTIROTOR:
|
||||
tmp = new ActuatorEffectivenessMultirotor(this);
|
||||
tmp = new ActuatorEffectivenessRotors(this);
|
||||
break;
|
||||
|
||||
case EffectivenessSource::STANDARD_VTOL:
|
||||
tmp = new ActuatorEffectivenessStandardVTOL();
|
||||
tmp = new ActuatorEffectivenessStandardVTOL(this);
|
||||
break;
|
||||
|
||||
case EffectivenessSource::TILTROTOR_VTOL:
|
||||
tmp = new ActuatorEffectivenessTiltrotorVTOL();
|
||||
tmp = new ActuatorEffectivenessTiltrotorVTOL(this);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -191,7 +210,11 @@ ControlAllocator::update_effectiveness_source()
|
|||
// Save source id
|
||||
_effectiveness_source_id = source;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -216,7 +239,7 @@ ControlAllocator::Run()
|
|||
parameters_updated();
|
||||
}
|
||||
|
||||
if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) {
|
||||
if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -281,18 +304,35 @@ ControlAllocator::Run()
|
|||
|
||||
update_effectiveness_matrix_if_needed();
|
||||
|
||||
// Set control setpoint vector
|
||||
matrix::Vector<float, NUM_AXES> c;
|
||||
c(0) = _torque_sp(0);
|
||||
c(1) = _torque_sp(1);
|
||||
c(2) = _torque_sp(2);
|
||||
c(3) = _thrust_sp(0);
|
||||
c(4) = _thrust_sp(1);
|
||||
c(5) = _thrust_sp(2);
|
||||
_control_allocation->setControlSetpoint(c);
|
||||
// Set control setpoint vector(s)
|
||||
matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
c[0](0) = _torque_sp(0);
|
||||
c[0](1) = _torque_sp(1);
|
||||
c[0](2) = _torque_sp(2);
|
||||
c[0](3) = _thrust_sp(0);
|
||||
c[0](4) = _thrust_sp(1);
|
||||
c[0](5) = _thrust_sp(2);
|
||||
|
||||
// Do allocation
|
||||
_control_allocation->allocate();
|
||||
if (_num_control_allocation > 1) {
|
||||
_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint);
|
||||
_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint);
|
||||
c[1](0) = vehicle_torque_setpoint.xyz[0];
|
||||
c[1](1) = vehicle_torque_setpoint.xyz[1];
|
||||
c[1](2) = vehicle_torque_setpoint.xyz[2];
|
||||
c[1](3) = vehicle_thrust_setpoint.xyz[0];
|
||||
c[1](4) = vehicle_thrust_setpoint.xyz[1];
|
||||
c[1](5) = vehicle_thrust_setpoint.xyz[2];
|
||||
}
|
||||
|
||||
for (int i = 0; i < _num_control_allocation; ++i) {
|
||||
|
||||
_control_allocation[i]->setControlSetpoint(c[i]);
|
||||
|
||||
// Do allocation
|
||||
_control_allocation[i]->allocate();
|
||||
_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp);
|
||||
_control_allocation[i]->clipActuatorSetpoint();
|
||||
}
|
||||
|
||||
// Publish actuator setpoint and allocator status
|
||||
publish_actuator_controls();
|
||||
|
|
@ -309,26 +349,61 @@ ControlAllocator::Run()
|
|||
void
|
||||
ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
|
||||
{
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
|
||||
ActuatorEffectiveness::Configuration config{};
|
||||
|
||||
if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) {
|
||||
if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
|
||||
return;
|
||||
}
|
||||
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
|
||||
if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) {
|
||||
_last_effectiveness_update = hrt_absolute_time();
|
||||
|
||||
// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
|
||||
matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
|
||||
matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
|
||||
memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes,
|
||||
sizeof(_control_allocation_selection_indexes));
|
||||
|
||||
for (size_t j = 0; j < NUM_ACTUATORS; j++) {
|
||||
if (actuator_min(j) >= actuator_max(j)) {
|
||||
for (size_t i = 0; i < NUM_AXES; i++) {
|
||||
effectiveness(i, j) = 0.0f;
|
||||
// Get the minimum and maximum depending on type and configuration
|
||||
ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
int actuator_idx = 0;
|
||||
int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
|
||||
|
||||
for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) {
|
||||
_num_actuators[actuator_type] = config.num_actuators[actuator_type];
|
||||
|
||||
for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) {
|
||||
if (actuator_idx >= NUM_ACTUATORS) {
|
||||
PX4_ERR("Too many actuators");
|
||||
break;
|
||||
}
|
||||
|
||||
int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
|
||||
|
||||
if ((ActuatorType)actuator_type == ActuatorType::MOTORS) {
|
||||
if (_param_r_rev.get() & (1u << actuator_type_idx)) {
|
||||
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
|
||||
|
||||
} else {
|
||||
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f;
|
||||
}
|
||||
|
||||
} else {
|
||||
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
|
||||
}
|
||||
|
||||
maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f;
|
||||
++actuator_idx_matrix[selected_matrix];
|
||||
++actuator_idx;
|
||||
}
|
||||
}
|
||||
|
||||
// Assign control effectiveness matrix
|
||||
_control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators());
|
||||
for (int i = 0; i < _num_control_allocation; ++i) {
|
||||
_control_allocation[i]->setActuatorMin(minimum[i]);
|
||||
_control_allocation[i]->setActuatorMax(maximum[i]);
|
||||
|
||||
// Assign control effectiveness matrix
|
||||
int total_num_actuators = config.num_actuators_matrix[i];
|
||||
_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], total_num_actuators);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -338,8 +413,10 @@ ControlAllocator::publish_control_allocator_status()
|
|||
control_allocator_status_s control_allocator_status{};
|
||||
control_allocator_status.timestamp = hrt_absolute_time();
|
||||
|
||||
// TODO: handle multiple matrices & disabled motors (?)
|
||||
|
||||
// Allocated control
|
||||
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation->getAllocatedControl();
|
||||
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[0]->getAllocatedControl();
|
||||
control_allocator_status.allocated_torque[0] = allocated_control(0);
|
||||
control_allocator_status.allocated_torque[1] = allocated_control(1);
|
||||
control_allocator_status.allocated_torque[2] = allocated_control(2);
|
||||
|
|
@ -348,7 +425,7 @@ ControlAllocator::publish_control_allocator_status()
|
|||
control_allocator_status.allocated_thrust[2] = allocated_control(5);
|
||||
|
||||
// Unallocated control
|
||||
matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation->getControlSetpoint() - allocated_control;
|
||||
matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control;
|
||||
control_allocator_status.unallocated_torque[0] = unallocated_control(0);
|
||||
control_allocator_status.unallocated_torque[1] = unallocated_control(1);
|
||||
control_allocator_status.unallocated_torque[2] = unallocated_control(2);
|
||||
|
|
@ -363,11 +440,11 @@ ControlAllocator::publish_control_allocator_status()
|
|||
unallocated_control(5)).norm_squared() < 1e-6f);
|
||||
|
||||
// Actuator saturation
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation->getActuatorMin();
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation->getActuatorMax();
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[0]->getActuatorSetpoint();
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[0]->getActuatorMin();
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[0]->getActuatorMax();
|
||||
|
||||
for (size_t i = 0; i < NUM_ACTUATORS; i++) {
|
||||
for (int i = 0; i < NUM_ACTUATORS; i++) {
|
||||
if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {
|
||||
control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER;
|
||||
|
||||
|
|
@ -386,17 +463,57 @@ ControlAllocator::publish_actuator_controls()
|
|||
actuator_motors.timestamp = hrt_absolute_time();
|
||||
actuator_motors.timestamp_sample = _timestamp_sample;
|
||||
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
|
||||
matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
|
||||
actuator_sp);
|
||||
actuator_servos_s actuator_servos;
|
||||
actuator_servos.timestamp = actuator_motors.timestamp;
|
||||
actuator_servos.timestamp_sample = _timestamp_sample;
|
||||
|
||||
for (int i = 0; i < _control_allocation->numConfiguredActuators(); i++) {
|
||||
actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
|
||||
actuator_motors.reversible_flags = _param_r_rev.get();
|
||||
|
||||
int actuator_idx = 0;
|
||||
int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
|
||||
|
||||
uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors();
|
||||
|
||||
// motors
|
||||
int motors_idx;
|
||||
|
||||
for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
|
||||
int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
|
||||
float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
|
||||
actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
|
||||
|
||||
if (stopped_motors & (1u << motors_idx)) {
|
||||
actuator_motors.control[motors_idx] = NAN;
|
||||
}
|
||||
|
||||
++actuator_idx_matrix[selected_matrix];
|
||||
++actuator_idx;
|
||||
}
|
||||
|
||||
for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) {
|
||||
actuator_motors.control[i] = NAN;
|
||||
}
|
||||
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
// TODO: servos
|
||||
// servos
|
||||
if (_num_actuators[1] > 0) {
|
||||
int servos_idx;
|
||||
|
||||
for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) {
|
||||
int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
|
||||
float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
|
||||
actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
|
||||
++actuator_idx_matrix[selected_matrix];
|
||||
++actuator_idx;
|
||||
}
|
||||
|
||||
for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) {
|
||||
actuator_servos.control[i] = NAN;
|
||||
}
|
||||
|
||||
_actuator_servos_pub.publish(actuator_servos);
|
||||
}
|
||||
}
|
||||
|
||||
int ControlAllocator::task_spawn(int argc, char *argv[])
|
||||
|
|
@ -439,6 +556,10 @@ int ControlAllocator::print_status()
|
|||
case AllocationMethod::SEQUENTIAL_DESATURATION:
|
||||
PX4_INFO("Method: Sequential desaturation");
|
||||
break;
|
||||
|
||||
case AllocationMethod::AUTO:
|
||||
PX4_INFO("Method: Auto");
|
||||
break;
|
||||
}
|
||||
|
||||
// Print current airframe
|
||||
|
|
@ -447,11 +568,20 @@ int ControlAllocator::print_status()
|
|||
}
|
||||
|
||||
// Print current effectiveness matrix
|
||||
if (_control_allocation != nullptr) {
|
||||
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness = _control_allocation->getEffectivenessMatrix();
|
||||
PX4_INFO("Effectiveness.T =");
|
||||
for (int i = 0; i < _num_control_allocation; ++i) {
|
||||
const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix();
|
||||
|
||||
if (_num_control_allocation > 1) {
|
||||
PX4_INFO("Instance: %i", i);
|
||||
}
|
||||
|
||||
PX4_INFO(" Effectiveness.T =");
|
||||
effectiveness.T().print();
|
||||
PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators());
|
||||
PX4_INFO(" minimum =");
|
||||
_control_allocation[i]->getActuatorMin().T().print();
|
||||
PX4_INFO(" maximum =");
|
||||
_control_allocation[i]->getActuatorMax().T().print();
|
||||
PX4_INFO(" Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
|
||||
}
|
||||
|
||||
// Print perf
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <ActuatorEffectiveness.hpp>
|
||||
#include <ActuatorEffectivenessMultirotor.hpp>
|
||||
#include <ActuatorEffectivenessRotors.hpp>
|
||||
#include <ActuatorEffectivenessStandardVTOL.hpp>
|
||||
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
|
||||
|
||||
|
|
@ -61,8 +61,6 @@
|
|||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/actuator_servos.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||
|
|
@ -72,6 +70,11 @@
|
|||
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
|
||||
static constexpr int NUM_AXES = ControlAllocation::NUM_AXES;
|
||||
|
||||
using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
|
||||
|
||||
ControlAllocator();
|
||||
|
||||
virtual ~ControlAllocator();
|
||||
|
|
@ -92,9 +95,6 @@ public:
|
|||
|
||||
bool init();
|
||||
|
||||
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
|
||||
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
|
|
@ -102,8 +102,8 @@ private:
|
|||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void update_allocation_method();
|
||||
void update_effectiveness_source();
|
||||
void update_allocation_method(bool force);
|
||||
bool update_effectiveness_source();
|
||||
|
||||
void update_effectiveness_matrix_if_needed(bool force = false);
|
||||
|
||||
|
|
@ -111,14 +111,10 @@ private:
|
|||
|
||||
void publish_actuator_controls();
|
||||
|
||||
enum class AllocationMethod {
|
||||
NONE = -1,
|
||||
PSEUDO_INVERSE = 0,
|
||||
SEQUENTIAL_DESATURATION = 1,
|
||||
};
|
||||
|
||||
AllocationMethod _allocation_method_id{AllocationMethod::NONE};
|
||||
ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations
|
||||
ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations
|
||||
int _num_control_allocation{0};
|
||||
hrt_abstime _last_effectiveness_update{0};
|
||||
|
||||
enum class EffectivenessSource {
|
||||
NONE = -1,
|
||||
|
|
@ -130,10 +126,16 @@ private:
|
|||
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
|
||||
ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness
|
||||
|
||||
uint8_t _control_allocation_selection_indexes[NUM_ACTUATORS * ActuatorEffectiveness::MAX_NUM_MATRICES] {};
|
||||
int _num_actuators[(int)ActuatorType::COUNT] {};
|
||||
|
||||
// Inputs
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */
|
||||
|
||||
uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1}; /**< vehicle torque setpoint subscription (2. instance) */
|
||||
uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */
|
||||
|
||||
// Outputs
|
||||
uORB::Publication<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */
|
||||
|
||||
|
|
@ -142,15 +144,11 @@ private:
|
|||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
matrix::Vector3f _torque_sp;
|
||||
matrix::Vector3f _thrust_sp;
|
||||
|
||||
// float _battery_scale_factor{1.0f};
|
||||
// float _airspeed_scale_factor{1.0f};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop duration performance counter */
|
||||
|
||||
hrt_abstime _last_run{0};
|
||||
|
|
@ -159,7 +157,8 @@ private:
|
|||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
|
||||
(ParamInt<px4::params::CA_METHOD>) _param_ca_method
|
||||
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||
)
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,4 +1,6 @@
|
|||
__max_num_mc_motors: &max_num_mc_motors 8
|
||||
__max_num_servos: &max_num_servos 8
|
||||
__max_num_tilts: &max_num_tilts 4
|
||||
|
||||
module_name: Control Allocation
|
||||
|
||||
|
|
@ -22,30 +24,50 @@ parameters:
|
|||
description:
|
||||
short: Control allocation method
|
||||
long: |
|
||||
Selects the algorithm and desaturation method
|
||||
Selects the algorithm and desaturation method.
|
||||
If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
|
||||
type: enum
|
||||
values:
|
||||
0: Pseudo-inverse with output clipping
|
||||
1: Pseudo-inverse with sequential desaturation technique
|
||||
default: 1
|
||||
2: Automatic
|
||||
default: 2
|
||||
|
||||
# MC rotors
|
||||
CA_MC_R_COUNT:
|
||||
# Motor parameters
|
||||
CA_R_REV:
|
||||
description:
|
||||
short: Reversible motors
|
||||
long: |
|
||||
Configure motors to be reversible. Note that the output driver needs to support this as well.
|
||||
type: bitmask
|
||||
bit:
|
||||
0: Motor 1
|
||||
1: Motor 2
|
||||
2: Motor 3
|
||||
3: Motor 4
|
||||
4: Motor 5
|
||||
5: Motor 6
|
||||
6: Motor 7
|
||||
7: Motor 8
|
||||
default: 0
|
||||
|
||||
# (MC) Rotors
|
||||
CA_ROTOR_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
|
||||
0: '0'
|
||||
1: '1'
|
||||
2: '2'
|
||||
3: '3'
|
||||
4: '4'
|
||||
5: '5'
|
||||
6: '6'
|
||||
7: '7'
|
||||
8: '8'
|
||||
default: 0
|
||||
CA_MC_R${i}_PX:
|
||||
CA_ROTOR${i}_PX:
|
||||
description:
|
||||
short: Position of rotor ${i} along X body axis
|
||||
type: float
|
||||
|
|
@ -56,7 +78,7 @@ parameters:
|
|||
min: -100
|
||||
max: 100
|
||||
default: 0.0
|
||||
CA_MC_R${i}_PY:
|
||||
CA_ROTOR${i}_PY:
|
||||
description:
|
||||
short: Position of rotor ${i} along Y body axis
|
||||
type: float
|
||||
|
|
@ -67,7 +89,7 @@ parameters:
|
|||
min: -100
|
||||
max: 100
|
||||
default: 0.0
|
||||
CA_MC_R${i}_PZ:
|
||||
CA_ROTOR${i}_PZ:
|
||||
description:
|
||||
short: Position of rotor ${i} along Z body axis
|
||||
type: float
|
||||
|
|
@ -79,7 +101,7 @@ parameters:
|
|||
max: 100
|
||||
default: 0.0
|
||||
|
||||
CA_MC_R${i}_AX:
|
||||
CA_ROTOR${i}_AX:
|
||||
description:
|
||||
short: Axis of rotor ${i} thrust vector, X body axis component
|
||||
long: Only the direction is considered (the vector is normalized).
|
||||
|
|
@ -90,7 +112,7 @@ parameters:
|
|||
min: -100
|
||||
max: 100
|
||||
default: 0.0
|
||||
CA_MC_R${i}_AY:
|
||||
CA_ROTOR${i}_AY:
|
||||
description:
|
||||
short: Axis of rotor ${i} thrust vector, Y body axis component
|
||||
long: Only the direction is considered (the vector is normalized).
|
||||
|
|
@ -101,7 +123,7 @@ parameters:
|
|||
min: -100
|
||||
max: 100
|
||||
default: 0.0
|
||||
CA_MC_R${i}_AZ:
|
||||
CA_ROTOR${i}_AZ:
|
||||
description:
|
||||
short: Axis of rotor ${i} thrust vector, Z body axis component
|
||||
long: Only the direction is considered (the vector is normalized).
|
||||
|
|
@ -113,7 +135,7 @@ parameters:
|
|||
max: 100
|
||||
default: -1.0
|
||||
|
||||
CA_MC_R${i}_CT:
|
||||
CA_ROTOR${i}_CT:
|
||||
description:
|
||||
short: Thrust coefficient of rotor ${i}
|
||||
long: |
|
||||
|
|
@ -128,7 +150,7 @@ parameters:
|
|||
max: 100
|
||||
default: 6.5
|
||||
|
||||
CA_MC_R${i}_KM:
|
||||
CA_ROTOR${i}_KM:
|
||||
description:
|
||||
short: Moment coefficient of rotor ${i}
|
||||
long: |
|
||||
|
|
@ -143,6 +165,163 @@ parameters:
|
|||
min: -1
|
||||
max: 1
|
||||
default: 0.05
|
||||
CA_ROTOR${i}_TILT:
|
||||
description:
|
||||
short: Rotor ${i} tilt assignment
|
||||
long: If not set to None, this motor is tilted by the configured tilt servo.
|
||||
type: enum
|
||||
values:
|
||||
0: 'None'
|
||||
1: 'Tilt 1'
|
||||
2: 'Tilt 2'
|
||||
3: 'Tilt 3'
|
||||
4: 'Tilt 4'
|
||||
num_instances: *max_num_mc_motors
|
||||
instance_start: 0
|
||||
default: 0
|
||||
|
||||
# control surfaces
|
||||
CA_SV_CS_COUNT:
|
||||
description:
|
||||
short: Total number of Control Surfaces
|
||||
type: enum
|
||||
values:
|
||||
0: '0'
|
||||
1: '1'
|
||||
2: '2'
|
||||
3: '3'
|
||||
4: '4'
|
||||
5: '5'
|
||||
6: '6'
|
||||
7: '7'
|
||||
8: '8'
|
||||
default: 0
|
||||
CA_SV_CS${i}_TYPE:
|
||||
description:
|
||||
short: Control Surface ${i} type
|
||||
type: enum
|
||||
values:
|
||||
1: Elevator
|
||||
2: Rudder
|
||||
3: Left Elevon
|
||||
4: Right Elevon
|
||||
5: Left V-Tail
|
||||
6: Right V-Tail
|
||||
7: Left Flaps
|
||||
8: Right Flaps
|
||||
9: Left Aileron
|
||||
10: Right Aileron
|
||||
11: Airbrakes
|
||||
12: Custom
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 1
|
||||
|
||||
CA_SV_CS${i}_TRQ_R:
|
||||
description:
|
||||
short: Control Surface ${i} roll torque scaling
|
||||
type: float
|
||||
decimal: 2
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0.0
|
||||
|
||||
CA_SV_CS${i}_TRQ_P:
|
||||
description:
|
||||
short: Control Surface ${i} pitch torque scaling
|
||||
type: float
|
||||
decimal: 2
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0.0
|
||||
|
||||
CA_SV_CS${i}_TRQ_Y:
|
||||
description:
|
||||
short: Control Surface ${i} yaw torque scaling
|
||||
type: float
|
||||
decimal: 2
|
||||
num_instances: *max_num_servos
|
||||
instance_start: 0
|
||||
default: 0.0
|
||||
|
||||
# Tilts
|
||||
CA_SV_TL_COUNT:
|
||||
description:
|
||||
short: Total number of Tilt Servos
|
||||
type: enum
|
||||
values:
|
||||
0: '0'
|
||||
1: '1'
|
||||
2: '2'
|
||||
3: '3'
|
||||
4: '4'
|
||||
default: 0
|
||||
CA_SV_TL${i}_CT:
|
||||
description:
|
||||
short: Tilt ${i} is used for control
|
||||
long: Define if this servo is used for additional control.
|
||||
type: enum
|
||||
values:
|
||||
0: 'None'
|
||||
1: 'Yaw'
|
||||
num_instances: *max_num_tilts
|
||||
instance_start: 0
|
||||
default: 1
|
||||
CA_SV_TL${i}_MINA:
|
||||
description:
|
||||
short: Tilt Servo ${i} Tilt Angle at Minimum
|
||||
long: |
|
||||
Defines the tilt angle when the servo is at the minimum.
|
||||
An angle of zero means upwards.
|
||||
type: float
|
||||
decimal: 0
|
||||
unit: 'deg'
|
||||
num_instances: *max_num_tilts
|
||||
instance_start: 0
|
||||
min: -90.0
|
||||
max: 90.0
|
||||
default: 0.0
|
||||
CA_SV_TL${i}_MAXA:
|
||||
description:
|
||||
short: Tilt Servo ${i} Tilt Angle at Maximum
|
||||
long: |
|
||||
Defines the tilt angle when the servo is at the maximum.
|
||||
An angle of zero means upwards.
|
||||
type: float
|
||||
decimal: 0
|
||||
unit: 'deg'
|
||||
num_instances: *max_num_tilts
|
||||
instance_start: 0
|
||||
min: -90.0
|
||||
max: 90.0
|
||||
default: 90.0
|
||||
CA_SV_TL${i}_TD:
|
||||
description:
|
||||
short: Tilt Servo ${i} Tilt Direction
|
||||
long: |
|
||||
Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
|
||||
For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
|
||||
the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
|
||||
type: enum
|
||||
values:
|
||||
0: 'Towards Front'
|
||||
90: 'Towards Right'
|
||||
num_instances: *max_num_tilts
|
||||
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:
|
||||
|
|
@ -153,6 +332,12 @@ mixer:
|
|||
min: 0
|
||||
max: 1
|
||||
default_is_nan: true
|
||||
per_item_parameters:
|
||||
- name: 'CA_R_REV'
|
||||
label: 'Reversible'
|
||||
show_as: 'bitset'
|
||||
index_offset: 0
|
||||
advanced: true
|
||||
servo:
|
||||
functions: 'Servo'
|
||||
actuator_testing_values:
|
||||
|
|
@ -165,42 +350,125 @@ mixer:
|
|||
max: 1
|
||||
default: -1
|
||||
|
||||
rules:
|
||||
- select_identifier: 'servo-type' # restrict torque based on servo type
|
||||
apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw']
|
||||
items:
|
||||
# TODO
|
||||
1: # Elevator
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
2: # Rudder
|
||||
- { 'hidden': True, 'default': 0.0 } # roll
|
||||
- { 'hidden': True, 'default': 0.0 } # pitch
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw
|
||||
3: # Left elevon
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll
|
||||
- { 'min': -1.0, 'max': 0.0, 'default': -1.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
4: # Right elevon
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll
|
||||
- { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch
|
||||
- { 'hidden': True, 'default': 0.0 } # yaw
|
||||
|
||||
config:
|
||||
param: CA_AIRFRAME
|
||||
types:
|
||||
0: # Multirotor
|
||||
type: "multirotor"
|
||||
type: 'multirotor'
|
||||
actuators:
|
||||
- actuator_type: 'motor'
|
||||
count: 'CA_MC_R_COUNT'
|
||||
count: 'CA_ROTOR_COUNT'
|
||||
per_item_parameters:
|
||||
standard:
|
||||
position: [ 'CA_MC_R${i}_PX', 'CA_MC_R${i}_PY', 'CA_MC_R${i}_PZ' ]
|
||||
position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ]
|
||||
extra:
|
||||
- name: 'CA_MC_R${i}_KM'
|
||||
- name: 'CA_ROTOR${i}_KM'
|
||||
label: 'Direction CCW'
|
||||
function: 'spin-dir'
|
||||
show_as: 'true-if-positive'
|
||||
|
||||
2: # Tiltrotor VTOL example
|
||||
1: # Standard VTOL
|
||||
actuators:
|
||||
- actuator_type: 'motor'
|
||||
instances:
|
||||
- name: 'Front Left Motor'
|
||||
position: [ 0.3, -0.3, 0 ]
|
||||
- name: 'Front Right Motor'
|
||||
position: [ 0.3, 0.3, 0 ]
|
||||
- name: 'Rear Motor'
|
||||
position: [ -0.75, 0.3, 0 ]
|
||||
group_label: 'MC Motors'
|
||||
count: 'CA_ROTOR_COUNT'
|
||||
per_item_parameters:
|
||||
standard:
|
||||
position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ]
|
||||
extra:
|
||||
- name: 'CA_ROTOR${i}_KM'
|
||||
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}'
|
||||
- actuator_type: 'servo'
|
||||
instances:
|
||||
- name: 'Front Left Tilt'
|
||||
position: [ 0.3, -0.3, 0 ]
|
||||
- name: 'Front Right Tilt'
|
||||
position: [ 0.3, 0.3, 0 ]
|
||||
- name: 'Left Elevon'
|
||||
position: [ 0, -0.3, 0 ]
|
||||
- name: 'Right Elevon'
|
||||
position: [ 0, 0.3, 0 ]
|
||||
group_label: 'Control Surfaces'
|
||||
count: 'CA_SV_CS_COUNT'
|
||||
per_item_parameters:
|
||||
extra:
|
||||
- name: 'CA_SV_CS${i}_TYPE'
|
||||
label: 'Type'
|
||||
function: 'type'
|
||||
identifier: 'servo-type'
|
||||
- name: 'CA_SV_CS${i}_TRQ_R'
|
||||
label: 'Roll Scale'
|
||||
identifier: 'servo-torque-roll'
|
||||
- name: 'CA_SV_CS${i}_TRQ_P'
|
||||
label: 'Pitch Scale'
|
||||
identifier: 'servo-torque-pitch'
|
||||
- name: 'CA_SV_CS${i}_TRQ_Y'
|
||||
label: 'Yaw Scale'
|
||||
identifier: 'servo-torque-yaw'
|
||||
|
||||
2: # Tiltrotor VTOL
|
||||
actuators:
|
||||
- actuator_type: 'motor'
|
||||
group_label: 'MC Motors'
|
||||
count: 'CA_ROTOR_COUNT'
|
||||
per_item_parameters:
|
||||
standard:
|
||||
position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ]
|
||||
extra:
|
||||
- name: 'CA_ROTOR${i}_TILT'
|
||||
label: 'Tilted by'
|
||||
- actuator_type: 'servo'
|
||||
group_label: 'Control Surfaces'
|
||||
count: 'CA_SV_CS_COUNT'
|
||||
item_label_prefix: 'Surface ${i}'
|
||||
per_item_parameters:
|
||||
extra:
|
||||
- name: 'CA_SV_CS${i}_TYPE'
|
||||
label: 'Type'
|
||||
function: 'type'
|
||||
identifier: 'servo-type'
|
||||
- name: 'CA_SV_CS${i}_TRQ_R'
|
||||
label: 'Roll Torque'
|
||||
identifier: 'servo-torque-roll'
|
||||
- name: 'CA_SV_CS${i}_TRQ_P'
|
||||
label: 'Pitch Torque'
|
||||
identifier: 'servo-torque-pitch'
|
||||
- name: 'CA_SV_CS${i}_TRQ_Y'
|
||||
label: 'Yaw Torque'
|
||||
identifier: 'servo-torque-yaw'
|
||||
- actuator_type: 'servo'
|
||||
group_label: 'Tilt Servos'
|
||||
count: 'CA_SV_TL_COUNT'
|
||||
item_label_prefix: 'Tilt ${i}'
|
||||
per_item_parameters:
|
||||
extra:
|
||||
- name: 'CA_SV_TL${i}_MINA'
|
||||
label: "Angle at Min\nTilt"
|
||||
- name: 'CA_SV_TL${i}_MAXA'
|
||||
label: "Angle at Max\nTilt"
|
||||
- name: 'CA_SV_TL${i}_TD'
|
||||
label: 'Tilt Direction'
|
||||
- name: 'CA_SV_TL${i}_CT'
|
||||
label: 'Use for Control'
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue