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

ControlAllocator: Move timing into main loop

Browse Source 
This separates concerns more neatly:
 - The main loop decides when and for what reason the effectiveness
   update should be called
 - update_effectiveness_matrix_if_needed updates it (if the given reason
   demands it)
This commit is contained in:
Balduin
2026-01-08 10:22:16 +01:00
parent 1f7a8048cf
commit f6e64bf037
1 changed files with 18 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+18 -31
View File
@@ -398,7 +398,24 @@ ControlAllocator::Run()
_last_run = now;
check_for_motor_failures();
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
const hrt_abstime time_since_last_effectiveness_udpate = now - _last_effectiveness_update;
// If the above check_for_motor_failures has already triggered
// an effectiveness update, these timing checks all return false
// and no additional update is done. This is by design, because
// an effectiveness update due to MOTOR_ACTIVATION_UPDATE also
// updates the battery scale and directions of tiltable rotors.
if (time_since_last_effectiveness_udpate > 1_s) {
// Update battery scaling at relatively low rate
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::BATTERY_SCALE_UPDATE);
} else if (time_since_last_effectiveness_udpate > 100_ms) {
// Update periodically for "no external" reason,
// allowing tiltrotor directions to update.
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
}
// Set control setpoint vector(s)
matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
@@ -464,36 +481,6 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
{
ActuatorEffectiveness::Configuration config{};
const hrt_abstime time_since_last_update = hrt_elapsed_time(&_last_effectiveness_update);
if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
if (time_since_last_update < 100_ms) {
return;
} else if (time_since_last_update > 1_s) {
// Kind of ugly way to force updating it due to battery scale at ~1Hz
// The only way anything happens with NO_EXTERNAL_UPDATE
// is in ActuatorEffectivenessTiltrotorVTOL, where we
// additionally watch the collective tilt update:
// if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
// return false;
// }
// Therefore, changing it once per second to
// BATTERY_SCALE_UPDATE should have no effect, as the
// Tiltrotor class will still just update a bit more
// often but never less.
// It is still needed though as all the other classes
// (non-tilting motors) do nothing when
// NO_EXTERNAL_UPDATE is given.
reason = EffectivenessUpdateReason::BATTERY_SCALE_UPDATE;
}
}
if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) {
_last_effectiveness_update = hrt_absolute_time();