fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 21:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

CA: refactor logic for matrix updating

Browse Source 
-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:
Silvan Fuhrer
2022-02-01 10:36:54 +01:00
committed by Beat Küng
parent 36d440f895
commit df0e402c44
28 changed files with 209 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+8 -7
View File
@@ -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();