fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 05:13:34 +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-09-11 15:07:49 -04:00
committed by Daniel Agar
parent cf5917d188
commit f09b34007e
9 changed files with 28 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
+1 -1
View File
@@ -77,7 +77,7 @@ public:
* *
* @return true if updated and matrix is set * @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 * Get the actuator trims
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
+6 -10
View File
@@ -41,21 +41,16 @@
#include "ActuatorEffectivenessMultirotor.hpp" #include "ActuatorEffectivenessMultirotor.hpp"
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent):
ModuleParams(nullptr) ModuleParams(parent)
{ {
} }
bool 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 || force) {
if (_updated || _parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
_updated = false; _updated = false;
// Get multirotor geometry // Get multirotor geometry
@@ -148,6 +143,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
effectiveness.setZero(); effectiveness.setZero();
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
// Get rotor axis // Get rotor axis
matrix::Vector3f axis( matrix::Vector3f axis(
geometry.rotors[i].axis_x, geometry.rotors[i].axis_x,
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
+2 -5
View File
@@ -46,14 +46,13 @@
#include <px4_platform_common/module_params.h> #include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp> #include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals; using namespace time_literals;
class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
{ {
public: public:
ActuatorEffectivenessMultirotor(); ActuatorEffectivenessMultirotor(ModuleParams *parent);
virtual ~ActuatorEffectivenessMultirotor() = default; virtual ~ActuatorEffectivenessMultirotor() = default;
static constexpr int NUM_ROTORS_MAX = 8; static constexpr int NUM_ROTORS_MAX = 8;
@@ -76,12 +75,10 @@ public:
static int computeEffectivenessMatrix(const MultirotorGeometry &geometry, static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness); 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; } int numActuators() const override { return _num_actuators; }
private: private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
bool _updated{true}; bool _updated{true};
int _num_actuators{0}; int _num_actuators{0};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+3 -2
View File
@@ -47,9 +47,10 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
} }
bool 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; return false;
} }
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+1 -1
View File
@@ -49,7 +49,7 @@ public:
ActuatorEffectivenessStandardVTOL(); ActuatorEffectivenessStandardVTOL();
virtual ~ActuatorEffectivenessStandardVTOL() = default; 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 * Set the current flight phase
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+3 -2
View File
@@ -46,9 +46,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
setFlightPhase(FlightPhase::HOVER_FLIGHT); setFlightPhase(FlightPhase::HOVER_FLIGHT);
} }
bool 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; return false;
} }
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+1 -1
View File
@@ -49,7 +49,7 @@ public:
ActuatorEffectivenessTiltrotorVTOL(); ActuatorEffectivenessTiltrotorVTOL();
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; 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 * Set the current flight phase
src/modules/control_allocator/ControlAllocator.cpp
+10 -12
View File
@@ -132,6 +132,9 @@ ControlAllocator::parameters_updated()
actuator_max(14) = _param_ca_act14_max.get(); actuator_max(14) = _param_ca_act14_max.get();
actuator_max(15) = _param_ca_act15_max.get(); actuator_max(15) = _param_ca_act15_max.get();
_control_allocation->setActuatorMax(actuator_max); _control_allocation->setActuatorMax(actuator_max);
_control_allocation->updateParameters();
update_effectiveness_matrix_if_needed(true);
} }
void void
@@ -198,7 +201,7 @@ ControlAllocator::update_effectiveness_source()
switch (source) { switch (source) {
case EffectivenessSource::NONE: case EffectivenessSource::NONE:
case EffectivenessSource::MULTIROTOR: case EffectivenessSource::MULTIROTOR:
tmp = new ActuatorEffectivenessMultirotor(); tmp = new ActuatorEffectivenessMultirotor(this);
break; break;
case EffectivenessSource::STANDARD_VTOL: case EffectivenessSource::STANDARD_VTOL:
@@ -245,18 +248,12 @@ ControlAllocator::Run()
// Check if parameters have changed // Check if parameters have changed
if (_parameter_update_sub.updated()) { if (_parameter_update_sub.updated()) {
updateParams();
parameters_updated();
if (_control_allocation) {
_control_allocation->updateParameters();
}
update_effectiveness_matrix_if_needed();
// clear update // clear update
parameter_update_s param_update; parameter_update_s param_update;
_parameter_update_sub.copy(&param_update); _parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
} }
if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) { if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) {
@@ -351,11 +348,12 @@ ControlAllocator::Run()
} }
void void
ControlAllocator::update_effectiveness_matrix_if_needed() ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
{ {
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness; 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(); const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
// Set 0 effectiveness for actuators that are disabled (act_min >= act_max) // Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
src/modules/control_allocator/ControlAllocator.hpp
+1 -1
View File
@@ -105,7 +105,7 @@ private:
void update_allocation_method(); void update_allocation_method();
void update_effectiveness_source(); 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_actuator_setpoint();
void publish_control_allocator_status(); void publish_control_allocator_status();