improved motion control

VoidLink/Input/ControllerSupport.m

@@ -89,7 +89,7 @@ static const double MOUSE_SPEED_DIVISOR = 1.25;
     bool _captureMouse;
     
     bool _controllerGyroSwitchEnabled;
-    bool _gyroEnabledFlag;
+    bool _gyroEnabledFlag; // bool flag for both DS4 gyro & VL motion control
     int _controllerGyroSwitchToggle;
     int _controllerGyroSwitchHold;
     bool _reverseHoldButton;
@@ -1059,10 +1059,10 @@ double rc_expo(double x, double expo) {
            ||self->tempSettings.gyroMode.intValue==GyroModeOff;
 }
 
-- (void)switchGyroOnOffByControllerButton{
+- (void)switchMotionControlOnOffByControllerButton{
     self->_gyroEnabledFlag = self->_gyroEnabledFlag && self->_controllerGyroSwitchEnabled;
     if([self useMotionHandler]){
-        if(self->_gyroEnabledFlag) [self->motionHandler startGyroByControllerButton];
+        if(self->_gyroEnabledFlag) [self->motionHandler startMotionControlByControllerButton];
         else [self->motionHandler stopMotionUpdateWithInterruptNoneGyroInput:false];
     }
     else self->_gyroEnabledFlag = self->_gyroEnabledFlag || !self->_controllerGyroSwitchEnabled;
@@ -1187,14 +1187,14 @@ double rc_expo(double x, double expo) {
                                     self->_controllerGyroSwitchTogglePressed = true;
                                     
                                     self->_gyroEnabledFlag = !self->_gyroEnabledFlag;
-                                    [self switchGyroOnOffByControllerButton];
+                                    [self switchMotionControlOnOffByControllerButton];
                                 }
                                 if (buttonFlagId.intValue == self->_controllerGyroSwitchHold
                                     && !self->_controllerGyroSwitchHoldPressed) {
                                     self->_controllerGyroSwitchHoldPressed = true;
                                     
                                     self->_gyroEnabledFlag = !self->_reverseHoldButton;
-                                    [self switchGyroOnOffByControllerButton];
+                                    [self switchMotionControlOnOffByControllerButton];
                                 }
                             }
                             else{
@@ -1207,7 +1207,7 @@ double rc_expo(double x, double expo) {
                                     self->_controllerGyroSwitchHoldPressed = false;
                                     
                                     self->_gyroEnabledFlag = self->_reverseHoldButton;
-                                    [self switchGyroOnOffByControllerButton];
+                                    [self switchMotionControlOnOffByControllerButton];
                                 }
                             }
                         }

VoidLink/Input/MotionHandler.swift

@@ -39,7 +39,7 @@ import CoreMotion
     }
 
     public func gyroMixInputStarted() -> Bool {
-        return gyroIsWorking
+        return motionIsWorking
     }
     
     private static let sharedInstance = MotionHandler()
@@ -61,16 +61,19 @@ import CoreMotion
         return sharedInstance
     }
 
-    @objc public var gyroControlStarted: Bool = false
+    @objc public var motionControlStarted: Bool = false
     @objc var useBuiltinGyro: Bool = true
     @objc var swapYawAndRoll: Bool = false
-    private var gyroIsWorking: Bool = false
+    private var motionIsWorking: Bool = false
     private var accelControlStarted: Bool = false
     private let motionManager = CMMotionManager()
     private weak var activeGCController:GCController?
     private var synthesizePhysicalStick:Bool = false
     private var mapGyroTo:MapGyroTo = .mapGyroToControllerStick
     private var rollToLeftStick:Bool = false
+    private var gravityYAngleRef:Double?
+    private var previousGravityYAngle:Double?
+    private var gravityYUnwrappedAngle:Double = 0
     private var yawPitchToRightStick:Bool = false
     public var sensitvityYaw:CGFloat = 1.0
     public var sensitvityPitch:CGFloat = 1.0
@@ -81,7 +84,7 @@ import CoreMotion
     @objc public var previousWidgetYawFactor:CGFloat = 1.0
     @objc public var previousWidgetPitchFactor:CGFloat = 1.0
     @objc public var previousWidgetRollFactor:CGFloat = 1.0
-    public var gyroStarter:Any?
+    public var motionStarter:Any?
     private var windowScene: Any?
     
     @objc public var onScreenControls: OnScreenControls?
@@ -91,7 +94,7 @@ import CoreMotion
     private var yaw:Double = 0
     private var pitch:Double = 0
     private var roll:Double = 0
-    private var rollIntegral:Double = 0
+    private var leftStickMotion:Double = 0
 
     private var isCalibrating: Bool = false
     private var sumX: Double = 0
@@ -141,7 +144,7 @@ import CoreMotion
         motionManager.gyroUpdateInterval = updateInterval
     }
     
-    public func startGyroByOnScreenButton(_ sender: OnScreenWidgetView, yawFactor:CGFloat, pitchFactor:CGFloat, rollFactor:CGFloat){
+    public func startMotionControlByOnScreenButton(_ sender: OnScreenWidgetView, yawFactor:CGFloat, pitchFactor:CGFloat, rollFactor:CGFloat){
         self.previousWidgetYawFactor = self.widgetYawFactor
         self.previousWidgetPitchFactor = self.widgetPitchFactor
         self.previousWidgetRollFactor = self.widgetRollFactor
@@ -149,31 +152,31 @@ import CoreMotion
         self.widgetPitchFactor = pitchFactor
         self.widgetRollFactor = rollFactor
         print("self.widgetRollFactor \(self.widgetRollFactor)")
-        if self.gyroStarter == nil {
+        if self.motionStarter == nil {
-            self.gyroStarter = sender
+            self.motionStarter = sender
-            if sender.motionControlButtonString != "GYROPAUSE" {self.startGyroUpdate()}
+            if sender.motionControlButtonString != "GYROPAUSE" {self.startMotionUpdate()}
         }
         else if sender.motionControlButtonString == "GYROPAUSE" {
-            self.startGyroUpdate()
+            self.startMotionUpdate()
         }
     }
     
-    @objc public func startGyroByControllerButton(){
+    @objc public func startMotionControlByControllerButton(){
         self.previousWidgetYawFactor = self.widgetYawFactor
         self.previousWidgetPitchFactor = self.widgetPitchFactor
         self.previousWidgetRollFactor = self.widgetRollFactor
         self.widgetYawFactor = 1
         self.widgetPitchFactor = 1
         self.widgetRollFactor = 1
-        self.startGyroUpdate()
+        self.startMotionUpdate()
     }
     
-    @objc public func startGyroUpdate() {
+    @objc public func startMotionUpdate() {
-        if !gyroControlStarted {
+        if !motionControlStarted {
-            gyroControlStarted = true
+            motionControlStarted = true
-            
+            resetGravityYOffsetTracking()
-            print("startGyroUpdate useBuiltinGyro \(useBuiltinGyro) \(CACurrentMediaTime())")
             
+            // print("startGyroUpdate useBuiltinGyro \(useBuiltinGyro) \(CACurrentMediaTime())")
             
             if useBuiltinGyro {
                 if motionManager.isGyroAvailable {
@@ -227,8 +230,9 @@ import CoreMotion
     private var interruptNoneGyroInput:Bool = false
 
     @objc public func stopMotionUpdate(interruptNoneGyroInput:Bool=false) {
-        gyroControlStarted = false
+        motionControlStarted = false
-        gyroIsWorking = false
+        motionIsWorking = false
+        resetGravityYOffsetTracking()
         if motionManager.isDeviceMotionActive{
             motionManager.stopDeviceMotionUpdates()
         }
@@ -256,20 +260,25 @@ import CoreMotion
         var x:Double = 0
         var y:Double = 0
         var z:Double = 0
-        var attitudeRoll:Double = 0
+        // var attitudeRoll:Double = 0
+        var gravityYOffset:Double = 0
 
         if let deviceMotion = deviceMotion {
             x = deviceMotion.rotationRate.x
             y = deviceMotion.rotationRate.y
             z = deviceMotion.rotationRate.z
-            attitudeRoll = deviceMotion.attitude.yaw
+            // attitudeRoll = deviceMotion.attitude.yaw
+            gravityYOffset = updateGravityYOffset(
+                gravityX: deviceMotion.gravity.x,
+                gravityY: deviceMotion.gravity.y,
+            )
         }
         
         if let gcMotion = gcMotion {
             x = gcMotion.rotationRate.x
             y = gcMotion.rotationRate.y
             z = gcMotion.rotationRate.z
-            attitudeRoll = gcMotion.attitude.z
+            // attitudeRoll = gcMotion.attitude.z
         }
         
             
@@ -350,13 +359,13 @@ import CoreMotion
             rollSource = (swapYawAndRoll ? -z : y) - rollBias
         }
         
-        if !gyroControlStarted {
+        if !motionControlStarted {
             self.clearGyroInput(interruptNonGyroInput: interruptNoneGyroInput)
             // print("Gyro: stopped")
             return
         }
         
-        gyroIsWorking = true
+        motionIsWorking = true
         
         if mapGyroTo == MapGyroTo.mapGyroToMouse {
             yaw = yawSource*sensitvityYaw*widgetYawFactor*30
@@ -383,9 +392,9 @@ import CoreMotion
             if rollToLeftStick {
                 roll = gyroInputToStickInput(input:rollSource*sensitvityRoll*widgetRollFactor*0.2)
                                 
-                rollIntegral = useBuiltinGyro ? -stickMaxOffset*(attitudeRoll/Double.pi)*3*sensitvityRoll*widgetRollFactor :                 rollIntegral + roll
+                leftStickMotion = useBuiltinGyro ? -stickMaxOffset*(gravityYOffset/Double.pi)*3*sensitvityRoll*widgetRollFactor :                 leftStickMotion + roll
 
-                let mixedLeftStickOffsetX = self.clampStickInput(input: rollIntegral+leftStickTouchInputX+leftStickPhysicalInputX)
+                let mixedLeftStickOffsetX = self.clampStickInput(input: leftStickMotion+leftStickTouchInputX+leftStickPhysicalInputX)
                 let mixedLeftStickOffsetY = self.clampStickInput(input: leftStickTouchInputY+leftStickPhysicalInputY)
 
                 let offsetVector = ControllerUtil.compensated(offsetVector: CGVector(dx: mixedLeftStickOffsetX, dy: mixedLeftStickOffsetY), minOffset: gyroToStickMinOffset)
@@ -402,7 +411,7 @@ import CoreMotion
             onScreenControls?.sendRightStickTouchPadEvent(rightStickPhysicalInputX+rightStickTouchInputX-yawBias, rightStickPhysicalInputY+rightStickTouchInputY-pitchBias)
         }
         if rollToLeftStick{
-            rollIntegral = 0
+            leftStickMotion = 0
             onScreenControls?.sendLeftStickTouchPadEvent(leftStickPhysicalInputX+leftStickTouchInputX-rollBias,leftStickPhysicalInputY+leftStickTouchInputY)
         }
         if(interruptNonGyroInput){
@@ -411,6 +420,34 @@ import CoreMotion
         }
     }
     
+    private func resetGravityYOffsetTracking() {
+        gravityYAngleRef = nil
+        previousGravityYAngle = nil
+        gravityYUnwrappedAngle = 0
+    }
+    
+    private func updateGravityYOffset(gravityX: Double, gravityY: Double) -> Double {
+        let gravityYAngle = atan2(gravityX, -gravityY)
+        
+        guard let previousGravityYAngle = previousGravityYAngle else {
+            gravityYAngleRef = gravityYAngle
+            previousGravityYAngle = gravityYAngle
+            gravityYUnwrappedAngle = gravityYAngle
+            return 0
+        }
+        
+        var delta = gravityYAngle - previousGravityYAngle
+        if delta > Double.pi {
+            delta -= 2 * Double.pi
+        } else if delta < -Double.pi {
+            delta += 2 * Double.pi
+        }
+        
+        gravityYUnwrappedAngle += delta
+        self.previousGravityYAngle = gravityYAngle
+        return -(gravityYUnwrappedAngle - (gravityYAngleRef ?? gravityYUnwrappedAngle))
+    }
+    
     private func clampStickInput(input: CGFloat) -> CGFloat{
         return fmax(fmin(input, stickMaxOffset),-stickMaxOffset)
     }

VoidLink/Input/OnScreenWidgetView.swift

@@ -2807,7 +2807,7 @@ import ObjectiveC.runtime
         GenericUtils.handleGyroButtonTip(in: self.parentViewController)
         switch self.motionControlButtonString {
         case "GYRO":
-            self.motionHandler?.startGyroByOnScreenButton(self, yawFactor: yawFactor, pitchFactor: pitchFactor, rollFactor: rollFactor)
+            self.motionHandler?.startMotionControlByOnScreenButton(self, yawFactor: yawFactor, pitchFactor: pitchFactor, rollFactor: rollFactor)
             if !OnScreenWidgetView.gamepadArrivalReported {OnScreenWidgetView.gamepadArrivalReported = oscProfile.mapGyroTo == .mapGyroToControllerStick}
         case "GYROPAUSE":
             self.motionHandler?.stopMotionUpdate(interruptNoneGyroInput:false)
@@ -2825,7 +2825,7 @@ import ObjectiveC.runtime
     private func handleMotionControlButtonUp(){
         switch self.motionControlButtonString {
         case "GYRO":
-            if let gyroStarter = motionHandler?.gyroStarter as? OnScreenWidgetView, self === gyroStarter {
+            if let gyroStarter = motionHandler?.motionStarter as? OnScreenWidgetView, self === gyroStarter {
                 self.forEachWidget{ widget in
                     if widget.motionControlButtonString != "GYRO" || widget === self {return}
                     if(widget.buttonMode == .tapToToggle && widget.logicallyDown) {
@@ -2834,18 +2834,18 @@ import ObjectiveC.runtime
                     }
                 }
                 self.motionHandler?.stopMotionUpdate(interruptNoneGyroInput: false)
-                self.motionHandler?.gyroStarter = nil
+                self.motionHandler?.motionStarter = nil
             }
             else {
-                if self.motionHandler?.gyroStarter != nil {
+                if self.motionHandler?.motionStarter != nil {
-                    if let gyroStarter = motionHandler?.gyroStarter as? OnScreenWidgetView {
+                    if let gyroStarter = motionHandler?.motionStarter as? OnScreenWidgetView {
-                        self.motionHandler?.startGyroByOnScreenButton(self, yawFactor: gyroStarter.yawFactor, pitchFactor: gyroStarter.pitchFactor, rollFactor: gyroStarter.rollFactor)
+                        self.motionHandler?.startMotionControlByOnScreenButton(self, yawFactor: gyroStarter.yawFactor, pitchFactor: gyroStarter.pitchFactor, rollFactor: gyroStarter.rollFactor)
                     }
                 }
             }
         case "GYROPAUSE":
-            if self.motionHandler?.gyroStarter != nil {
+            if self.motionHandler?.motionStarter != nil {
-                self.motionHandler?.startGyroByOnScreenButton(self, yawFactor: motionHandler?.widgetYawFactor ?? 0, pitchFactor: motionHandler?.widgetPitchFactor ?? 0, rollFactor: motionHandler?.widgetRollFactor ?? 0)
+                self.motionHandler?.startMotionControlByOnScreenButton(self, yawFactor: motionHandler?.widgetYawFactor ?? 0, pitchFactor: motionHandler?.widgetPitchFactor ?? 0, rollFactor: motionHandler?.widgetRollFactor ?? 0)
             }
         case "ACCEL":
             break
@@ -4119,7 +4119,7 @@ import ObjectiveC.runtime
             autoDockRestoreOriginalAlpha()
             if self.motionControlButtonString == "GYRO" {
                 if OnScreenWidgetView.gamepadArrivalReported {self.motionHandler?.stopMotionUpdate(interruptNoneGyroInput: true)}
-                self.motionHandler?.gyroStarter = nil
+                self.motionHandler?.motionStarter = nil
             }
             if self.motionControlButtonString == "ACCEL" {}
             if self.motionControlButtonString == "MOTION" {}

VoidLink/ViewControllers/StreamFrameViewController.m

@@ -2015,17 +2015,18 @@ static NSString* VLTerminationHintForErrorCode(int errorCode) {
     [self reConfigStreamViewRealtime];
 }
 
+/*
 - (NSMutableDictionary *)startGyroUpdate:(OnScreenWidgetView *)sender yawFactor:(CGFloat)yawFactor pitchFactor:(CGFloat)pitchFactor rollFactor:(CGFloat)rollFactor{
     NSMutableDictionary* gyroControlPreviousStatus = [NSMutableDictionary dictionary];
 
-    if(!_motionHandler.gyroControlStarted) [gyroControlPreviousStatus setObject:sender forKey:@"gyroControlStarter"];
+    if(!_motionHandler.motionControlStarted) [gyroControlPreviousStatus setObject:sender forKey:@"gyroControlStarter"];
     [gyroControlPreviousStatus setObject:@(_motionHandler.widgetYawFactor) forKey:@"previousYawFactor"];
     [gyroControlPreviousStatus setObject:@(_motionHandler.widgetPitchFactor) forKey:@"previousPitchFactor"];
     [gyroControlPreviousStatus setObject:@(_motionHandler.widgetRollFactor) forKey:@"previousRollFactor"];
     _motionHandler.widgetYawFactor = yawFactor;
     _motionHandler.widgetPitchFactor = pitchFactor;
     _motionHandler.widgetRollFactor = rollFactor;
-    [_motionHandler startGyroUpdate];
+    [_motionHandler startMotionUpdate];
 
     return gyroControlPreviousStatus;
 }
@@ -2033,8 +2034,8 @@ static NSString* VLTerminationHintForErrorCode(int errorCode) {
 
 - (NSMutableDictionary*)start:(CGFloat)yawFactor pitchFactor:(CGFloat)pitchFactor rollFactor:(CGFloat)rollFactor{
     NSMutableDictionary* gyroControlPreviousStatus = [NSMutableDictionary dictionary];
-    if(!_motionHandler.gyroControlStarted){
+    if(!_motionHandler.motionControlStarted){
-        [gyroControlPreviousStatus setObject:@(_motionHandler.gyroControlStarted) forKey:@"gyroStarted"];
+        [gyroControlPreviousStatus setObject:@(_motionHandler.motionControlStarted) forKey:@"gyroStarted"];
     }
     [gyroControlPreviousStatus setObject:@(_motionHandler.widgetYawFactor) forKey:@"previousYawFactor"];
     [gyroControlPreviousStatus setObject:@(_motionHandler.widgetPitchFactor) forKey:@"previousPitchFactor"];
@@ -2043,10 +2044,11 @@ static NSString* VLTerminationHintForErrorCode(int errorCode) {
     _motionHandler.widgetYawFactor = yawFactor;
     _motionHandler.widgetPitchFactor = pitchFactor;
     _motionHandler.widgetRollFactor = rollFactor;
-    [_motionHandler startGyroUpdate];
+    [_motionHandler startMotionUpdate];
     
     return gyroControlPreviousStatus;
 }
+*/
 
 - (void)startAccelUpdate{
     [_motionHandler startAccelUpdate];