Kalibrierung und Einbaulage gehören in den Sensor, nicht in die Apps

Die Kalibrier-Nullpunkte lagen schon immer im ESP und wurden dort auch abgezogen – nur verriet die Firmware sie niemandem. Die Einbaulage lag dagegen in jeder App einzeln, und damit stand sie bei drei Endgeräten dreimal da, im Zweifel dreimal verschieden. Sie beschreibt aber den Einbau und nicht das Telefon. Zwei neue Charakteristiken: ...3426 Nullpunkte, zwei Floats, nur lesen ...3428 Einbaulage, acht Byte, lesen und schreiben 0 Version, 1 = gültig gesetzt, 0 = nie geschrieben 1 Längsachse: 0 = Pitch des Sensors, 1 = Roll 2 längs umgekehrt (0/1) 3 quer umgekehrt (0/1) 4..7 Verdrehung um die Hochachse, float32, Grad Das Versionsbyte löst die Umstellung: Steht dort 0, hat nie jemand geschrieben – dann gilt weiter, was die App örtlich gespeichert hat, und beim nächsten Bestimmen wandert sie hinauf. Mit älterer Firmware ohne die Charakteristik passiert schlicht nichts. Angewandt wird die Lage weiterhin in den Apps. Der Sensor verwahrt sie nur: Meldete er die Winkel bereits umgerechnet, rechnete jede ältere App die Korrektur ein zweites Mal ein. Beim Verbinden liest jede App die Lage und übernimmt sie; nach dem Einbaulage-Assistenten schreibt sie sie hinauf. Die Uhr braucht das iPhone dafür nicht mehr – sie holt sie sich selbst. Die Android-App bekommt dabei auch die Verdrehung, die ihrer Fassung bisher ganz fehlte: Modell, Erkennung und Persistenz nachgezogen. Geprüft: Firmware übersetzt, im erzeugten Code stehen beide Charakteristiken mit den richtigen Rechten und der Schreib-Handler; iPhone- und Watch-Ziel bauen; sieben neue Prüfungen in run-tests.sh nageln das Byte-Format fest, samt Rundlauf und der Bedeutung von Version 0. Nicht geprüft ist die Android-Fassung: Auf dieser Maschine liegt nur Java 8, Gradle verlangt mindestens 11. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
2026-08-31 20:50:23 +02:00
parent 0761706d98
commit f0df433421
13 changed files with 470 additions and 20 deletions
@@ -1,6 +1,9 @@
 package de.fritob.campermonitor.protocol

 import kotlin.math.abs
+import kotlin.math.atan2
+import kotlin.math.cos
+import kotlin.math.sin

 /**
 * Wie der Neigungsmesser im Fahrzeug sitzt.
@@ -21,18 +24,42 @@ data class SensorOrientation(
    val longitudinalSource: Source = Source.PITCH,
    val invertLongitudinal: Boolean = false,
    val invertLateral: Boolean = false,
+    /**
+     * Verdrehung des Sensors um die Hochachse, in Grad – der Rest, den der
+     * Achsentausch nicht abdeckt.
+     *
+     * Sitzt der Sensor schräg im Fahrzeug, verteilt sich eine reine
+     * Querneigung auf beide Sensorachsen: Das Fahrzeug kippt zur Seite, die
+     * Anzeige meldet zusätzlich Längsneigung. Achsentausch und Vorzeichen
+     * helfen dagegen nicht, die springen in 90°-Schritten.
+     */
+    val twist: Double = 0.0,
 ) {
    /** Welche Achse des Sensors die Längsneigung des Fahrzeugs liefert. */
    enum class Source { PITCH, ROLL }

    val isIdentity: Boolean get() = this == IDENTITY

-    /** Rechnet Sensorwerte in Fahrzeugwerte um. */
+    /**
+     * Rechnet Sensorwerte in Fahrzeugwerte um.
+     *
+     * Zwei Schritte, in dieser Reihenfolge: erst die grobe Zuordnung der
+     * Achsen samt Vorzeichen, dann die Verdrehung zurückdrehen. Für kleine
+     * Winkel verhält sich das Wertepaar wie ein Vektor in der Ebene – genau
+     * deshalb lässt sich die Verdrehung überhaupt herausrechnen.
+     */
    fun apply(pitch: Double?, roll: Double?): Pair<Double?, Double?> {
        val longitudinal = if (longitudinalSource == Source.PITCH) pitch else roll
        val lateral = if (longitudinalSource == Source.PITCH) roll else pitch
-        return longitudinal?.let { if (invertLongitudinal) -it else it } to
-            lateral?.let { if (invertLateral) -it else it }
+        val mappedLongitudinal = longitudinal?.let { if (invertLongitudinal) -it else it }
+        val mappedLateral = lateral?.let { if (invertLateral) -it else it }
+
+        if (twist == 0.0 || mappedLongitudinal == null || mappedLateral == null) {
+            return mappedLongitudinal to mappedLateral
+        }
+        val angle = Math.toRadians(twist)
+        return (mappedLongitudinal * cos(angle) + mappedLateral * sin(angle)) to
+            (-mappedLongitudinal * sin(angle) + mappedLateral * cos(angle))
    }

    val summary: String
@@ -42,6 +69,7 @@ data class SensorOrientation(
            if (longitudinalSource == Source.ROLL) parts.add("Achsen getauscht")
            if (invertLongitudinal) parts.add("längs umgekehrt")
            if (invertLateral) parts.add("quer umgekehrt")
+            if (twist != 0.0) parts.add("um %.0f° verdreht".format(twist))
            return parts.joinToString(", ")
        }

@@ -68,6 +96,9 @@ object OrientationDetection {
    /** Soviel deutlicher muss die gewinnende Deutung sein als die andere. */
    const val AMBIGUITY_MARGIN = 1.3

+    /** Ab hier gilt eine Verdrehung als echt und nicht als Wackeln der Hand. */
+    const val MINIMUM_TWIST = 2.0
+
    data class Reading(val pitch: Double, val roll: Double) {
        operator fun minus(other: Reading) = Reading(pitch - other.pitch, roll - other.roll)
    }
@@ -131,16 +162,31 @@ object OrientationDetection {
        if (abs(longitudinal) < MINIMUM_TILT || abs(lateral) < MINIMUM_TILT) {
            return Result.Error(Failure.TOO_LITTLE_MOVEMENT)
        }
+        val coarse = SensorOrientation(
+            longitudinalSource = source,
+            // Front nach unten heisst: das Heck steht höher, die
+            // Längsneigung des Fahrzeugs ist also positiv.
+            invertLongitudinal = longitudinal < 0,
+            // Linke Seite nach unten heisst: rechts steht höher, die
+            // Querneigung ist positiv.
+            invertLateral = lateral < 0,
+        )
+
+        // Was nach dem Achsentausch noch übrig ist, ist die Verdrehung um die
+        // Hochachse. Beim Kippen der Front nach unten dürfte sich nur die
+        // Längsneigung ändern; wandert die Querneigung mit, sitzt der Sensor
+        // schräg – und zwar um genau diesen Winkel.
+        val (correctedLong, correctedLat) = coarse.apply(nose.pitch, nose.roll)
+        val residual = if (correctedLong != null && correctedLat != null) {
+            Math.toDegrees(atan2(correctedLat, correctedLong))
+        } else {
+            0.0
+        }
+        // Unter zwei Grad ist es Messrauschen. Zwei Kippbewegungen von Hand
+        // sind nicht genauer, und eine erfundene Verdrehung wäre schlimmer
+        // als keine.
        return Result.Success(
-            SensorOrientation(
-                longitudinalSource = source,
-                // Front nach unten heisst: das Heck steht höher, die
-                // Längsneigung des Fahrzeugs ist also positiv.
-                invertLongitudinal = longitudinal < 0,
-                // Linke Seite nach unten heisst: rechts steht höher, die
-                // Querneigung ist positiv.
-                invertLateral = lateral < 0,
-            )
+            coarse.copy(twist = if (abs(residual) >= MINIMUM_TWIST) residual else 0.0)
        )
    }
 }
@@ -24,6 +24,9 @@ object VanAlignProtocol {
    /** Ein Byte: 0 setzt zurück, alles andere kalibriert auf die aktuelle Lage. */
    const val CALIBRATE_UUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233427"

+    /** Acht Byte: die Einbaulage, lesbar und schreibbar. Siehe [orientation]. */
+    const val ORIENTATION_UUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233428"
+
    val calibrateCommand = byteArrayOf(0x01)
    val resetCommand = byteArrayOf(0x00)

@@ -50,6 +53,55 @@ object VanAlignProtocol {
        return value.toDouble()
    }

+    /**
+     * Die Einbaulage, wie sie im Gerät liegt.
+     *
+     * Acht Byte:
+     *
+     *     0      Version, 1 = gültig gesetzt, 0 = nie geschrieben
+     *     1      Längsachse: 0 = Pitch des Sensors, 1 = Roll des Sensors
+     *     2      längs umgekehrt (0/1)
+     *     3      quer umgekehrt (0/1)
+     *     4..7   Verdrehung um die Hochachse, float32, Grad
+     *
+     * Sie gehört ins Gerät, weil sie den Einbau beschreibt und nicht das
+     * Telefon: iPhone, Uhr und Android sollen dieselbe sehen, ohne sie je
+     * einzeln zu bestimmen. Gerechnet wird trotzdem in den Apps – das Gerät
+     * verwahrt sie nur, sonst rechnete ein älterer Client die Korrektur ein
+     * zweites Mal ein.
+     *
+     * Version 0 heisst „hier stand noch nie etwas" und ergibt null; dann gilt,
+     * was die App örtlich gespeichert hat, und sie schreibt es hinauf.
+     */
+    fun orientation(data: ByteArray): SensorOrientation? {
+        if (data.size < 8 || data.u(0) != 1) return null
+        val twist = angle(data, 4) ?: return null
+        return SensorOrientation(
+            longitudinalSource = if (data.u(1) == 1) {
+                SensorOrientation.Source.ROLL
+            } else {
+                SensorOrientation.Source.PITCH
+            },
+            invertLongitudinal = data.u(2) != 0,
+            invertLateral = data.u(3) != 0,
+            twist = twist,
+        )
+    }
+
+    /** Dieselben acht Byte in die andere Richtung. */
+    fun encoded(orientation: SensorOrientation): ByteArray {
+        val bytes = ByteArray(8)
+        bytes[0] = 1
+        bytes[1] = if (orientation.longitudinalSource == SensorOrientation.Source.ROLL) 1 else 0
+        bytes[2] = if (orientation.invertLongitudinal) 1 else 0
+        bytes[3] = if (orientation.invertLateral) 1 else 0
+        val raw = orientation.twist.toFloat().toRawBits()
+        for (index in 0 until 4) {
+            bytes[4 + index] = ((raw shr (8 * index)) and 0xFF).toByte()
+        }
+        return bytes
+    }
+
    /** Die beiden gespeicherten Offsets. */
    fun offsets(data: ByteArray): Pair<Double, Double>? {
        if (data.size < 8) return null