diff --git a/VoidLink/Input/ControllerSupport.m b/VoidLink/Input/ControllerSupport.m index 40c57294..bc90d83c 100644 --- a/VoidLink/Input/ControllerSupport.m +++ b/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]; } } } diff --git a/VoidLink/Input/MotionHandler.swift b/VoidLink/Input/MotionHandler.swift index 826b77f7..f99820ea 100644 --- a/VoidLink/Input/MotionHandler.swift +++ b/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 { - self.gyroStarter = sender - if sender.motionControlButtonString != "GYROPAUSE" {self.startGyroUpdate()} + if self.motionStarter == nil { + self.motionStarter = sender + 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() { - if !gyroControlStarted { - gyroControlStarted = true + @objc public func startMotionUpdate() { + if !motionControlStarted { + 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 - gyroIsWorking = false + motionControlStarted = 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 - - let mixedLeftStickOffsetX = self.clampStickInput(input: rollIntegral+leftStickTouchInputX+leftStickPhysicalInputX) + leftStickMotion = useBuiltinGyro ? -stickMaxOffset*(gravityYOffset/Double.pi)*3*sensitvityRoll*widgetRollFactor : leftStickMotion + roll + + 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) } diff --git a/VoidLink/Input/OnScreenWidgetView.swift b/VoidLink/Input/OnScreenWidgetView.swift index 08fb333c..a201960d 100644 --- a/VoidLink/Input/OnScreenWidgetView.swift +++ b/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 let gyroStarter = motionHandler?.gyroStarter as? OnScreenWidgetView { - self.motionHandler?.startGyroByOnScreenButton(self, yawFactor: gyroStarter.yawFactor, pitchFactor: gyroStarter.pitchFactor, rollFactor: gyroStarter.rollFactor) + if self.motionHandler?.motionStarter != nil { + if let gyroStarter = motionHandler?.motionStarter as? OnScreenWidgetView { + self.motionHandler?.startMotionControlByOnScreenButton(self, yawFactor: gyroStarter.yawFactor, pitchFactor: gyroStarter.pitchFactor, rollFactor: gyroStarter.rollFactor) } } } case "GYROPAUSE": - if self.motionHandler?.gyroStarter != nil { - self.motionHandler?.startGyroByOnScreenButton(self, yawFactor: motionHandler?.widgetYawFactor ?? 0, pitchFactor: motionHandler?.widgetPitchFactor ?? 0, rollFactor: motionHandler?.widgetRollFactor ?? 0) + if self.motionHandler?.motionStarter != nil { + 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" {} diff --git a/VoidLink/ViewControllers/StreamFrameViewController.m b/VoidLink/ViewControllers/StreamFrameViewController.m index 88a19bcf..b7a5685b 100644 --- a/VoidLink/ViewControllers/StreamFrameViewController.m +++ b/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){ - [gyroControlPreviousStatus setObject:@(_motionHandler.gyroControlStarted) forKey:@"gyroStarted"]; + if(!_motionHandler.motionControlStarted){ + [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];