Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix potential divide-by-zero in RKDP (#5242)

Browse Source 
If f(x, u) has no dynamics, the truncation error can be zero.
This commit is contained in:
Tyler Veness
2023-03-26 17:00:09 -07:00
committed by GitHub
parent 9227b2166e
commit 63512bbbb8
6 changed files with 36 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -122,7 +122,11 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
(b1[6] - b2[6]) * k7))
.norm();
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
if (truncationError == 0.0) {
h = dt.value() - dtElapsed;
} else {
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
}
} while (truncationError > maxError);
dtElapsed += h;