Add filtered orientation sensor

build.gradle.kts

@@ -2,7 +2,7 @@ buildscript {
     extra.apply {
         set("groupId", "com.kylecorry.andromeda")
         set("versionName", "7.0.0-beta02")
-        set("solVersion", "9.0.0")
+        set("solVersion", "9.1.1")
         set("lunaVersion", "6a88851e2b")
         set("coreKtxVersion", "1.12.0")
         set("appCompatVersion", "1.6.1")

sense/src/main/java/com/kylecorry/andromeda/sense/orientation/filter/FilteredOrientationSensor.kt

@@ -0,0 +1,52 @@
+package com.kylecorry.andromeda.sense.orientation.filter
+
+import com.kylecorry.andromeda.core.sensors.AbstractSensor
+import com.kylecorry.andromeda.sense.orientation.IOrientationSensor
+import com.kylecorry.sol.math.Quaternion
+
+class FilteredOrientationSensor(
+    private val sensor: IOrientationSensor,
+    private val filter: OrientationSensorFilter
+) : AbstractSensor(), IOrientationSensor {
+
+    private val reading = Quaternion.zero.toFloatArray()
+    private var hasReading = false
+
+    override fun startImpl() {
+        hasReading = false
+        sensor.start(this::onSensorUpdate)
+    }
+
+    override fun stopImpl() {
+        sensor.stop(this::onSensorUpdate)
+    }
+
+    override val hasValidReading: Boolean
+        get() = sensor.hasValidReading
+
+    override val headingAccuracy: Float?
+        get() = null
+
+    override val orientation: Quaternion
+        get() = Quaternion.from(rawOrientation)
+
+    override val rawOrientation: FloatArray
+        get() = reading
+
+    private fun onSensorUpdate(): Boolean {
+        if (!hasReading) {
+            sensor.rawOrientation.copyInto(reading)
+            hasReading = true
+            filter.reset(reading)
+            notifyListeners()
+            return true
+        }
+
+        val newReading = sensor.rawOrientation.copyOf()
+        filter.filter(newReading, reading)
+
+        notifyListeners()
+        return true
+    }
+}
+

sense/src/main/java/com/kylecorry/andromeda/sense/orientation/filter/LowPassOrientationSensorFilter.kt

@@ -0,0 +1,36 @@
+package com.kylecorry.andromeda.sense.orientation.filter
+
+import com.kylecorry.sol.math.Quaternion
+import com.kylecorry.sol.math.QuaternionMath
+
+class LowPassOrientationSensorFilter(
+    var alpha: Float,
+    var useSlerp: Boolean = true
+) : OrientationSensorFilter {
+
+    val value: FloatArray
+        get() = synchronized(lock) {
+            _current.copyOf()
+        }
+
+    private val lock = Any()
+    private val _current = Quaternion.zero.toFloatArray()
+
+    override fun filter(quaternion: FloatArray, out: FloatArray) {
+        synchronized(lock) {
+            if (useSlerp) {
+                QuaternionMath.slerp(_current, quaternion, alpha, _current, true)
+            } else {
+                QuaternionMath.lerp(_current, quaternion, alpha, _current, true)
+            }
+            QuaternionMath.normalize(_current, _current)
+            _current.copyInto(out)
+        }
+    }
+
+    override fun reset(value: FloatArray) {
+        synchronized(lock) {
+            value.copyInto(_current)
+        }
+    }
+}

sense/src/main/java/com/kylecorry/andromeda/sense/orientation/filter/OrientationSensorFilter.kt

@@ -0,0 +1,6 @@
+package com.kylecorry.andromeda.sense.orientation.filter
+
+interface OrientationSensorFilter {
+    fun filter(quaternion: FloatArray, out: FloatArray)
+    fun reset(value: FloatArray)
+}