fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

[RFC] control_allocator: custom configuration parameters

Browse Source
This commit is contained in:
Daniel Agar 2021-11-02 21:48:21 -04:00
parent 8f6fd5f37b
commit 7e224d6bb5
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
8 changed files with 543 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

253
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp Normal file
View File

@ -0,0 +1,253 @@
/****************************************************************************
*
* 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 "ActuatorEffectivenessCustom.hpp"
ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
ModuleParams(parent)
{
}
float getScaleParameter(uint8_t n)
{
// CA_ACTn_SC: scale
char buffer[17];
sprintf(buffer, "CA_ACT%u_SC", n);
param_t param_handle = param_find(buffer);
if (param_handle == PARAM_INVALID) {
PX4_ERR("invalid parameter %s", buffer);
return 0.f;
}
float scale = 1.f;
if (param_get(param_handle, &scale) != PX4_OK) {
PX4_ERR("param_get %s failed", buffer);
return scale;
}
return 0.f;
}
bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
// TODO:
// - airspeed scaling
// - battery scaling
// - publish motors and servos (handle ranges)
// - disable individual actuators
// - feedback from output module (actual failures)
// - VTOL transitions (cleanly blend in and out?)
// - eg disable all downward thrust
// - eg disable all control surfaces
// - blend in/out higher level by adjusting max? then completely purge from effectiveness once off
if (_updated || force) {
_updated = false;
int num_actuators = 0;
_effectiveness.zero();
for (int n = 0; n < NUM_ACTUATORS; n++) {
// CA_ACTn_FUNC: look up function
char buffer[17];
sprintf(buffer, "CA_ACT%u_FUNC", n);
int32_t ctrl_func = 0;
param_get(param_find(buffer), &ctrl_func);
switch (ctrl_func) {
case 0:
// disabled
break;
case 1: {
// 1: custom thrust (parameters)
matrix::Vector3f axis{};
matrix::Vector3f position{};
float thrust_coef = 0.f;
float moment_ratio = 0.f;
for (int i = 0; i < 3; i++) {
char axis_char = 'X' + i;
// CA_ACTn_P{X,Y,Z}
sprintf(buffer, "CA_ACT%u_P%c", n, axis_char);
if (param_get(param_find(buffer), &position(i)) != PX4_OK) {
PX4_ERR("unable to get %s", buffer);
}
// CA_ACTn_A_{X,Y,Z}
sprintf(buffer, "CA_ACT%u_A%c", n, axis_char);
if (param_get(param_find(buffer), &axis(i)) != PX4_OK) {
PX4_ERR("unable to get %s", buffer);
}
}
// CA_ACTn_CT TODO
sprintf(buffer, "CA_ACT%u_CT", n);
param_get(param_find(buffer), &thrust_coef);
// CA_ACTn_KM TODO
sprintf(buffer, "CA_ACT%u_KM", n);
param_get(param_find(buffer), &moment_ratio);
// Normalize axis
float axis_norm = axis.norm();
if (axis_norm > FLT_EPSILON) {
axis /= axis_norm;
} else {
// Bad axis definition, ignore this rotor
continue;
}
// Get coefficients
float ct = thrust_coef;
float km = moment_ratio;
if (fabsf(ct) < FLT_EPSILON) {
continue;
}
// Compute thrust generated by this rotor
const matrix::Vector3f thrust{ct * axis};
// Compute moment generated by this rotor
const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};
// Fill corresponding items in effectiveness matrix
for (size_t j = 0; j < 3; j++) {
_effectiveness(j, n) = moment(j);
_effectiveness(j + 3, n) = thrust(j);
}
}
break;
case 2: {
// 2: custom torque (parameters)
// CA_ACTn_TRQ_R
// CA_ACTn_TRQ_P
// CA_ACTn_TRQ_Y
char torque_str[3][17];
sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
for (int i = 0; i < 3; i++) {
// CA_ACTn_TRQ_{R,P,Y}
param_get(param_find(torque_str[i]), &_effectiveness(i, n));
}
}
break;
case 100: {
// 100: Aileron, scaled (CA_ACTn_SC) roll torque
_effectiveness(0, n) = getScaleParameter(n);
}
break;
case 101: {
// 101: Elevator, scaled (CA_ACTn_SC) pitch torque
_effectiveness(1, n) = getScaleParameter(n);
}
break;
case 102: {
// 102: Rudder, scaled (CA_ACTn_SC) yaw torque
_effectiveness(2, n) = getScaleParameter(n);
}
break;
case 103: {
// 103: Throttle, scaled (CA_ACTn_SC) X thrust
_effectiveness(4, n) = getScaleParameter(n);
}
break;
case 104: {
// 104: Elevon(+), scaled (CA_ACTn_SC) roll & pitch torque
float scale = getScaleParameter(n);
_effectiveness(0, n) = 0.5f * scale;
_effectiveness(1, n) = 0.5f * scale;
}
break;
case 105: {
// 105: Elevon(+), scaled (CA_ACTn_SC) -roll & pitch torque
float scale = getScaleParameter(n);
_effectiveness(0, n) = -0.5f * scale;
_effectiveness(1, n) = 0.5f * scale;
}
break;
case 106: {
// 106: V-Tail (+), scaled (CA_ACTn_SC) pitch & yaw torque
float scale = getScaleParameter(n);
_effectiveness(1, n) = 0.5f * scale;
_effectiveness(2, n) = 0.5f * scale;
}
break;
case 107: {
// 107: V-Tail (-), scaled (CA_ACTn_SC) pitch & -yaw torque
float scale = getScaleParameter(n);
_effectiveness(1, n) = 0.5f * scale;
_effectiveness(2, n) = -0.5f * scale;
}
break;
default:
PX4_ERR("%s invalid function %d", buffer, n);
break;
}
if (_effectiveness.col(n).longerThan(0.f)) {
num_actuators = n + 1;
}
}
_effectiveness.transpose().print();
_num_actuators = num_actuators;
matrix = _effectiveness;
return true;
}
return false;
}

67
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp Normal file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ActuatorEffectivenessCustom.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 ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
{
public:
ActuatorEffectivenessCustom(ModuleParams *parent);
virtual ~ActuatorEffectivenessCustom() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
int numActuators() const override { return _num_actuators; }
private:
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness{};
int _num_actuators{0};
bool _updated{true};
};

4
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-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
@ -33,6 +33,8 @@
px4_add_library(ActuatorEffectiveness
ActuatorEffectiveness.hpp
ActuatorEffectivenessCustom.cpp
ActuatorEffectivenessCustom.hpp
ActuatorEffectivenessMultirotor.cpp
ActuatorEffectivenessMultirotor.hpp
ActuatorEffectivenessStandardVTOL.cpp

4
src/modules/control_allocator/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-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
@ -49,4 +49,6 @@ px4_add_module(
ControlAllocation
mixer
px4_work_queue
MODULE_CONFIG
module.yaml
)

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-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
@ -51,8 +51,7 @@ using namespace time_literals;
ControlAllocator::ControlAllocator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc)
{
parameters_updated();
}
@ -212,6 +211,10 @@ ControlAllocator::update_effectiveness_source()
tmp = new ActuatorEffectivenessTiltrotorVTOL();
break;
case EffectivenessSource::CUSTOM:
tmp = new ActuatorEffectivenessCustom(this);
break;
default:
PX4_ERR("Unknown airframe");
break;
@ -512,6 +515,10 @@ int ControlAllocator::print_status()
case EffectivenessSource::TILTROTOR_VTOL:
PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
break;
case EffectivenessSource::CUSTOM:
PX4_INFO("EffectivenessSource: Custom");
break;
}
// Print current effectiveness matrix

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-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
@ -42,6 +42,7 @@
#pragma once
#include <ActuatorEffectiveness.hpp>
#include <ActuatorEffectivenessCustom.hpp>
#include <ActuatorEffectivenessMultirotor.hpp>
#include <ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
@ -61,8 +62,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>
@ -124,10 +123,11 @@ private:
ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations
enum class EffectivenessSource {
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
TILTROTOR_VTOL = 2,
CUSTOM = 3,
};
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
@ -146,16 +146,14 @@ 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 */
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_run{0};
hrt_abstime _timestamp_sample{0};

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-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
@ -50,6 +50,7 @@
* @value 0 Multirotor
* @value 1 Standard VTOL (WIP)
* @value 2 Tiltrotor VTOL (WIP)
* @value 3 Custom (CA_ACTn* torque and thrust parameters)
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIRFRAME, 0);

198
src/modules/control_allocator/module.yaml Normal file
View File

@ -0,0 +1,198 @@
__max_num_config_instances: &max_num_config_instances 16
module_name: control_allocator
parameters:
- group: Control Allocator
definitions:
CA_ACT${i}_FUNC:
description:
short: actuator ${i} function
long: actuator ${i} function
type: enum
values:
0: disabled
1: custom thrust (parameters)
2: custom torque (parameters)
100: Aileron
101: Elevator
102: Rudder
103: Throttle
104: Elevon (+)
105: Elevon (-)
106: V-Tail (+)
107: V-Tail (-)
num_instances: *max_num_config_instances
instance_start: 0
default: 0
CA_ACT${i}_PX:
description:
short: actuator ${i} position X
long: Actuator ${i} position along X body axis
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_PY:
description:
short: actuator ${i} position Y
long: Actuator ${i} position along Y body axis
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_PZ:
description:
short: actuator ${i} position Z
long: Actuator ${i} position along Z body axis
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_X:
description:
short: actuator ${i} thrust x
long: Actuator ${i} thrust x
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_Y:
description:
short: actuator ${i} thrust y
long: Actuator ${i} thrust y
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_THR_Z:
description:
short: actuator ${i} thrust z
long: Actuator ${i} thrust z
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_AX:
description:
short: actuator ${i} axis of thrust vector, X body axis component
long: Actuator ${i} axis of thrust vector, X body axis component
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_AY:
description:
short: actuator ${i} axis of thrust vector, Y body axis component
long: Actuator ${i} axis of thrust vector, Y body axis component
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_AZ:
description:
short: actuator ${i} axis of thrust vector, Z body axis component
long: Actuator ${i} axis of thrust vector, Z body axis component
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_CT:
description:
short: actuator ${i} thrust coefficient
long: The thrust coefficient if defined as Thrust = CT * u^2,
where u (with value between CA_ACT0_MIN and CA_ACT0_MAX)
is the output signal sent to the motor controller.
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_KM:
description:
short: actuator ${i} moment coefficient
long: The moment coefficient if defined as Torque = KM * Thrust
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_TRQ_R:
description:
short: actuator ${i} roll torque
long: Actuator ${i} roll torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_TRQ_P:
description:
short: actuator ${i} pitch torque
long: Actuator ${i} pitch torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_TRQ_Y:
description:
short: actuator ${i} yaw torque
long: Actuator ${i} yaw torque
type: float
decimal: 2
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 0.0
CA_ACT${i}_SC:
description:
short: actuator ${i} scale
long: Actuator ${i} scale
type: float
decimal: 2
minimum: -1.0
maximum: 1.0
reboot_required: false
num_instances: *max_num_config_instances
instance_start: 0
default: 1.0