fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 14:50:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
+22 -8
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;
}
}
}
}
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+19 -9
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};
};
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
+4 -2
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);
}
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
+1 -1
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;
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp
+2 -1
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();
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
+2
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();