mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 22:14:07 +08:00
control_allocator: add helicopter mixer
Same logic as the existing mixer. Untested.
This commit is contained in:
Beat Küng
parent
32402f31df
commit
5cdb6fbd8e
@ -73,6 +73,15 @@ public:
|
||||
static constexpr int NUM_ACTUATORS = 16;
|
||||
static constexpr int NUM_AXES = 6;
|
||||
|
||||
enum ControlAxis {
|
||||
ROLL = 0,
|
||||
PITCH,
|
||||
YAW,
|
||||
THRUST_X,
|
||||
THRUST_Y,
|
||||
THRUST_Z
|
||||
};
|
||||
|
||||
static constexpr int MAX_NUM_MATRICES = 2;
|
||||
|
||||
using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
|
||||
|
||||
@ -0,0 +1,143 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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 "ActuatorEffectivenessHelicopter.hpp"
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent)
|
||||
: ModuleParams(parent)
|
||||
{
|
||||
for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) {
|
||||
char buffer[17];
|
||||
snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i);
|
||||
_param_handles.swash_plate_servos[i].angle = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i);
|
||||
_param_handles.swash_plate_servos[i].arm_length = param_find(buffer);
|
||||
}
|
||||
|
||||
_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
|
||||
|
||||
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
|
||||
char buffer[17];
|
||||
snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
|
||||
_param_handles.throttle_curve[i] = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i);
|
||||
_param_handles.pitch_curve[i] = param_find(buffer);
|
||||
}
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessHelicopter::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
int32_t count = 0;
|
||||
|
||||
if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) {
|
||||
PX4_ERR("param_get failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX);
|
||||
|
||||
for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
|
||||
float angle_deg{};
|
||||
param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg);
|
||||
_geometry.swash_plate_servos[i].angle = math::radians(angle_deg);
|
||||
param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length);
|
||||
}
|
||||
|
||||
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
|
||||
param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
|
||||
param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
|
||||
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
|
||||
|
||||
// Tail (yaw) servo
|
||||
configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
|
||||
|
||||
// N swash plate servos
|
||||
_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
|
||||
|
||||
for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
|
||||
configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{});
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
int matrix_index, ActuatorVector &actuator_sp)
|
||||
{
|
||||
// Find index to use for curves
|
||||
float thrust_cmd = -control_sp(ControlAxis::THRUST_Z);
|
||||
float num_intervals = NUM_CURVE_POINTS - 1;
|
||||
// We access idx + 1 below, so max legal index is (size - 2)
|
||||
int idx = math::constrain((int)(thrust_cmd * num_intervals), 0, NUM_CURVE_POINTS - 2);
|
||||
|
||||
// Local throttle curve gradient and offset
|
||||
float tg = (_geometry.throttle_curve[idx + 1] - _geometry.throttle_curve[idx]) * num_intervals;
|
||||
float to = (_geometry.throttle_curve[idx]) - (tg * idx / num_intervals);
|
||||
float throttle = math::constrain(tg * thrust_cmd + to, 0.0f, 1.0f);
|
||||
|
||||
// Local pitch curve gradient and offset
|
||||
float pg = (_geometry.pitch_curve[idx + 1] - _geometry.pitch_curve[idx]) * num_intervals;
|
||||
float po = (_geometry.pitch_curve[idx]) - (pg * idx / num_intervals);
|
||||
float collective_pitch = math::constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
|
||||
|
||||
float roll_cmd = control_sp(ControlAxis::ROLL);
|
||||
float pitch_cmd = control_sp(ControlAxis::PITCH);
|
||||
|
||||
actuator_sp(0) = throttle;
|
||||
|
||||
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
|
||||
actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
|
||||
+ cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length
|
||||
- sinf(_geometry.swash_plate_servos[i].angle) * roll_cmd * _geometry.swash_plate_servos[i].arm_length;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,89 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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 ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
|
||||
static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
|
||||
static constexpr int NUM_CURVE_POINTS = 5;
|
||||
|
||||
struct SwashPlateGeometry {
|
||||
float angle;
|
||||
float arm_length;
|
||||
};
|
||||
|
||||
struct Geometry {
|
||||
SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
|
||||
int num_swash_plate_servos{0};
|
||||
float throttle_curve[NUM_CURVE_POINTS];
|
||||
float pitch_curve[NUM_CURVE_POINTS];
|
||||
};
|
||||
|
||||
ActuatorEffectivenessHelicopter(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessHelicopter() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Helicopter"; }
|
||||
|
||||
|
||||
const Geometry &geometry() const { return _geometry; }
|
||||
|
||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
||||
ActuatorVector &actuator_sp) override;
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
struct ParamHandlesSwashPlate {
|
||||
param_t angle;
|
||||
param_t arm_length;
|
||||
};
|
||||
struct ParamHandles {
|
||||
ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
|
||||
param_t num_swash_plate_servos;
|
||||
param_t throttle_curve[NUM_CURVE_POINTS];
|
||||
param_t pitch_curve[NUM_CURVE_POINTS];
|
||||
};
|
||||
ParamHandles _param_handles{};
|
||||
|
||||
Geometry _geometry{};
|
||||
|
||||
int _first_swash_plate_servo_index{};
|
||||
};
|
||||
@ -40,6 +40,8 @@ px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectivenessCustom.hpp
|
||||
ActuatorEffectivenessFixedWing.cpp
|
||||
ActuatorEffectivenessFixedWing.hpp
|
||||
ActuatorEffectivenessHelicopter.cpp
|
||||
ActuatorEffectivenessHelicopter.hpp
|
||||
ActuatorEffectivenessMCTilt.cpp
|
||||
ActuatorEffectivenessMCTilt.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
|
||||
@ -254,6 +254,10 @@ ControlAllocator::update_effectiveness_source()
|
||||
tmp = new ActuatorEffectivenessCustom(this);
|
||||
break;
|
||||
|
||||
case EffectivenessSource::HELICOPTER:
|
||||
tmp = new ActuatorEffectivenessHelicopter(this);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Unknown airframe");
|
||||
break;
|
||||
|
||||
@ -51,6 +51,7 @@
|
||||
#include <ActuatorEffectivenessFixedWing.hpp>
|
||||
#include <ActuatorEffectivenessMCTilt.hpp>
|
||||
#include <ActuatorEffectivenessCustom.hpp>
|
||||
#include <ActuatorEffectivenessHelicopter.hpp>
|
||||
|
||||
#include <ControlAllocation.hpp>
|
||||
#include <ControlAllocationPseudoInverse.hpp>
|
||||
@ -148,6 +149,7 @@ private:
|
||||
MOTORS_6DOF = 7,
|
||||
MULTIROTOR_WITH_TILT = 8,
|
||||
CUSTOM = 9,
|
||||
HELICOPTER = 10,
|
||||
};
|
||||
|
||||
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
|
||||
|
||||
@ -27,6 +27,7 @@ parameters:
|
||||
7: Motors (6DOF)
|
||||
8: Multirotor with Tilt
|
||||
9: Custom
|
||||
10: Helicopter
|
||||
default: 0
|
||||
|
||||
CA_METHOD:
|
||||
@ -378,6 +379,62 @@ parameters:
|
||||
instance_start: 0
|
||||
default: 0
|
||||
|
||||
# helicopter
|
||||
CA_SP0_COUNT:
|
||||
description:
|
||||
short: Number of swash plates servos
|
||||
type: enum
|
||||
values:
|
||||
3: '3'
|
||||
4: '4'
|
||||
default: 3
|
||||
CA_SP0_ANG${i}:
|
||||
description:
|
||||
short: Angle for swash plate servo ${i}
|
||||
type: float
|
||||
decimal: 0
|
||||
increment: 10
|
||||
unit: deg
|
||||
num_instances: 4
|
||||
min: 0
|
||||
max: 360
|
||||
default: [0, 140, 220, 0]
|
||||
CA_SP0_ARM_L${i}:
|
||||
description:
|
||||
short: Arm length for swash plate servo ${i}
|
||||
long: |
|
||||
This is relative to the other arm lengths.
|
||||
type: float
|
||||
decimal: 3
|
||||
increment: 0.1
|
||||
num_instances: 4
|
||||
min: 0
|
||||
max: 10
|
||||
default: 1.0
|
||||
CA_HELI_THR_C${i}:
|
||||
description:
|
||||
short: Throttle curve at position ${i}
|
||||
long: |
|
||||
Defines the output throttle at the interval position ${i}.
|
||||
type: float
|
||||
decimal: 3
|
||||
increment: 0.1
|
||||
num_instances: 5
|
||||
min: 0
|
||||
max: 1
|
||||
default: [0, 0.3, 0.6, 0.8, 1]
|
||||
CA_HELI_PITCH_C${i}:
|
||||
description:
|
||||
short: Collective pitch curve at position ${i}
|
||||
long: |
|
||||
Defines the collective pitch at the interval position ${i} for a given thrust setpoint.
|
||||
type: float
|
||||
decimal: 3
|
||||
increment: 0.1
|
||||
num_instances: 5
|
||||
min: 0
|
||||
max: 1
|
||||
default: [0.05, 0.15, 0.25, 0.35, 0.45]
|
||||
|
||||
# Mixer
|
||||
mixer:
|
||||
@ -767,3 +824,20 @@ mixer:
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
|
||||
10: # Helicopter
|
||||
actuators:
|
||||
- actuator_type: 'motor'
|
||||
count: 1
|
||||
item_label_prefix: ['Rotor']
|
||||
- actuator_type: 'servo'
|
||||
item_label_prefix: ['Yaw tail servo']
|
||||
count: 1
|
||||
- actuator_type: 'servo'
|
||||
group_label: 'Swash plate servos'
|
||||
count: 'CA_SP0_COUNT'
|
||||
per_item_parameters:
|
||||
extra:
|
||||
- name: 'CA_SP0_ANG${i}'
|
||||
label: 'Angle'
|
||||
- name: 'CA_SP0_ARM_L${i}'
|
||||
label: 'Arm Length (relative)'
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user