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

CA: reload CA matrix to newly created CA class properly

Browse Source 
Also remove the dependency of ActuatorEffectivenessMultirotor
to param update uORB topic; the CA module
sends a "force" parameter when needed
This commit is contained in:
bresch 2021-08-17 09:53:39 +02:00 committed by Daniel Agar
parent cf5917d188
commit f09b34007e
9 changed files with 28 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,7 +77,7 @@ public:
*
* @return true if updated and matrix is set
*/
virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) = 0;
/**
* Get the actuator trims

16
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
View File

@ -41,21 +41,16 @@
#include "ActuatorEffectivenessMultirotor.hpp"
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
ModuleParams(nullptr)
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent):
ModuleParams(parent)
{
}
bool
ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
// Check if parameters have changed
if (_updated || _parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
if (_updated || force) {
_updated = false;
// Get multirotor geometry
@ -148,6 +143,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
effectiveness.setZero();
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
// Get rotor axis
matrix::Vector3f axis(
geometry.rotors[i].axis_x,

7
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
View File

@ -46,14 +46,13 @@
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
{
public:
ActuatorEffectivenessMultirotor();
ActuatorEffectivenessMultirotor(ModuleParams *parent);
virtual ~ActuatorEffectivenessMultirotor() = default;
static constexpr int NUM_ROTORS_MAX = 8;
@ -76,12 +75,10 @@ public:
static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
int numActuators() const override { return _num_actuators; }
private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
bool _updated{true};
int _num_actuators{0};

5
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
View File

@ -47,9 +47,10 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
}
bool
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
if (!_updated) {
if (!(_updated || force)) {
return false;
}

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

@ -49,7 +49,7 @@ public:
ActuatorEffectivenessStandardVTOL();
virtual ~ActuatorEffectivenessStandardVTOL() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
/**
* Set the current flight phase

5
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -46,9 +46,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
if (!_updated) {
if (!(_updated || force)) {
return false;
}

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

@ -49,7 +49,7 @@ public:
ActuatorEffectivenessTiltrotorVTOL();
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
/**
* Set the current flight phase

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

@ -132,6 +132,9 @@ ControlAllocator::parameters_updated()
actuator_max(14) = _param_ca_act14_max.get();
actuator_max(15) = _param_ca_act15_max.get();
_control_allocation->setActuatorMax(actuator_max);
_control_allocation->updateParameters();
update_effectiveness_matrix_if_needed(true);
}
void
@ -198,7 +201,7 @@ ControlAllocator::update_effectiveness_source()
switch (source) {
case EffectivenessSource::NONE:
case EffectivenessSource::MULTIROTOR:
tmp = new ActuatorEffectivenessMultirotor();
tmp = new ActuatorEffectivenessMultirotor(this);
break;
case EffectivenessSource::STANDARD_VTOL:
@ -245,18 +248,12 @@ ControlAllocator::Run()
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
updateParams();
parameters_updated();
if (_control_allocation) {
_control_allocation->updateParameters();
}
update_effectiveness_matrix_if_needed();
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
}
if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) {
@ -351,11 +348,12 @@ ControlAllocator::Run()
}
void
ControlAllocator::update_effectiveness_matrix_if_needed()
ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
{
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) {
if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) {
const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)

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

@ -105,7 +105,7 @@ private:
void update_allocation_method();
void update_effectiveness_source();
void update_effectiveness_matrix_if_needed();
void update_effectiveness_matrix_if_needed(bool force = false);
void publish_actuator_setpoint();
void publish_control_allocator_status();