adafruit-mirror/Bluefruit-Playground
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Quaternion: fix z axis rotation

Browse source
This commit is contained in:
Antonio 2020-03-17 21:50:16 +01:00
parent ec13361e4f
commit 5f9bfb3ee9
7 changed files with 88 additions and 32 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
BluefruitPlayground.xcodeproj/project.pbxproj
View file

@ -1603,7 +1603,7 @@
ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
CLANG_ENABLE_MODULES = YES;
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 18;
CURRENT_PROJECT_VERSION = 19;
DEVELOPMENT_TEAM = 2X94RM7457;
INFOPLIST_FILE = BluefruitPlayground/Info.plist;
IPHONEOS_DEPLOYMENT_TARGET = 12.0;
@ -1628,7 +1628,7 @@
ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
CLANG_ENABLE_MODULES = YES;
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 18;
CURRENT_PROJECT_VERSION = 19;
DEVELOPMENT_TEAM = 2X94RM7457;
INFOPLIST_FILE = BluefruitPlayground/Info.plist;
IPHONEOS_DEPLOYMENT_TARGET = 12.0;
@ -1692,7 +1692,7 @@
ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
CLANG_ENABLE_MODULES = YES;
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 18;
CURRENT_PROJECT_VERSION = 19;
DEVELOPMENT_TEAM = 2X94RM7457;
INFOPLIST_FILE = "BluefruitPlayground-SimulatedBluetooth-Info.plist";
IPHONEOS_DEPLOYMENT_TARGET = 12.0;
@ -1718,7 +1718,7 @@
ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
CLANG_ENABLE_MODULES = YES;
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 18;
CURRENT_PROJECT_VERSION = 19;
DEVELOPMENT_TEAM = 2X94RM7457;
INFOPLIST_FILE = "BluefruitPlayground-SimulatedBluetooth-Info.plist";
IPHONEOS_DEPLOYMENT_TARGET = 12.0;

2
BluefruitPlayground/AdafruitKit/Ble/AdafruitSensors/BlePeripheral+AdafruitQuaternion.swift
View file

@ -94,7 +94,7 @@ extension BlePeripheral {
guard let bytes = adafruitDataToFloatArray(data) else { return nil }
guard bytes.count >= 4 else { return nil }
// return QuaternionValue(qx: bytes[0], qy: bytes[1], qz: bytes[2], qw: bytes[3])
//return QuaternionValue(x: bytes[0], y: bytes[1], z: bytes[2], w: bytes[3])
return QuaternionValue(x: bytes[1], y: bytes[2], z: bytes[3], w: bytes[0])
}
}

30
BluefruitPlayground/AdafruitKit/Ble/Tests/BlePeripheralSimulated+AdafruitHumidity.swift
View file

@ -11,18 +11,18 @@ import CoreBluetooth
extension BlePeripheral {
// MARK: - Custom properties
private struct CustomPropertiesKeys {
static var adafruitHumidityResponseDataTimer: Timer?
}
private var adafruitHumidityResponseDataTimer: Timer? {
get {
return objc_getAssociatedObject(self, &CustomPropertiesKeys.adafruitHumidityResponseDataTimer) as! Timer?
}
set {
objc_setAssociatedObject(self, &CustomPropertiesKeys.adafruitHumidityResponseDataTimer, newValue, .OBJC_ASSOCIATION_RETAIN)
}
}
private struct CustomPropertiesKeys {
static var adafruitHumidityResponseDataTimer: Timer?
}
private var adafruitHumidityResponseDataTimer: Timer? {
get {
return objc_getAssociatedObject(self, &CustomPropertiesKeys.adafruitHumidityResponseDataTimer) as! Timer?
}
set {
objc_setAssociatedObject(self, &CustomPropertiesKeys.adafruitHumidityResponseDataTimer, newValue, .OBJC_ASSOCIATION_RETAIN)
}
}
// MARK: - Actions
func adafruitHumidityEnable(responseHandler: @escaping(Result<(Float, UUID), Error>) -> Void, completion: ((Result<Void, Error>) -> Void)?) {
@ -35,14 +35,14 @@ extension BlePeripheral {
completion?(.success(()))
}
func adafruitHumidityIsEnabled() -> Bool {
return self.adafruitManufacturerData()?.boardModel == .clue_nRF52840
}
func adafruitHumidityDisable() {
}
func adafruitHumidityLastValue() -> Float? {
return Float.random(in: 28.5 ..< 29.0)
}

43
BluefruitPlayground/Model/AdafruitBoard/QuaternionUtils.swift
View file

@ -16,8 +16,8 @@ struct QuaternionUtils {
static func quaternionToEuler(x: Float, y: Float, z: Float, w: Float) -> (x: Float, y: Float, z: Float) {
let pitch = asin(min(1, max(-1, 2 * (w * y - z * x))))
let yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
let roll = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
let yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
let roll = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
return (pitch, yaw, roll)
}
@ -30,4 +30,43 @@ struct QuaternionUtils {
let vector = result.vector
return BlePeripheral.QuaternionValue(x: vector.x, y: vector.y, z: vector.z, w: vector.w)
}
/**
Decompose the rotation on to 2 parts.
1. Twist - rotation around the "direction" vector
2. Swing - rotation around axis that is perpendicular to "direction" vector
The rotation can be composed back by
rotation = swing * twist
has singularity in case of swing_rotation close to 180 degrees rotation.
if the input quaternion is of non-unit length, the outputs are non-unit as well
otherwise, outputs are both unit
Singularity appears if rotation close to 180 deg, and rotation axis is orthogonal to "direction" vector. There is a infinity decompositions in this case. It can be checked if magnitude of unnormalized twist close to zero. Than you can select one of possible , valid twist
*/
static func swing_twist_decomposition( rotation: simd_quatf, direction: simd_float3) -> (twist: simd_quatf, swing: simd_quatf) {
// Based on: https://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis/22401169#22401169
let ra = simd_float3(rotation.axis.x, rotation.axis.y, rotation.axis.z) // rotation axis
let p = simd_project(ra, direction) // return projection v1 on to v2 (parallel component)
let twist = simd_quatf(angle: rotation.angle, axis: p)
let twistNormalized = twist.normalized
let swing = rotation * twistNormalized.conjugate
return (twist, swing)
}
static func twist_decomposition( rotation: simd_quatf, direction: simd_float3) -> simd_quatf {
// Based on: https://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis/22401169#22401169
let ra = simd_float3(rotation.axis.x, rotation.axis.y, rotation.axis.z) // rotation axis
let p = simd_project(ra, direction) // return projection v1 on to v2 (parallel component)
let twist = simd_quatf(angle: rotation.angle, axis: p)
let twistNormalized = twist.normalized
return twistNormalized
}
}

1
BluefruitPlayground/ViewControllers/Accelerometer/AccelerometerViewController.swift
View file

@ -72,6 +72,7 @@ class AccelerometerViewController: ModuleViewController {
guard let acceleration = acceleration else { return }
SCNTransaction.animationDuration = BlePeripheral.kAdafruitSensorDefaultPeriod
SCNTransaction.animationTimingFunction = CAMediaTimingFunction(name: .linear)
// Calculate Euler Angles
let eulerAngles = AccelerometerUtils.accelerationToEuler(acceleration)

3
BluefruitPlayground/ViewControllers/Puppet/PuppetViewController.swift
View file

@ -284,7 +284,8 @@ class PuppetViewController: UIViewController {
// Update sparky rotation (only if the intro animation is not currently playing)
if !isPlayingIntroAnimation {
SCNTransaction.animationDuration = BlePeripheral.kAdafruitSensorDefaultPeriod
SCNTransaction.animationTimingFunction = CAMediaTimingFunction(name: .linear)
// Calculate euler angles and feed them to the low pass filters
let eulerAngles = AccelerometerUtils.accelerationToEuler(acceleration)
filteredAngleX.update(newValue: eulerAngles.x)

33
BluefruitPlayground/ViewControllers/Quaternion/QuaternionViewController.swift
View file

@ -14,7 +14,7 @@ class QuaternionViewController: ModuleViewController {
static let kIdentifier = "QuaternionViewController"
// Config
private static let kDisableZComponent = true // Z component is disabled until gyro calibration is implemented
private static let kIsZAxisDisabled = true // Z axis is disabled until gyro calibration is implemented
// UI
@IBOutlet weak var sceneView: SCNView!
@ -40,7 +40,7 @@ class QuaternionViewController: ModuleViewController {
sceneView.autoenablesDefaultLighting = true
sceneView.isUserInteractionEnabled = true
}
// Localization
let localizationManager = LocalizationManager.shared
self.title = localizationManager.localizedString("quaternion_title")
@ -69,19 +69,33 @@ class QuaternionViewController: ModuleViewController {
board?.quaternionDelegate = nil
}
var angle: Float = 0
var quat = simd_quatf(angle: 0, axis: simd_float3(0, 1, 0))
// MARK: - UI
private func updateValueUI() {
guard let quaternion = quaternion else { return }
guard let boardNode = boardNode else { return }
let scnQuaternion = simd_quatf(vector: simd_float4(quaternion.x, quaternion.y, quaternion.z, quaternion.w))
//DLog("quat: \(scnQuaternion.vector.x), \(scnQuaternion.vector.y), \(scnQuaternion.vector.z), \(scnQuaternion.vector.w)")
let scnQuaternion = simd_quatf(ix: quaternion.x, iy: quaternion.y, iz: QuaternionViewController.kDisableZComponent ? 0 : quaternion.z, r: quaternion.w)
// Update circuit model orientation
SCNTransaction.animationTimingFunction = CAMediaTimingFunction(name: .linear)
SCNTransaction.animationDuration = BlePeripheral.kAdafruitSensorDefaultPeriod
boardNode?.simdOrientation = scnQuaternion
// Update panel
let (x, y, z) = QuaternionUtils.quaternionToEuler(quaternion: quaternion)
let eulerAngles = simd_float3(x, y, z)
if QuaternionViewController.kIsZAxisDisabled {
// Calculate rotation around the z axis.
let twist = QuaternionUtils.twist_decomposition(rotation: scnQuaternion, direction: simd_float3(0, 0, 1))
// Remove z axis rotation
boardNode.simdOrientation = scnQuaternion * twist.inverse
}
else {
boardNode.simdOrientation = scnQuaternion
}
let eulerAngles = boardNode.simdEulerAngles
//DLog("Euler: pitch: \(eulerAngles.xquaternionToEuler) yaw: \(eulerAngles.y) roll: \(eulerAngles.z)")
valuesPanelViewController.accelerationReceived(quaternion: scnQuaternion, eulerAngles: eulerAngles)
}
@ -94,3 +108,4 @@ extension QuaternionViewController: AdafruitQuaternionDelegate {
updateValueUI()
}
}
var debugI = 0