fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 22:20:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
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(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)