Adds a Kaitai Struct definition for the WPILOG format. This can be used to generate code to read/write data log files without needing to build out a parser/serializer by hand. Or just an additional resource for readers to help understand the WPILOG format.
Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.
This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.
To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.
Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
A Discord user reported that StackWalker gives blank stacktraces.
MSVC's C++23 support is unstable, so we can't use std::stacktrace yet.
In the meantime, we can just return an empty string and remove the
unmaintained StackWalker library and its hacky upstream_utils script.
I was getting:
external/arm_frc_linux_gnueabi_repo/bin/../arm-nilrt-linux-gnueabi/sysroot/usr/lib/gcc/arm-nilrt-linux-gnueabi/12/../../../../../../../arm-nilrt-linux-gnueabi/bin/ld: bazel-out/k8-opt--roborio/bin/external/com_github_wpilibsuite_allwpilib/wpinet/libwpinet.static.a(fs.o): undefined reference to symbol 'dlsym@@GLIBC_2.4'
external/arm_frc_linux_gnueabi_repo/bin/../arm-nilrt-linux-gnueabi/sysroot/usr/lib/gcc/arm-nilrt-linux-gnueabi/12/../../../../../../../arm-nilrt-linux-gnueabi/bin/ld: external/arm_frc_linux_gnueabi_repo/bin/../arm-nilrt-linux-gnueabi/sysroot/lib/libdl.so.2: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status
This fixes that error for me.
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
The magic static adds yet another thing that needs to be reset if you
want to run a unit test with a completely reset wpilib state. Use the
existing Sendable static map to store the data instead.
This leaks memory if ResetSmartDashboardInstance is called.
Many LED strips use different color order (GRB in particular is common).
This makes the change at the HAL level. This solves 2 problems; first, no code needs to change in the high level drivers, which was challenging for C++, and second, simulation will behave properly as no conversion is needed. The HAL will accept an array of data objects in the same order no matter what the selected output order is, and will convert before sending it to the FPGA for output.
To accomplish this, NEON bulk load/interleave instructions are utilized. The low level implementation (load, store, and alignment functions) come from the Simd Library. The high level implementations are inspired by the image conversion functions in the simd library, but have diverged significantly.
Much of the implementation uses templates and inlined functions rather than runtime parameters; This is a trade off between the size of the generated code and the amount of function calls done at runtime. Currently, the entire conversion operation is inlined.
Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.
```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 20_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.
I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.
Fixes#7743.
