Add extrapolated prediction to drift compensation

gui/public/i18n/en/translation.ftl

@@ -322,6 +322,12 @@ settings-general-tracker_mechanics-drift_compensation-description =
     Compensates IMU yaw drift by applying an inverse rotation.
     Change amount of compensation and up to how many resets are taken into account.
 settings-general-tracker_mechanics-drift_compensation-enabled-label = Drift compensation
+settings-general-tracker_mechanics-drift_compensation-prediction = Drift compensation prediction
+# This cares about multilines
+settings-general-tracker_mechanics-drift_compensation-prediction-description =
+    Predicts yaw drift compensation beyond previously measured range.
+    Enable this if the tracker is continuously spinning on yaw axis.
+settings-general-tracker_mechanics-drift_compensation-prediction-label = Drift compensation prediction
 settings-general-tracker_mechanics-drift_compensation-amount-label = Compensation amount
 settings-general-tracker_mechanics-drift_compensation-max_resets-label = Use up to x last resets
 settings-general-tracker_mechanics-save_mounting_reset = Save automatic mounting reset calibration

gui/src/components/settings/pages/GeneralSettings.tsx

@@ -52,6 +52,7 @@ interface SettingsForm {
   };
   driftCompensation: {
     enabled: boolean;
+    prediction: boolean;
     amount: number;
     maxResets: number;
   };
@@ -138,6 +139,7 @@ const defaultValues: SettingsForm = {
   filtering: { amount: 0.1, type: FilteringType.NONE },
   driftCompensation: {
     enabled: false,
+    prediction: false,
     amount: 0.1,
     maxResets: 1,
   },
@@ -282,6 +284,7 @@ export function GeneralSettings() {
 
     const driftCompensation = new DriftCompensationSettingsT();
     driftCompensation.enabled = values.driftCompensation.enabled;
+    driftCompensation.prediction = values.driftCompensation.prediction;
     driftCompensation.amount = values.driftCompensation.amount;
     driftCompensation.maxResets = values.driftCompensation.maxResets;
     settings.driftCompensation = driftCompensation;
@@ -698,6 +701,33 @@ export function GeneralSettings() {
                 'settings-general-tracker_mechanics-drift_compensation-enabled-label'
               )}
             />
+            <div className="flex flex-col pt-2 pb-4"></div>
+            <Typography bold>
+              {l10n.getString(
+                'settings-general-tracker_mechanics-drift_compensation-prediction'
+              )}
+            </Typography>
+            <div className="flex flex-col pt-2 pb-4">
+              {l10n
+                .getString(
+                  'settings-general-tracker_mechanics-drift_compensation-prediction-description'
+                )
+                .split('\n')
+                .map((line, i) => (
+                  <Typography color="secondary" key={i}>
+                    {line}
+                  </Typography>
+                ))}
+            </div>
+            <CheckBox
+              variant="toggle"
+              outlined
+              control={control}
+              name="driftCompensation.prediction"
+              label={l10n.getString(
+                'settings-general-tracker_mechanics-drift_compensation-prediction-label'
+              )}
+            />
             <div className="flex gap-5 pt-5 md:flex-row flex-col">
               <NumberSelector
                 control={control}

server/core/src/main/java/dev/slimevr/config/DriftCompensationConfig.kt

@@ -7,6 +7,9 @@ class DriftCompensationConfig {
 	// Is drift compensation enabled
 	var enabled = false
 
+	// Is drift prediction enabled
+	var prediction = false
+
 	// Amount of drift compensation applied
 	var amount = 0.8f
 

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java

@@ -123,6 +123,7 @@ public static int createDriftCompensationSettings(
 			.createDriftCompensationSettings(
 				fbb,
 				driftCompensationConfig.getEnabled(),
+				driftCompensationConfig.getPrediction(),
 				driftCompensationConfig.getAmount(),
 				driftCompensationConfig.getMaxResets()
 			);

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt

@@ -84,6 +84,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 				.vrConfig
 				.driftCompensation
 			driftCompensationConfig.enabled = req.driftCompensation().enabled()
+			driftCompensationConfig.prediction = req.driftCompensation().prediction()
 			driftCompensationConfig.amount = req.driftCompensation().amount()
 			driftCompensationConfig.maxResets = req.driftCompensation().maxResets()
 			driftCompensationConfig.updateTrackersDriftCompensation()

server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt

@@ -37,6 +37,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	private var driftSince: Long = 0
 	private var timeAtLastReset: Long = 0
 	private var compensateDrift = false
+	private var driftPrediction = false
 	private var driftCompensationEnabled = false
 	private var resetMountingFeet = false
 	private var armsResetMode = ArmsResetModes.BACK
@@ -78,6 +79,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	 */
 	fun readDriftCompensationConfig(config: DriftCompensationConfig) {
 		compensateDrift = config.enabled
+		driftPrediction = config.prediction
 		driftAmount = config.amount
 		val maxResets = config.maxResets
 
@@ -186,11 +188,11 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	 */
 	private fun adjustToDrift(rotation: Quaternion): Quaternion {
 		if (driftCompensationEnabled && totalDriftTime > 0) {
-			return rotation
-				.interpR(
-					averagedDriftQuat * rotation,
-					driftAmount * ((System.currentTimeMillis() - driftSince).toFloat() / totalDriftTime),
-				)
+			var driftTimeRatio = ((System.currentTimeMillis() - driftSince).toFloat() / totalDriftTime)
+			if (!driftPrediction) {
+				driftTimeRatio = min(1.0f, driftTimeRatio)
+			}
+			return averagedDriftQuat.pow(driftAmount * driftTimeRatio) * rotation
 		}
 		return rotation
 	}