diff --git a/yall/java/limelight/networktables/LimelightSettings.java b/yall/java/limelight/networktables/LimelightSettings.java index bc25f56..f67b8cc 100644 --- a/yall/java/limelight/networktables/LimelightSettings.java +++ b/yall/java/limelight/networktables/LimelightSettings.java @@ -63,6 +63,10 @@ public class LimelightSettings * Imu Mode for the Limelight 4. */ private NetworkTableEntry imuMode; + /** + * Imu Assist Alpha value for the Limelight 4. + */ + private NetworkTableEntry imuAssistAlpha; /** * Sets 3d offset point for easy 3d targeting Sets the 3D point-of-interest offset for the current fiducial pipeline. *

@@ -109,6 +113,7 @@ public LimelightSettings(Limelight camera) streamMode = limelightTable.getEntry("stream"); cropWindow = limelightTable.getDoubleArrayTopic("crop").getEntry(new double[0]); imuMode = limelightTable.getEntry("imumode_set"); + imuAssistAlpha = limelightTable.getEntry("imuassistalpha_set"); robotOrientationSet = limelightTable.getDoubleArrayTopic("robot_orientation_set").getEntry(new double[0]); downscale = limelightTable.getEntry("fiducial_downscale_set"); fiducial3DOffset = limelightTable.getDoubleArrayTopic("fiducial_offset_set").getEntry(new double[0]); @@ -197,6 +202,20 @@ public LimelightSettings withImuMode(ImuMode mode) return this; } + /** + * Set the IMU Assist filter alpha/strength value. Higher values will cause the internal imu to converge on + * assist source more rapidly. + *

This method changes the Limelight - normally immediately. + * + * @param alpha Assist alpha value (0.001 by default). + * @return {@link LimelightSettings} for chaining. + */ + public LimelightSettings withImuAssistAlpha(double alpha) + { + imuAssistAlpha.setNumber(alpha); + return this; + } + /** * Set the current robot {@link Orientation3d} (normally given by the robot gyro) for LL to use in its MegaTag2 * determination. @@ -370,7 +389,15 @@ public enum ImuMode /** * Use internal IMU for MT2 localization. Ignores external IMU updates from {@link LimelightSettings#withRobotOrientation(Orientation3d)}. */ - InternalImu + InternalImu, + /** + * Use internal IMU for MT2 localization. The internal IMU will utilize filtered MT1 yaw estimates for continuous heading correction. + */ + InternalImuMT1Assist, + /** + * Use internal IMU for MT2 localization. The internal IMU will utilize the external IMU for continuous heading correction. + */ + InternalImuExternalAssist } } diff --git a/yall/python/yall/limelight.py b/yall/python/yall/limelight.py index 8b73998..4ab9675 100644 --- a/yall/python/yall/limelight.py +++ b/yall/python/yall/limelight.py @@ -709,6 +709,8 @@ class ImuMode(Enum): ExternalImu = 0 # Use external IMU yaw submitted via {@link withRobotOrientation} for MT2 localization. The internal IMU is ignored entirely. SyncInternalImu = 1 # Use external IMU yaw submitted via {@link withRobotOrientation} for MT2 localization. The internal IMU is synced with the external IMU. InternalImu = 2 # Use internal IMU for MT2 localization. Ignores external IMU updates from {@link withRobotOrientation}. + InternalImuMT1Assist = 3 # Use internal IMU for MT2 localization. The internal IMU will utilize filtered MT1 yaw estimates for continuous heading correction. + InternalImuExternalAssist = 4 # Use internal IMU for MT2 localization. The internal IMU will utilize the external IMU for continuous heading correction. class DownscalingOverride(Enum): Pipeline = 0 # Pipeline downscaling, equivalent to 0 @@ -747,6 +749,9 @@ def __init__(self, camera: "Limelight"): self.imuMode: ntcore.NetworkTableEntry = self.limelightTable.getEntry( "imumode_set" ) + self.imuAssistAlpha: ntcore.NetworkTableEntry = self.limelightTable.getEntry( + "imuassistalpha_set" + ) self.robotOrientationSet: ntcore.DoubleArrayEntry = ( self.limelightTable.getDoubleArrayTopic("robot_orientation_set").getEntry( [] @@ -803,6 +808,15 @@ def withImuMode(self, mode: ImuMode) -> "LimelightSettings": self.imuMode.setDouble(mode.value) return self + def withImuAssistAlpha(self, alpha: float) -> "LimelightSettings": + """ + Set the IMU Assist filter alpha/strength value. + Higher values will cause the internal imu to converge on assist source more rapidly. + Default value is 0.001. + """ + self.imuAssistAlpha.setDouble(alpha) + return self + def withRobotOrientation(self, orientation: Orientation3d) -> "LimelightSettings": """Set the robot orientation used for localization.""" self.robotOrientationSet.set(LimelightUtils.orientation3dToArray(orientation))