forked from Archive/PX4-Autopilot
CA: refactor logic for matrix updating
-pass flag EffectivenessUpdateReason into effectiveness, indicating if there was an external update or not. Reasons for external updates are: -config changes (parameter) -motor failure detected or certain redundant motors are switched off to save energy Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
36d440f895
commit
df0e402c44
|
@ -57,6 +57,12 @@ enum class ActuatorType {
|
|||
COUNT
|
||||
};
|
||||
|
||||
enum class EffectivenessUpdateReason {
|
||||
NO_EXTERNAL_UPDATE = 0,
|
||||
CONFIGURATION_UPDATE = 1,
|
||||
MOTOR_ACTIVATION_UPDATE = 2,
|
||||
};
|
||||
|
||||
|
||||
class ActuatorEffectiveness
|
||||
{
|
||||
|
@ -147,7 +153,7 @@ public:
|
|||
*
|
||||
* @return true if updated and matrix is set
|
||||
*/
|
||||
virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0;
|
||||
virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
|
||||
|
||||
/**
|
||||
* Get the current flight phase
|
||||
|
|
|
@ -124,12 +124,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
|
|||
}
|
||||
}
|
||||
|
||||
bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration)
|
||||
{
|
||||
if (!force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _count; i++) {
|
||||
int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
|
||||
|
||||
|
|
|
@ -69,7 +69,7 @@ public:
|
|||
ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessControlSurfaces() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool addActuators(Configuration &configuration);
|
||||
|
||||
const char *name() const override { return "Control Surfaces"; }
|
||||
|
||||
|
|
|
@ -41,19 +41,20 @@ ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent)
|
|||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// motors
|
||||
_motors.enableYawControl(false);
|
||||
_motors.getEffectivenessMatrix(configuration, true);
|
||||
const bool motors_added_successfully = _motors.addActuators(configuration);
|
||||
|
||||
// Torque
|
||||
_torque.getEffectivenessMatrix(configuration, true);
|
||||
const bool torque_added_successfully = _torque.addActuators(configuration);
|
||||
|
||||
return true;
|
||||
return (motors_added_successfully && torque_added_successfully);
|
||||
}
|
||||
|
||||
|
|
|
@ -43,7 +43,7 @@ public:
|
|||
ActuatorEffectivenessCustom(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessCustom() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Custom"; }
|
||||
|
||||
|
|
|
@ -43,21 +43,22 @@ ActuatorEffectivenessFixedWing::ActuatorEffectivenessFixedWing(ModuleParams *par
|
|||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Motors
|
||||
_rotors.enableYawControl(false);
|
||||
_rotors.getEffectivenessMatrix(configuration, true);
|
||||
const bool rotors_added_successfully = _rotors.addActuators(configuration);
|
||||
|
||||
// Control Surfaces
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[0];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
|
||||
|
||||
return true;
|
||||
return (rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
|
|
|
@ -45,7 +45,7 @@ public:
|
|||
ActuatorEffectivenessFixedWing(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessFixedWing() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Fixed Wing"; }
|
||||
|
||||
|
|
|
@ -43,20 +43,21 @@ ActuatorEffectivenessMCTilt::ActuatorEffectivenessMCTilt(ModuleParams *parent)
|
|||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// MC motors
|
||||
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
|
||||
|
||||
// Tilts
|
||||
int first_tilt_idx = configuration.num_actuators_matrix[0];
|
||||
_tilts.updateTorqueSign(_mc_rotors.geometry());
|
||||
_tilts.getEffectivenessMatrix(configuration, true);
|
||||
const bool tilts_added_successfully = _tilts.addActuators(configuration);
|
||||
|
||||
// Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle).
|
||||
// Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated
|
||||
|
@ -72,7 +73,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
|
|||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
return (rotors_added_successfully && tilts_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
|
|
|
@ -43,7 +43,7 @@ public:
|
|||
ActuatorEffectivenessMCTilt(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessMCTilt() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
|
|
|
@ -0,0 +1,56 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-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 "ActuatorEffectivenessMultirotor.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent)
|
||||
: ModuleParams(parent),
|
||||
_mc_rotors(this)
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Motors
|
||||
const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
|
||||
|
||||
return rotors_added_successfully;
|
||||
}
|
|
@ -0,0 +1,61 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-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 "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectivenessMultirotor(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessMultirotor() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
|
||||
}
|
||||
|
||||
void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
|
||||
{
|
||||
normalize[0] = true;
|
||||
}
|
||||
|
||||
const char *name() const override { return "Multirotor"; }
|
||||
|
||||
protected:
|
||||
ActuatorEffectivenessRotors _mc_rotors;
|
||||
};
|
|
@ -136,24 +136,18 @@ void ActuatorEffectivenessRotors::updateParams()
|
|||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessRotors::addActuators(Configuration &configuration)
|
||||
{
|
||||
if (_updated || force) {
|
||||
_updated = false;
|
||||
|
||||
if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
|
||||
PX4_ERR("Wrong actuator ordering: servos need to be after motors");
|
||||
return false;
|
||||
}
|
||||
|
||||
int num_actuators = computeEffectivenessMatrix(_geometry,
|
||||
configuration.effectiveness_matrices[configuration.selected_matrix],
|
||||
configuration.num_actuators_matrix[configuration.selected_matrix]);
|
||||
configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
|
||||
return true;
|
||||
if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
|
||||
PX4_ERR("Wrong actuator ordering: servos need to be after motors");
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
int num_actuators = computeEffectivenessMatrix(_geometry,
|
||||
configuration.effectiveness_matrices[configuration.selected_matrix],
|
||||
configuration.num_actuators_matrix[configuration.selected_matrix]);
|
||||
configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
|
@ -93,7 +93,7 @@ public:
|
|||
static int computeEffectivenessMatrix(const Geometry &geometry,
|
||||
EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool addActuators(Configuration &configuration);
|
||||
|
||||
const char *name() const override { return "Multirotor"; }
|
||||
|
||||
|
@ -117,8 +117,6 @@ public:
|
|||
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
bool _updated{true};
|
||||
const AxisConfiguration _axis_config;
|
||||
const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
|
||||
|
||||
|
|
|
@ -37,9 +37,10 @@
|
|||
using namespace matrix;
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -41,7 +41,7 @@ public:
|
|||
ActuatorEffectivenessRoverAckermann() = default;
|
||||
virtual ~ActuatorEffectivenessRoverAckermann() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Rover (Ackermann)"; }
|
||||
private:
|
||||
|
|
|
@ -37,9 +37,10 @@
|
|||
using namespace matrix;
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -41,7 +41,7 @@ public:
|
|||
ActuatorEffectivenessRoverDifferential() = default;
|
||||
virtual ~ActuatorEffectivenessRoverDifferential() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Rover (Differential)"; }
|
||||
private:
|
||||
|
|
|
@ -42,15 +42,16 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParam
|
|||
}
|
||||
|
||||
bool
|
||||
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// MC motors
|
||||
configuration.selected_matrix = 0;
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
|
||||
_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
|
||||
|
||||
// Pusher/Puller
|
||||
|
@ -63,9 +64,9 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
|
|||
// Control Surfaces
|
||||
configuration.selected_matrix = 1;
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
|
||||
|
||||
return true;
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
|
|
|
@ -53,7 +53,7 @@ public:
|
|||
ActuatorEffectivenessStandardVTOL(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessStandardVTOL() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
const char *name() const override { return "Standard VTOL"; }
|
||||
|
||||
|
|
|
@ -47,24 +47,24 @@ ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleP
|
|||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
bool
|
||||
ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!force) {
|
||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// MC motors
|
||||
configuration.selected_matrix = 0;
|
||||
_mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
|
||||
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
|
||||
|
||||
// Control Surfaces
|
||||
configuration.selected_matrix = 1;
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
|
||||
|
||||
return true;
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
|
||||
|
|
|
@ -52,7 +52,7 @@ public:
|
|||
ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessTailsitterVTOL() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
int numMatrices() const override { return 2; }
|
||||
|
||||
|
@ -75,7 +75,6 @@ public:
|
|||
|
||||
uint32_t getStoppedMotors() const override { return _stopped_motors; }
|
||||
protected:
|
||||
bool _updated{true};
|
||||
ActuatorEffectivenessRotors _mc_rotors;
|
||||
ActuatorEffectivenessControlSurfaces _control_surfaces;
|
||||
|
||||
|
|
|
@ -51,9 +51,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar
|
|||
setFlightPhase(FlightPhase::HOVER_FLIGHT);
|
||||
}
|
||||
bool
|
||||
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
|
||||
EffectivenessUpdateReason external_update)
|
||||
{
|
||||
if (!_updated && !force) {
|
||||
if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -62,30 +63,31 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
|
|||
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
|
||||
|
||||
// Update matrix with tilts in vertical position when update is triggered by a manual
|
||||
// configuration (parameter) change (=force update). This is to make sure the normalization
|
||||
// scales are tilt-invariant. Note: force update only possible when disarm.
|
||||
const float tilt_control_applied = force ? -1.f : _last_tilt_control;
|
||||
// configuration (parameter) change. This is to make sure the normalization
|
||||
// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
|
||||
const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
|
||||
_last_tilt_control;
|
||||
_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
|
||||
<< configuration.num_actuators[(int)ActuatorType::MOTORS];
|
||||
|
||||
_mc_rotors.getEffectivenessMatrix(configuration, true);
|
||||
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
|
||||
|
||||
// Control Surfaces
|
||||
configuration.selected_matrix = 1;
|
||||
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_control_surfaces.getEffectivenessMatrix(configuration, true);
|
||||
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
|
||||
|
||||
// Tilts
|
||||
configuration.selected_matrix = 0;
|
||||
_first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
|
||||
_tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */);
|
||||
_tilts.getEffectivenessMatrix(configuration, true);
|
||||
const bool tilts_added_successfully = _tilts.addActuators(configuration);
|
||||
|
||||
// If it was a forced update (thus coming from a config change), then make sure to update matrix in
|
||||
// If it was an update coming from a config change, then make sure to update matrix in
|
||||
// the next iteration again with the correct tilt (but without updating the normalization scale).
|
||||
_updated = force;
|
||||
_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
|
||||
|
||||
return true;
|
||||
return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
|
||||
}
|
||||
|
||||
void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
||||
|
@ -114,7 +116,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
|
|||
_last_tilt_control = control_tilt;
|
||||
|
||||
} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
|
||||
_updated = true;
|
||||
_combined_tilt_updated = true;
|
||||
_last_tilt_control = control_tilt;
|
||||
}
|
||||
|
||||
|
|
|
@ -55,7 +55,7 @@ public:
|
|||
ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
||||
|
||||
int numMatrices() const override { return 2; }
|
||||
|
||||
|
@ -81,7 +81,7 @@ public:
|
|||
|
||||
uint32_t getStoppedMotors() const override { return _stopped_motors; }
|
||||
protected:
|
||||
bool _updated{true};
|
||||
bool _combined_tilt_updated{true};
|
||||
ActuatorEffectivenessRotors _mc_rotors;
|
||||
ActuatorEffectivenessControlSurfaces _control_surfaces;
|
||||
ActuatorEffectivenessTilts _tilts;
|
||||
|
|
|
@ -84,12 +84,8 @@ void ActuatorEffectivenessTilts::updateParams()
|
|||
}
|
||||
}
|
||||
|
||||
bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force)
|
||||
bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration)
|
||||
{
|
||||
if (!force) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _count; i++) {
|
||||
configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{});
|
||||
}
|
||||
|
|
|
@ -67,7 +67,7 @@ public:
|
|||
ActuatorEffectivenessTilts(ModuleParams *parent);
|
||||
virtual ~ActuatorEffectivenessTilts() = default;
|
||||
|
||||
bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
|
||||
bool addActuators(Configuration &configuration);
|
||||
|
||||
const char *name() const override { return "Tilts"; }
|
||||
|
||||
|
|
|
@ -42,6 +42,8 @@ px4_add_library(ActuatorEffectiveness
|
|||
ActuatorEffectivenessFixedWing.hpp
|
||||
ActuatorEffectivenessMCTilt.cpp
|
||||
ActuatorEffectivenessMCTilt.hpp
|
||||
ActuatorEffectivenessMultirotor.cpp
|
||||
ActuatorEffectivenessMultirotor.hpp
|
||||
ActuatorEffectivenessTilts.cpp
|
||||
ActuatorEffectivenessTilts.hpp
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
|
|
|
@ -124,7 +124,7 @@ ControlAllocator::parameters_updated()
|
|||
_control_allocation[i]->updateParameters();
|
||||
}
|
||||
|
||||
update_effectiveness_matrix_if_needed(true);
|
||||
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -208,7 +208,7 @@ ControlAllocator::update_effectiveness_source()
|
|||
switch (source) {
|
||||
case EffectivenessSource::NONE:
|
||||
case EffectivenessSource::MULTIROTOR:
|
||||
tmp = new ActuatorEffectivenessRotors(this);
|
||||
tmp = new ActuatorEffectivenessMultirotor(this);
|
||||
break;
|
||||
|
||||
case EffectivenessSource::STANDARD_VTOL:
|
||||
|
@ -362,7 +362,7 @@ ControlAllocator::Run()
|
|||
if (do_update) {
|
||||
_last_run = now;
|
||||
|
||||
update_effectiveness_matrix_if_needed();
|
||||
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
|
||||
|
||||
// Set control setpoint vector(s)
|
||||
matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
|
||||
|
@ -414,15 +414,16 @@ ControlAllocator::Run()
|
|||
}
|
||||
|
||||
void
|
||||
ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
|
||||
ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
|
||||
{
|
||||
ActuatorEffectiveness::Configuration config{};
|
||||
|
||||
if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
|
||||
if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE
|
||||
&& hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
|
||||
return;
|
||||
}
|
||||
|
||||
if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) {
|
||||
if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) {
|
||||
_last_effectiveness_update = hrt_absolute_time();
|
||||
|
||||
memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes,
|
||||
|
@ -496,7 +497,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
|
|||
// Assign control effectiveness matrix
|
||||
int total_num_actuators = config.num_actuators_matrix[i];
|
||||
_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
|
||||
config.linearization_point[i], total_num_actuators, force);
|
||||
config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
|
||||
}
|
||||
|
||||
trims.timestamp = hrt_absolute_time();
|
||||
|
|
|
@ -42,7 +42,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <ActuatorEffectiveness.hpp>
|
||||
#include <ActuatorEffectivenessRotors.hpp>
|
||||
#include <ActuatorEffectivenessMultirotor.hpp>
|
||||
#include <ActuatorEffectivenessStandardVTOL.hpp>
|
||||
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
|
||||
#include <ActuatorEffectivenessTailsitterVTOL.hpp>
|
||||
|
@ -125,7 +125,7 @@ private:
|
|||
void update_allocation_method(bool force);
|
||||
bool update_effectiveness_source();
|
||||
|
||||
void update_effectiveness_matrix_if_needed(bool force = false);
|
||||
void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
|
||||
|
||||
void publish_control_allocator_status();
|
||||
|
||||
|
|
Loading…
Reference in New Issue