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

control_allocator: implement trim + slew rate limits configuration

Browse Source
This commit is contained in:
Beat Küng 2021-12-03 11:52:01 +01:00 committed by Daniel Agar
parent 301100ce0e
commit 4c80adfaf1
17 changed files with 224 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -44,6 +44,7 @@ set(msg_files
actuator_motors.msg
actuator_outputs.msg
actuator_servos.msg
actuator_servos_trim.msg
actuator_test.msg
adc_report.msg
airspeed.msg

5
msg/actuator_servos_trim.msg Normal file
View File

@ -0,0 +1,5 @@
# Servo trims, added as offset to servo outputs
uint64 timestamp # time since system start (microseconds)
uint8 NUM_CONTROLS = 8
float32[8] trim # range: [-1, 1]

11
src/lib/mixer_module/actuator_test.cpp
View File

@ -81,6 +81,17 @@ void ActuatorTest::update(int num_outputs, float thrust_curve)
FunctionMotors::updateValues(motors.reversible_flags >> motor_idx, thrust_curve, &value, 1);
}
// handle servos: add trim
if (actuator_test.function >= (int)OutputFunction::Servo1 && actuator_test.function <= (int)OutputFunction::ServoMax) {
actuator_servos_trim_s trim{};
_actuator_servos_trim_sub.copy(&trim);
int idx = actuator_test.function - (int)OutputFunction::Servo1;
if (idx < actuator_servos_trim_s::NUM_CONTROLS) {
value += trim.trim[idx];
}
}
_current_outputs[i] = value;
} else {

2
src/lib/mixer_module/actuator_test.hpp
View File

@ -38,6 +38,7 @@
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos_trim.h>
#include <uORB/Subscription.hpp>
static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch");
@ -66,6 +67,7 @@ private:
uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Subscription _actuator_servos_trim_sub{ORB_ID(actuator_servos_trim)};
bool _in_test_mode{false};
hrt_abstime _next_timeout{0};

2
src/lib/mixer_module/motor_params.c
View File

@ -44,7 +44,7 @@
*
* Minimum time allowed for the motor input signal to pass through
* a range of 1000 PWM units. A value x means that the motor signal
* can only go from 1000 to 2000 PWM in maximum x seconds.
* can only go from 1000 to 2000 PWM in minimum x seconds.
*
* Zero means that slew rate limiting is disabled.
*

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
View File

@ -98,6 +98,8 @@ public:
int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
ActuatorVector trim[MAX_NUM_MATRICES];
ActuatorVector linearization_point[MAX_NUM_MATRICES];
int selected_matrix;
uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];

10
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
View File

@ -50,6 +50,8 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
_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);
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
_param_handles[i].trim = param_find(buffer);
}
_count_handle = param_find("CA_SV_CS_COUNT");
@ -78,6 +80,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
param_get(_param_handles[i].torque[n], &torque(n));
}
param_get(_param_handles[i].trim, &_params[i].trim);
// TODO: enforce limits?
switch (_params[i].type) {
case Type::Elevator:
@ -126,7 +130,11 @@ bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration
}
for (int i = 0; i < _count; i++) {
configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
if (actuator_idx >= 0) {
configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim;
}
}
return true;

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp
View File

@ -63,6 +63,7 @@ public:
Type type;
matrix::Vector3f torque;
float trim;
};
ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
@ -86,6 +87,7 @@ private:
param_t type;
param_t torque[3];
param_t trim;
};
ParamHandles _param_handles[MAX_COUNT];
param_t _count_handle;

30
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
View File

@ -51,18 +51,15 @@ ControlAllocation::ControlAllocation()
void
ControlAllocation::setEffectivenessMatrix(
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim, int num_actuators)
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators)
{
_effectiveness = effectiveness;
_actuator_trim = actuator_trim;
ActuatorVector linearization_point_clipped = linearization_point;
clipActuatorSetpoint(linearization_point_clipped);
_actuator_trim = actuator_trim + linearization_point_clipped;
clipActuatorSetpoint(_actuator_trim);
_control_trim = _effectiveness * _actuator_trim;
_num_actuators = num_actuators;
// make sure unused actuators are initialized to trim
for (int i = num_actuators; i < NUM_ACTUATORS; ++i) {
_actuator_sp(i) = _actuator_trim(i);
}
_control_trim = _effectiveness * linearization_point_clipped;
}
void
@ -109,3 +106,20 @@ const
return actuator_normalized;
}
void ControlAllocation::applySlewRateLimit(float dt)
{
for (int i = 0; i < _num_actuators; i++) {
if (_actuator_slew_rate_limit(i) > FLT_EPSILON) {
float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i);
float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i);
if (delta_sp > delta_sp_max) {
_actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max;
} else if (delta_sp < -delta_sp_max) {
_actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max;
}
}
}
}

28
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
View File

@ -104,7 +104,7 @@ public:
* @param B Effectiveness matrix
*/
virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators);
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators);
/**
* Get the allocated actuator vector
@ -181,6 +181,14 @@ public:
*/
void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
{ _actuator_slew_rate_limit = slew_rate_limit; }
/**
* Apply slew rate to current actuator setpoint
*/
void applySlewRateLimit(float dt);
/**
* Clip the actuator setpoint between minimum and maximum values.
*
@ -211,13 +219,15 @@ public:
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
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; //< Minimum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; //< Maximum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_sp; //< Actuator setpoint
matrix::Vector<float, NUM_AXES> _control_sp; //< Control setpoint
matrix::Vector<float, NUM_AXES> _control_trim; //< Control at trim actuator values
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
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; ///< Minimum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; ///< Maximum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; ///< Slew rate limit
matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp; ///< Previous actuator setpoint
matrix::Vector<float, NUM_ACTUATORS> _actuator_sp; ///< Actuator setpoint
matrix::Vector<float, NUM_AXES> _control_sp; ///< Control setpoint
matrix::Vector<float, NUM_AXES> _control_trim; ///< Control at trim actuator values
int _num_actuators{0};
};

6
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
View File

@ -44,9 +44,9 @@
void
ControlAllocationPseudoInverse::setEffectivenessMatrix(
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim, int num_actuators)
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators)
{
ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators);
ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators);
_mix_update_needed = true;
}
@ -120,6 +120,8 @@ ControlAllocationPseudoInverse::allocate()
//Compute new gains if needed
updatePseudoInverse();
_prev_actuator_sp = _actuator_sp;
// Allocate
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
}

2
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
View File

@ -55,7 +55,7 @@ public:
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;
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) override;
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;

3
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp
View File

@ -54,9 +54,10 @@ TEST(ControlAllocationTest, AllZeroCase)
matrix::Matrix<float, 6, 16> effectiveness;
matrix::Vector<float, 16> actuator_sp;
matrix::Vector<float, 16> actuator_trim;
matrix::Vector<float, 16> linearization_point;
matrix::Vector<float, 16> actuator_sp_expected;
method.setEffectivenessMatrix(effectiveness, actuator_trim, 16);
method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16);
method.setControlSetpoint(control_sp);
method.allocate();
method.clipActuatorSetpoint();

2
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
View File

@ -47,6 +47,8 @@ ControlAllocationSequentialDesaturation::allocate()
//Compute new gains if needed
updatePseudoInverse();
_prev_actuator_sp = _actuator_sp;
switch (_param_mc_airmode.get()) {
case 1:
mixAirmodeRP();

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

@ -54,6 +54,18 @@ ControlAllocator::ControlAllocator() :
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
_param_handles.slew_rate_motors[i] = param_find(buffer);
}
for (int i = 0; i < MAX_NUM_SERVOS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_SV%u_SLEW", i);
_param_handles.slew_rate_servos[i] = param_find(buffer);
}
parameters_updated();
}
@ -87,6 +99,18 @@ ControlAllocator::init()
void
ControlAllocator::parameters_updated()
{
_has_slew_rate = false;
for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
param_get(_param_handles.slew_rate_motors[i], &_params.slew_rate_motors[i]);
_has_slew_rate |= _params.slew_rate_motors[i] > FLT_EPSILON;
}
for (int i = 0; i < MAX_NUM_SERVOS; ++i) {
param_get(_param_handles.slew_rate_servos[i], &_params.slew_rate_servos[i]);
_has_slew_rate |= _params.slew_rate_servos[i] > FLT_EPSILON;
}
// Allocation method & effectiveness source
// Do this first: in case a new method is loaded, it will be configured below
bool updated = update_effectiveness_source();
@ -343,6 +367,11 @@ ControlAllocator::Run()
// Do allocation
_control_allocation[i]->allocate();
_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp);
if (_has_slew_rate) {
_control_allocation[i]->applySlewRateLimit(dt);
}
_control_allocation[i]->clipActuatorSetpoint();
}
@ -378,14 +407,19 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
// Get the minimum and maximum depending on type and configuration
ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES];
ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES];
ActuatorEffectiveness::ActuatorVector slew_rate[ActuatorEffectiveness::MAX_NUM_MATRICES];
int actuator_idx = 0;
int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
actuator_servos_trim_s trims{};
static_assert(actuator_servos_trim_s::NUM_CONTROLS == actuator_servos_s::NUM_CONTROLS, "size mismatch");
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) {
_num_actuators[actuator_type] = 0;
PX4_ERR("Too many actuators");
break;
}
@ -393,6 +427,12 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
if ((ActuatorType)actuator_type == ActuatorType::MOTORS) {
if (actuator_type_idx >= MAX_NUM_MOTORS) {
PX4_ERR("Too many motors");
_num_actuators[actuator_type] = 0;
break;
}
if (_param_r_rev.get() & (1u << actuator_type_idx)) {
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
@ -400,11 +440,25 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f;
}
slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx];
} else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) {
if (actuator_type_idx >= MAX_NUM_SERVOS) {
PX4_ERR("Too many servos");
_num_actuators[actuator_type] = 0;
break;
}
minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_servos[actuator_type_idx];
trims.trim[actuator_type_idx] = config.trim[selected_matrix](actuator_idx_matrix[selected_matrix]);
} 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;
}
@ -413,11 +467,16 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
for (int i = 0; i < _num_control_allocation; ++i) {
_control_allocation[i]->setActuatorMin(minimum[i]);
_control_allocation[i]->setActuatorMax(maximum[i]);
_control_allocation[i]->setSlewRateLimit(slew_rate[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);
_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
config.linearization_point[i], total_num_actuators);
}
trims.timestamp = hrt_absolute_time();
_actuator_servos_trim_pub.publish(trims);
}
}

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

@ -64,6 +64,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/actuator_servos_trim.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
@ -76,6 +77,9 @@ public:
static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
static constexpr int NUM_AXES = ControlAllocation::NUM_AXES;
static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS;
static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS;
using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
ControlAllocator();
@ -100,6 +104,16 @@ public:
private:
struct ParamHandles {
param_t slew_rate_motors[MAX_NUM_MOTORS];
param_t slew_rate_servos[MAX_NUM_SERVOS];
};
struct Params {
float slew_rate_motors[MAX_NUM_MOTORS];
float slew_rate_servos[MAX_NUM_SERVOS];
};
/**
* initialize some vectors/matrices from parameters
*/
@ -147,6 +161,7 @@ private:
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<actuator_servos_trim_s> _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -161,6 +176,10 @@ private:
hrt_abstime _timestamp_sample{0};
hrt_abstime _last_status_pub{0};
ParamHandles _param_handles{};
Params _params{};
bool _has_slew_rate{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,

62
src/modules/control_allocator/module.yaml
View File

@ -53,6 +53,42 @@ parameters:
6: Motor 7
7: Motor 8
default: 0
CA_R${i}_SLEW:
description:
short: Motor ${i} slew rate limit
long: |
Minimum time allowed for the motor input signal to pass through
the full output range. A value x means that the motor signal
can only go from 0 to 1 in minimum x seconds (in case of
reversible motors, the range is -1 to 1).
Zero means that slew rate limiting is disabled.
type: float
decimal: 2
increment: 0.01
num_instances: *max_num_mc_motors
min: 0
max: 10
default: 0.0
# Servo params
CA_SV${i}_SLEW:
description:
short: Servo ${i} slew rate limit
long: |
Minimum time allowed for the servo input signal to pass through
the full output range. A value x means that the servo signal
can only go from -1 to 1 in minimum x seconds.
Zero means that slew rate limiting is disabled.
type: float
decimal: 2
increment: 0.05
num_instances: *max_num_servos
min: 0
max: 10
default: 0.0
# (MC) Rotors
CA_ROTOR_COUNT:
@ -246,6 +282,17 @@ parameters:
num_instances: *max_num_servos
instance_start: 0
default: 0.0
CA_SV_CS${i}_TRIM:
description:
short: Control Surface ${i} trim
long: Can be used to add an offset to the servo control.
type: float
decimal: 2
min: -1.0
max: 1.0
num_instances: *max_num_servos
instance_start: 0
default: 0.0
# Tilts
CA_SV_TL_COUNT:
@ -341,12 +388,21 @@ mixer:
show_as: 'bitset'
index_offset: 0
advanced: true
- name: 'CA_R${i}_SLEW'
label: 'Slew Rate'
index_offset: 0
advanced: true
servo:
functions: 'Servo'
actuator_testing_values:
min: -1
max: 1
default: 0
per_item_parameters:
- name: 'CA_SV${i}_SLEW'
label: 'Slew Rate'
index_offset: 0
advanced: true
DEFAULT:
actuator_testing_values:
min: -1
@ -427,6 +483,8 @@ mixer:
- name: 'CA_SV_CS${i}_TRQ_Y'
label: 'Yaw Scale'
identifier: 'servo-torque-yaw'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
2: # Tiltrotor VTOL
actuators:
@ -458,6 +516,8 @@ mixer:
- name: 'CA_SV_CS${i}_TRQ_Y'
label: 'Yaw Torque'
identifier: 'servo-torque-yaw'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
- actuator_type: 'servo'
group_label: 'Tilt Servos'
count: 'CA_SV_TL_COUNT'
@ -519,5 +579,7 @@ mixer:
- name: 'CA_SV_CS${i}_TRQ_Y'
label: 'Yaw Torque'
identifier: 'servo-torque-yaw'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'