control_allocation: Consistently replace with ActuatorVector alias for readability

This commit is contained in:
Matthias Grob 2025-04-25 17:41:12 +02:00
parent 6a3a0d136b
commit 53efcbd2c2
23 changed files with 56 additions and 77 deletions

View File

@ -195,9 +195,8 @@ public:
* 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, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) {}
/**
* Get a bitmask of motors to be stopped

View File

@ -82,7 +82,7 @@ public:
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;
using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
enum ControlAxis {
ROLL = 0,
@ -121,7 +121,7 @@ public:
*
* @return Actuator vector
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const { return _actuator_sp; }
const ActuatorVector &getActuatorSetpoint() const { return _actuator_sp; }
/**
* Set the desired control vector
@ -157,28 +157,28 @@ public:
*
* @param actuator_min Minimum actuator values
*/
void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min) { _actuator_min = actuator_min; }
void setActuatorMin(const ActuatorVector &actuator_min) { _actuator_min = actuator_min; }
/**
* Get the minimum actuator values
*
* @return Minimum actuator values
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const { return _actuator_min; }
const ActuatorVector &getActuatorMin() const { return _actuator_min; }
/**
* Set the maximum actuator values
*
* @param actuator_max Maximum actuator values
*/
void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) { _actuator_max = actuator_max; }
void setActuatorMax(const ActuatorVector &actuator_max) { _actuator_max = actuator_max; }
/**
* Get the maximum actuator values
*
* @return Maximum actuator values
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const { return _actuator_max; }
const ActuatorVector &getActuatorMax() const { return _actuator_max; }
/**
* Set the current actuator setpoint.
@ -189,9 +189,9 @@ public:
*
* @param actuator_sp Actuator setpoint
*/
void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
void setActuatorSetpoint(const ActuatorVector &actuator_sp);
void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
void setSlewRateLimit(const ActuatorVector &slew_rate_limit)
{ _actuator_slew_rate_limit = slew_rate_limit; }
/**
@ -206,7 +206,7 @@ public:
*
* @param actuator Actuator vector to clip
*/
void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
void clipActuatorSetpoint(ActuatorVector &actuator) const;
void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
@ -219,7 +219,7 @@ public:
*
* @return Clipped actuator setpoint
*/
matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
ActuatorVector normalizeActuatorSetpoint(const ActuatorVector &actuator)
const;
virtual void updateParameters() {}
@ -233,12 +233,12 @@ protected:
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
ActuatorVector _actuator_trim; ///< Neutral actuator values
ActuatorVector _actuator_min; ///< Minimum actuator values
ActuatorVector _actuator_max; ///< Maximum actuator values
ActuatorVector _actuator_slew_rate_limit; ///< Slew rate limit
ActuatorVector _prev_actuator_sp; ///< Previous actuator setpoint
ActuatorVector _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};

View File

@ -145,7 +145,7 @@ ControlAllocator::update_allocation_method(bool force)
if (_allocation_method_id != configured_method || force) {
matrix::Vector<float, NUM_ACTUATORS> actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES];
ActuatorVector actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES];
// Cleanup first
for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
@ -626,9 +626,9 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
// Actuator saturation
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[matrix_index]->getActuatorMin();
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[matrix_index]->getActuatorMax();
const ActuatorVector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
const ActuatorVector &actuator_min = _control_allocation[matrix_index]->getActuatorMin();
const ActuatorVector &actuator_max = _control_allocation[matrix_index]->getActuatorMax();
for (int i = 0; i < NUM_ACTUATORS; i++) {
if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {

View File

@ -59,9 +59,8 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
return (motors_added_successfully && torque_added_successfully);
}
void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}

View File

@ -45,9 +45,8 @@ public:
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
const char *name() const override { return "Custom"; }

View File

@ -61,9 +61,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
return (rotors_added_successfully && surfaces_added_successfully);
}
void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
}

View File

@ -51,9 +51,8 @@ public:
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
private:
ActuatorEffectivenessRotors _rotors;

View File

@ -144,8 +144,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf
}
void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
_saturation_flags = {};

View File

@ -81,9 +81,8 @@ public:
const Geometry &geometry() const { return _geometry; }
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:

View File

@ -102,8 +102,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
}
void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
_saturation_flags = {};

View File

@ -69,9 +69,8 @@ public:
const Geometry &geometry() const { return _geometry; }
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:

View File

@ -76,9 +76,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
return (rotors_added_successfully && tilts_added_successfully);
}
void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
actuator_sp += _tilt_offsets;
// TODO: dynamic matrix update

View File

@ -55,9 +55,8 @@ public:
normalize[0] = true;
}
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
const char *name() const override { return "MC Tilt"; }

View File

@ -50,8 +50,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
}
void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}

View File

@ -43,9 +43,8 @@ public:
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
const char *name() const override { return "Rover (Ackermann)"; }
private:

View File

@ -84,8 +84,7 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
}
void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
if (matrix_index == 0) {
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);

View File

@ -75,9 +75,8 @@ public:
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;

View File

@ -89,8 +89,7 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
}
void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
if (matrix_index == 0) {
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);

View File

@ -72,9 +72,8 @@ public:
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;

View File

@ -125,8 +125,7 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
}
void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
// apply tilt
if (matrix_index == 0) {

View File

@ -80,9 +80,8 @@ public:
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
const char *name() const override { return "VTOL Tiltrotor"; }

View File

@ -55,9 +55,8 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati
return rotors_added_successfully;
}
void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}

View File

@ -54,9 +54,8 @@ public:
normalize[0] = true;
}
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
const char *name() const override { return "UUV"; }