fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 03:03:33 +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-14 09:23:10 +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/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
+6 -5
View File
@@ -43,20 +43,21 @@ ActuatorEffectivenessMCTilt::ActuatorEffectivenessMCTilt(ModuleParams *parent)
}
bool
ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, bool force)
ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration,
EffectivenessUpdateReason external_update)
{
if (!force) {
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
return false;
}
// MC motors
_mc_rotors.enableYawControl(!_tilts.hasYawControl());
_mc_rotors.getEffectivenessMatrix(configuration, true);
const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
// Tilts
int first_tilt_idx = configuration.num_actuators_matrix[0];
_tilts.updateTorqueSign(_mc_rotors.geometry());
_tilts.getEffectivenessMatrix(configuration, true);
const bool tilts_added_successfully = _tilts.addActuators(configuration);
// Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle).
// Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated
@@ -72,7 +73,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
}
}
return true;
return (rotors_added_successfully && tilts_added_successfully);
}
void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,