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

EffectivenessTiltrotor: fix logic for matrix update

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-12-16 17:50:48 +01:00 committed by Daniel Agar
parent 6c8f322dfe
commit 478724c9fe
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -100,8 +100,13 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f;
if (fabsf(control_tilt - _last_tilt_control) > 0.01f && PX4_ISFINITE(_last_tilt_control)) {
// initialize _last_tilt_control
if (!PX4_ISFINITE(_last_tilt_control)) {
_last_tilt_control = control_tilt;
} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
_updated = true;
_last_tilt_control = control_tilt;
}
for (int i = 0; i < _tilts.count(); ++i) {
@ -109,8 +114,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
actuator_sp(i + _first_tilt_idx) += control_tilt;
}
}
_last_tilt_control = control_tilt;
}
}
}