diff --git a/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/BluetoothManager.kt b/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/BluetoothManager.kt index 69e7de4..0a5d882 100644 --- a/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/BluetoothManager.kt +++ b/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/BluetoothManager.kt @@ -372,6 +372,11 @@ class BluetoothManager( onUpdate = { record(it) }, onStateChange = { handleLinkState(device, it) }, onLevelState = { state -> publish { levelStates[device.id] = state } }, + onDeviceOrientation = { orientation -> + // Im Gerät steht, wie der Sensor eingebaut ist – für alle + // Clients dasselbe. Also übernehmen statt überschreiben. + adoptOrientation(orientation, device.id) + }, ) session.orientation = device.sensorOrientation levelSessions[device.address] = session @@ -642,8 +647,32 @@ class BluetoothManager( // MARK: - Neigungsmesser + /** + * Nach einer Änderung der Einbaulage aufrufen. + * + * Die Lage wandert zusätzlich ins Gerät. Dort gehört sie hin: Sie + * beschreibt den Einbau, und iPhone wie Uhr finden sie dann vor, ohne dass + * jemand sie ein zweites Mal bestimmen muss. + */ fun updateSensorOrientation(device: ConfiguredDevice) { - handler.post { levelSessions[device.address]?.orientation = device.sensorOrientation } + handler.post { + val session = levelSessions[device.address] ?: return@post + session.orientation = device.sensorOrientation + session.storeOrientation(device.sensorOrientation) + } + } + + /** + * Übernimmt die Einbaulage, die das Gerät meldet. + * + * Steht dort noch nichts, meldet das Gerät nichts – dann bleibt es bei der + * örtlich gespeicherten Fassung, und die wird beim nächsten Bestimmen + * hinaufgeschrieben. + */ + private fun adoptOrientation(orientation: SensorOrientation, deviceID: UUID) { + val device = store.devices.firstOrNull { it.id == deviceID } ?: return + if (device.sensorOrientation == orientation) return + store.update(device.copy(sensorOrientation = orientation)) } fun calibrateLevel(deviceID: UUID) { diff --git a/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/LevelSession.kt b/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/LevelSession.kt index 9f97f1f..4607687 100644 --- a/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/LevelSession.kt +++ b/Android/app/src/main/kotlin/de/fritob/campermonitor/bluetooth/LevelSession.kt @@ -30,6 +30,12 @@ class LevelSession( private val onUpdate: (DeviceSnapshot) -> Unit, private val onStateChange: (DeviceLinkState) -> Unit, private val onLevelState: (LevelState) -> Unit, + /** + * Meldet die Einbaulage, die im Gerät steht. Sie gilt vor der örtlich + * gespeicherten: dort steht, wie der Sensor eingebaut ist, und das ist für + * alle Clients dasselbe. + */ + private val onDeviceOrientation: (SensorOrientation) -> Unit = {}, ) { private companion object { val SERVICE: UUID = UUID.fromString(VanAlignProtocol.SERVICE_UUID) @@ -37,6 +43,7 @@ class LevelSession( val ROLL: UUID = UUID.fromString(VanAlignProtocol.ROLL_UUID) val OFFSETS: UUID = UUID.fromString(VanAlignProtocol.OFFSETS_UUID) val CALIBRATE: UUID = UUID.fromString(VanAlignProtocol.CALIBRATE_UUID) + val ORIENTATION: UUID = UUID.fromString(VanAlignProtocol.ORIENTATION_UUID) val CLIENT_CONFIG: UUID = UUID.fromString("00002902-0000-1000-8000-00805F9B34FB") const val POLL_INTERVAL_MS = 1_000L } @@ -53,6 +60,7 @@ class LevelSession( private var rollCharacteristic: BluetoothGattCharacteristic? = null private var offsetsCharacteristic: BluetoothGattCharacteristic? = null private var calibrateCharacteristic: BluetoothGattCharacteristic? = null + private var orientationCharacteristic: BluetoothGattCharacteristic? = null /** Ob die Firmware die Werte von sich aus meldet. */ private var needsPolling = true @@ -84,6 +92,7 @@ class LevelSession( rollCharacteristic = null offsetsCharacteristic = null calibrateCharacteristic = null + orientationCharacteristic = null } /** Setzt die aktuelle Lage als neue Null. */ @@ -107,6 +116,22 @@ class LevelSession( handler.postDelayed({ readOffsets() }, 400) } + /** + * Schreibt die Einbaulage ins Gerät, damit alle Clients dieselbe sehen. + * + * Ältere Firmware hat die Charakteristik nicht; dann bleibt es bei der + * örtlich gespeicherten Fassung, und es passiert schlicht nichts. + */ + fun storeOrientation(value: SensorOrientation) { + val characteristic = orientationCharacteristic ?: return + val g = gatt ?: return + characteristic.writeType = BluetoothGattCharacteristic.WRITE_TYPE_DEFAULT + characteristic.value = VanAlignProtocol.encoded(value) + g.writeCharacteristic(characteristic) + // Zurücklesen, damit gilt, was wirklich im Gerät steht. + handler.postDelayed({ gatt?.readCharacteristic(characteristic) }, 300) + } + private fun readOffsets() { val characteristic = offsetsCharacteristic ?: return gatt?.readCharacteristic(characteristic) @@ -169,6 +194,7 @@ class LevelSession( rollCharacteristic = service.getCharacteristic(ROLL) offsetsCharacteristic = service.getCharacteristic(OFFSETS) calibrateCharacteristic = service.getCharacteristic(CALIBRATE) + orientationCharacteristic = service.getCharacteristic(ORIENTATION) val canNotify = pitchCharacteristic?.let { it.properties and BluetoothGattCharacteristic.PROPERTY_NOTIFY != 0 @@ -267,6 +293,26 @@ class LevelSession( state = state.copy(pitchOffset = pitch, rollOffset = roll) } publish() + // Android lässt nur eine Abfrage gleichzeitig zu, deshalb der + // Reihe nach: erst die Offsets, dann die Einbaulage, dann der + // Takt. Fehlt die Charakteristik – ältere Firmware –, geht es + // sofort weiter. + val characteristic = orientationCharacteristic + if (characteristic != null) { + gatt?.readCharacteristic(characteristic) + } else { + startPollingIfNeeded() + } + } + + ORIENTATION -> { + // Was im Gerät steht, gilt. Steht dort nichts (Version 0), + // bleibt es bei der örtlichen Fassung, und der Aufrufer + // schreibt sie hinauf. + VanAlignProtocol.orientation(value)?.let { stored -> + if (stored != orientation) orientation = stored + onDeviceOrientation(stored) + } startPollingIfNeeded() } } diff --git a/Android/app/src/main/kotlin/de/fritob/campermonitor/store/DeviceStore.kt b/Android/app/src/main/kotlin/de/fritob/campermonitor/store/DeviceStore.kt index 68f1677..9ef2467 100644 --- a/Android/app/src/main/kotlin/de/fritob/campermonitor/store/DeviceStore.kt +++ b/Android/app/src/main/kotlin/de/fritob/campermonitor/store/DeviceStore.kt @@ -206,6 +206,7 @@ class DeviceStore(context: Context) { put("orientationSource", device.sensorOrientation.longitudinalSource.name) put("invertLongitudinal", device.sensorOrientation.invertLongitudinal) put("invertLateral", device.sensorOrientation.invertLateral) + put("twist", device.sensorOrientation.twist) } private fun deviceFrom(json: JSONObject): ConfiguredDevice? { @@ -227,6 +228,9 @@ class DeviceStore(context: Context) { }.getOrDefault(SensorOrientation.Source.PITCH), invertLongitudinal = json.optBoolean("invertLongitudinal", false), invertLateral = json.optBoolean("invertLateral", false), + // Fehlt der Wert, ist er null – so überleben Einrichtungen aus + // der Zeit vor der Verdrehung. + twist = json.optDouble("twist", 0.0), ), ) } diff --git a/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/SensorOrientation.kt b/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/SensorOrientation.kt index af00c74..28c867d 100644 --- a/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/SensorOrientation.kt +++ b/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/SensorOrientation.kt @@ -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 { 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) ) } } diff --git a/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/VanAlignProtocol.kt b/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/VanAlignProtocol.kt index 24714ea..1ff62ac 100644 --- a/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/VanAlignProtocol.kt +++ b/Android/protocol/src/main/kotlin/de/fritob/campermonitor/protocol/VanAlignProtocol.kt @@ -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? { if (data.size < 8) return null diff --git a/CamperMonitor/Bluetooth/BluetoothManager.swift b/CamperMonitor/Bluetooth/BluetoothManager.swift index e8c2ee3..89caeca 100644 --- a/CamperMonitor/Bluetooth/BluetoothManager.swift +++ b/CamperMonitor/Bluetooth/BluetoothManager.swift @@ -381,14 +381,31 @@ final class BluetoothManager: NSObject { } /// Nach einer Änderung der Einbaulage aufrufen. + /// + /// Die Lage wandert zusätzlich ins Gerät. Dort gehört sie hin: Sie + /// beschreibt den Einbau, und Uhr wie Android-App finden sie dann vor, + /// ohne dass jemand sie ein zweites Mal bestimmen muss. func updateSensorOrientation(for device: ConfiguredDevice) { let deviceID = device.id, orientation = device.sensorOrientation queue.async { guard let session = self.levelSession(for: deviceID) else { return } session.orientation = orientation + session.storeOrientation(orientation) } } + /// Übernimmt die Einbaulage, die das Gerät meldet. + /// + /// Steht dort noch nichts, meldet das Gerät nichts – dann bleibt es bei der + /// örtlich gespeicherten Fassung, und die wird beim nächsten Bestimmen + /// hinaufgeschrieben. + private func adoptOrientation(_ orientation: SensorOrientation, for deviceID: UUID) { + guard var device = store.devices.first(where: { $0.id == deviceID }), + device.sensorOrientation != orientation else { return } + device.sensorOrientation = orientation + store.update(device) + } + /// Setzt die aktuelle Lage des Fahrzeugs als neue Null. func calibrateLevel(for deviceID: UUID) { queue.async { self.levelSession(for: deviceID)?.calibrate() } @@ -742,6 +759,11 @@ extension BluetoothManager: CBCentralManagerDelegate { }, onLevelState: { [weak self] state in self?.publish { self?.levelStates[device.id] = state } + }, + onDeviceOrientation: { [weak self] orientation in + // Im Gerät steht, wie der Sensor eingebaut ist – für alle + // Clients dasselbe. Also übernehmen statt überschreiben. + self?.publish { self?.adoptOrientation(orientation, for: device.id) } } ) session.orientation = device.sensorOrientation diff --git a/CamperMonitorWatch/CamperWatchApp.swift b/CamperMonitorWatch/CamperWatchApp.swift index 73bba06..678e366 100644 --- a/CamperMonitorWatch/CamperWatchApp.swift +++ b/CamperMonitorWatch/CamperWatchApp.swift @@ -30,10 +30,12 @@ struct CamperWatchApp: App { // die App einmal im Hintergrund war. link.setLive(true) } - // Die Einbaulage wird am iPhone bestimmt; die Uhr übernimmt sie - // und behält sie, damit sie auch allein richtig rechnet. + // Die Einbaulage kommt bevorzugt aus dem Sensor selbst. Nur + // falls dort noch nichts steht – ältere Firmware, oder nie + // bestimmt –, gilt, was das iPhone meldet. .onChange(of: link.payload?.levelDevice?.orientation) { _, orientation in - if let orientation { radio.orientation = orientation } + guard let orientation, !radio.hasDeviceOrientation else { return } + radio.orientation = orientation } } .onChange(of: scenePhase) { _, phase in diff --git a/CamperMonitorWatch/WatchLevelRadio.swift b/CamperMonitorWatch/WatchLevelRadio.swift index 2887c71..e3da607 100644 --- a/CamperMonitorWatch/WatchLevelRadio.swift +++ b/CamperMonitorWatch/WatchLevelRadio.swift @@ -28,7 +28,13 @@ final class WatchLevelRadio: NSObject { private(set) var updatedAt: Date? private(set) var statusText = "Bluetooth wird gestartet…" - /// Die Einbaulage des Sensors. Kommt vom iPhone und wird hier behalten, + /// Ob die Einbaulage aus dem Gerät selbst stammt. Dann hat sie Vorrang vor + /// dem, was das iPhone meldet – im Gerät steht, wie der Sensor eingebaut + /// ist, und das ist für alle Clients dasselbe. + private(set) var hasDeviceOrientation = false + + /// Die Einbaulage des Sensors. Kommt vom Gerät oder vom iPhone und wird + /// hier behalten, /// damit die Uhr auch ohne Telefon richtig rechnet – ohne sie stünden /// längs und quer je nach Einbau vertauscht oder mit falschem Vorzeichen. var orientation = SensorOrientation.identity { @@ -207,6 +213,14 @@ extension WatchLevelRadio: CBCentralManagerDelegate { self.updatedAt = now self.statusText = "Direkt am Sensor" } + }, + onDeviceOrientation: { [weak self] orientation in + guard let self else { return } + Self.store(orientation) + self.publish { + self.hasDeviceOrientation = true + self.orientation = orientation + } }) session.orientation = orientation self.session = session diff --git a/README.md b/README.md index 66b2dbb..08703b1 100644 --- a/README.md +++ b/README.md @@ -188,6 +188,33 @@ um die Hochachse steckt darin nicht – eben sieht in jeder Verdrehung gleich aus. Dafür braucht es zwei Kippbewegungen in bekannte Richtungen, und genau die macht der Einbaulage-Assistent. +### Beides liegt im Sensor, nicht in der App + +Sowohl die **Kalibrierung** als auch die **Einbaulage** speichert der ESP32 +selbst und gibt sie an jeden aus, der fragt. Das ist der Punkt, sobald mehr als +ein Gerät im Spiel ist: iPhone, Apple Watch und Android-App zeigen dasselbe, +und bestimmt werden muss beides nur ein einziges Mal, von welchem Gerät aus +auch immer. + +| | wo | +|---|---| +| Nullpunkt der Kalibrierung | im ESP (`pitch_offset`, `roll_offset`), wird dort auch schon abgezogen | +| Einbaulage samt Verdrehung | im ESP, Charakteristik `…3428`, lesbar und schreibbar | +| Spurweite und Radstand | in der jeweiligen App, im Fahrzeugprofil | + +Beim Verbinden liest jede App die Einbaulage aus dem Gerät und übernimmt sie. +Wer sie neu bestimmt, schreibt sie hinauf. Steht dort noch nichts – ältere +Firmware oder nie bestimmt –, gilt weiter, was die App örtlich gespeichert hat; +kaputtgehen kann dabei nichts. + +Angewandt wird sie trotzdem in den Apps und nicht im Sensor. Der Sensor +verwahrt sie nur: Würde er die Winkel schon umgerechnet melden, rechnete jede +ältere App die Korrektur ein zweites Mal ein. + +Die acht Byte der Charakteristik sind in `VanAlignProtocol` beschrieben und in +`run-tests.sh` byteweise festgenagelt – daran hängen drei Apps und die +Firmware. + **Danach kalibrieren:** Fahrzeug eben stellen, dann *Auf aktuelle Lage kalibrieren*. Ohne das zeigt die Anzeige die Lage des Sensors, nicht die des Fahrzeugs – je nachdem, wie schief er eingebaut ist. diff --git a/Shared/Bluetooth/LevelSession.swift b/Shared/Bluetooth/LevelSession.swift index c969d46..c22dd34 100644 --- a/Shared/Bluetooth/LevelSession.swift +++ b/Shared/Bluetooth/LevelSession.swift @@ -18,6 +18,7 @@ final class LevelSession: NSObject { private static let rollUUID = CBUUID(string: VanAlignProtocol.rollUUID) private static let offsetsUUID = CBUUID(string: VanAlignProtocol.offsetsUUID) private static let calibrateUUID = CBUUID(string: VanAlignProtocol.calibrateUUID) + private static let orientationUUID = CBUUID(string: VanAlignProtocol.orientationUUID) let deviceID: UUID private let queue: DispatchQueue @@ -25,11 +26,16 @@ final class LevelSession: NSObject { private let onUpdate: (DeviceSnapshot) -> Void private let onStateChange: (DeviceLinkState) -> Void private let onLevelState: (LevelState) -> Void + /// Meldet die Einbaulage, die im Gerät steht. Sie gilt vor der örtlich + /// gespeicherten: dort steht, wie der Sensor eingebaut ist, und das ist für + /// alle Clients dasselbe. + private let onDeviceOrientation: ((SensorOrientation) -> Void)? private var pitchCharacteristic: CBCharacteristic? private var rollCharacteristic: CBCharacteristic? private var offsetsCharacteristic: CBCharacteristic? private var calibrateCharacteristic: CBCharacteristic? + private var orientationCharacteristic: CBCharacteristic? private var state = LevelState() /// Aus den Geräteeinstellungen; rechnet Sensor- in Fahrzeugachsen um. @@ -49,7 +55,9 @@ final class LevelSession: NSObject { queue: DispatchQueue, onUpdate: @escaping (DeviceSnapshot) -> Void, onStateChange: @escaping (DeviceLinkState) -> Void, - onLevelState: @escaping (LevelState) -> Void) { + onLevelState: @escaping (LevelState) -> Void, + onDeviceOrientation: ((SensorOrientation) -> Void)? = nil) { + self.onDeviceOrientation = onDeviceOrientation self.deviceID = deviceID self.queue = queue self.peripheral = peripheral @@ -89,6 +97,28 @@ final class LevelSession: NSObject { rollCharacteristic = nil offsetsCharacteristic = nil calibrateCharacteristic = nil + orientationCharacteristic = nil + } + + // MARK: - Einbaulage im Gerät + + /// Schreibt die Einbaulage ins Gerät, damit alle Clients dieselbe sehen. + /// + /// Ältere Firmware hat die Charakteristik nicht; dann bleibt es bei der + /// örtlich gespeicherten Fassung, und es passiert schlicht nichts. + func storeOrientation(_ orientation: SensorOrientation) { + guard let characteristic = orientationCharacteristic, + peripheral.state == .connected else { return } + let type: CBCharacteristicWriteType = + characteristic.properties.contains(.write) ? .withResponse : .withoutResponse + peripheral.writeValue(VanAlignProtocol.encoded(orientation), + for: characteristic, type: type) + // Zurücklesen, damit angezeigt wird, was wirklich im Gerät steht. + queue.asyncAfter(deadline: .now() + 0.3) { [weak self] in + guard let self, let characteristic = self.orientationCharacteristic, + self.peripheral.state == .connected else { return } + self.peripheral.readValue(for: characteristic) + } } // MARK: - Kalibrieren @@ -169,7 +199,8 @@ extension LevelSession: CBPeripheralDelegate { return } peripheral.discoverCharacteristics( - [Self.pitchUUID, Self.rollUUID, Self.offsetsUUID, Self.calibrateUUID], + [Self.pitchUUID, Self.rollUUID, Self.offsetsUUID, + Self.calibrateUUID, Self.orientationUUID], for: service ) } @@ -188,6 +219,7 @@ extension LevelSession: CBPeripheralDelegate { case Self.rollUUID: rollCharacteristic = characteristic case Self.offsetsUUID: offsetsCharacteristic = characteristic case Self.calibrateUUID: calibrateCharacteristic = characteristic + case Self.orientationUUID: orientationCharacteristic = characteristic default: break } } @@ -210,7 +242,8 @@ extension LevelSession: CBPeripheralDelegate { startPollingIfNeeded() // Einmal alles lesen, damit sofort etwas dasteht. - for characteristic in [pitchCharacteristic, rollCharacteristic, offsetsCharacteristic] { + for characteristic in [pitchCharacteristic, rollCharacteristic, + offsetsCharacteristic, orientationCharacteristic] { guard let characteristic, characteristic.properties.contains(.read) else { continue } peripheral.readValue(for: characteristic) } @@ -233,6 +266,16 @@ extension LevelSession: CBPeripheralDelegate { state.pitchOffset = offsets.pitch state.rollOffset = offsets.roll } + case Self.orientationUUID: + // Was im Gerät steht, gilt: es beschreibt den Einbau, nicht das + // Telefon. Steht dort nichts (Version 0), bleibt es bei der + // örtlichen Fassung – der Aufrufer schreibt sie dann hinauf. + guard let stored = VanAlignProtocol.orientation(from: value) else { return } + if stored != orientation { + orientation = stored + } + onDeviceOrientation?(stored) + return default: return } diff --git a/Shared/Bluetooth/VanAlignProtocol.swift b/Shared/Bluetooth/VanAlignProtocol.swift index f79e1ce..0a83418 100644 --- a/Shared/Bluetooth/VanAlignProtocol.swift +++ b/Shared/Bluetooth/VanAlignProtocol.swift @@ -16,6 +16,8 @@ enum VanAlignProtocol { static let offsetsUUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233426" /// Ein Byte: 0 setzt zurück, alles andere kalibriert auf die aktuelle Lage. static let calibrateUUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233427" + /// Acht Byte: die Einbaulage, lesbar und schreibbar. Siehe `orientation`. + static let orientationUUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233428" static let calibrateCommand = Data([0x01]) static let resetCommand = Data([0x00]) @@ -41,6 +43,50 @@ enum VanAlignProtocol { return Double(value) } + /// 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 nil; dann gilt, + /// was die App örtlich gespeichert hat, und sie schreibt es hinauf. + static func orientation(from data: Data) -> SensorOrientation? { + let bytes = [UInt8](data) + guard bytes.count >= 8, bytes[0] == 1 else { return nil } + var orientation = SensorOrientation() + orientation.longitudinalSource = bytes[1] == 1 ? .roll : .pitch + orientation.invertLongitudinal = bytes[2] != 0 + orientation.invertLateral = bytes[3] != 0 + guard let twist = angle(from: data.dropFirst(4).prefix(4)) else { return nil } + orientation.twist = twist + return orientation + } + + /// Dieselben acht Byte in die andere Richtung. + static func encoded(_ orientation: SensorOrientation) -> Data { + var bytes = [UInt8](repeating: 0, count: 8) + bytes[0] = 1 + bytes[1] = orientation.longitudinalSource == .roll ? 1 : 0 + bytes[2] = orientation.invertLongitudinal ? 1 : 0 + bytes[3] = orientation.invertLateral ? 1 : 0 + let raw = Float(orientation.twist).bitPattern + for index in 0..<4 { + bytes[4 + index] = UInt8((raw >> UInt32(8 * index)) & 0xFF) + } + return Data(bytes) + } + /// Die beiden gespeicherten Offsets. static func offsets(from data: Data) -> (pitch: Double, roll: Double)? { guard data.count >= 8, diff --git a/Tests/main.swift b/Tests/main.swift index 016a18f..5ee4b74 100644 --- a/Tests/main.swift +++ b/Tests/main.swift @@ -970,6 +970,42 @@ let untouched = SensorOrientation.identity.apply(pitch: 1.5, roll: -0.5) checkEqual("unveränderte Lage lässt längs stehen", untouched.pitch, 1.5) checkEqual("unveränderte Lage lässt quer stehen", untouched.roll, -0.5) +// Die Einbaulage liegt im Gerät, damit iPhone, Uhr und Android dieselbe sehen. +// Acht Byte, hin und zurück. +print("\nEinbaulage im Gerät") + +var stored = SensorOrientation() +stored.longitudinalSource = .roll +stored.invertLongitudinal = true +stored.invertLateral = false +stored.twist = -17.5 + +let storedBytes = VanAlignProtocol.encoded(stored) +checkEqual("acht Byte lang", storedBytes.count, 8) +checkEqual("Version steht vorne", storedBytes.first, 1) +checkEqual("gelesen kommt dasselbe zurück", VanAlignProtocol.orientation(from: storedBytes), stored) + +// Ein Gerät, in dem noch nie etwas stand, meldet Version 0. Das ist kein +// Fehler, sondern heisst "hier gilt, was die App hat" – und die schreibt sie +// dann hinauf. +checkEqual("Version 0 gilt als leer", + VanAlignProtocol.orientation(from: Data(repeating: 0, count: 8)) == nil, true) +checkEqual("zu kurze Antwort ergibt nichts", + VanAlignProtocol.orientation(from: Data([1, 0, 0, 0])) == nil, true) + +// Die Feldbelegung byteweise festgenagelt: daran hängen drei Apps und die +// Firmware, das darf nicht unbemerkt verrutschen. +var plain = SensorOrientation() +plain.twist = 1 +checkEqual("Feldbelegung", [UInt8](VanAlignProtocol.encoded(plain)), + [1, 0, 0, 0, 0x00, 0x00, 0x80, 0x3F]) // 1.0f little endian + +var mounted = SensorOrientation() +mounted.longitudinalSource = .roll +mounted.invertLateral = true +checkEqual("Achsentausch und Vorzeichen an ihrem Platz", + [UInt8](VanAlignProtocol.encoded(mounted)).prefix(4).map { $0 }, [1, 1, 0, 1]) + // Gespeicherte Einrichtungen müssen ältere Fassungen überleben. Ein neues Feld // darf das Lesen nicht scheitern lassen – sonst ist die Geräteliste leer, und // das nächste Speichern schreibt diese Leere über den Bestand. diff --git a/firmware/vanalign/esp32_ble.yaml b/firmware/vanalign/esp32_ble.yaml index 8c1f2e7..fc9165b 100644 --- a/firmware/vanalign/esp32_ble.yaml +++ b/firmware/vanalign/esp32_ble.yaml @@ -105,6 +105,32 @@ globals: type: float restore_value: yes initial_value: '0.0' + # Die Einbaulage liegt im Gerät, nicht in den Apps: Sie beschreibt, wie der + # Sensor im Fahrzeug sitzt – eine Eigenschaft des Einbaus, nicht des Telefons. + # Damit sehen iPhone, Uhr und Android dasselbe, ohne sie je einzeln zu + # bestimmen. Angewandt wird sie weiterhin in den Apps; das Gerät verwahrt sie + # nur, sonst rechneten ältere Clients die Korrektur ein zweites Mal. + - id: orientation_version + type: uint8_t + restore_value: yes + initial_value: '0' + - id: orientation_source + type: uint8_t + restore_value: yes + initial_value: '0' + - id: orientation_invert_long + type: bool + restore_value: yes + initial_value: 'false' + - id: orientation_invert_lat + type: bool + restore_value: yes + initial_value: 'false' + - id: orientation_twist + type: float + restore_value: yes + initial_value: '0.0' + - id: enable_captive type: bool restore_value: yes @@ -133,6 +159,63 @@ esp32_ble_server: float val = id(roll).state; memcpy(v.data(), &val, sizeof(float)); return v; + # Die gespeicherten Nullpunkte, zwei Floats. Daran erkennen die Apps, + # ob überhaupt schon kalibriert wurde. + - id: offsets_ble + uuid: cad48e28-7fbe-41cf-bae9-d77a6c233426 + description: "Kalibrier-Offsets" + read: true + value: !lambda |- + std::vector v(2 * sizeof(float)); + float p = id(pitch_offset); + float r = id(roll_offset); + memcpy(v.data(), &p, sizeof(float)); + memcpy(v.data() + sizeof(float), &r, sizeof(float)); + return v; + + # Die Einbaulage, 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 + - id: orientation_ble + uuid: cad48e28-7fbe-41cf-bae9-d77a6c233428 + description: "Einbaulage" + read: true + write: true + value: !lambda |- + std::vector v(8, 0); + v[0] = id(orientation_version); + v[1] = id(orientation_source); + v[2] = id(orientation_invert_long) ? 1 : 0; + v[3] = id(orientation_invert_lat) ? 1 : 0; + float t = id(orientation_twist); + memcpy(v.data() + 4, &t, sizeof(float)); + return v; + on_write: + then: + - lambda: |- + if (x.size() < 8 || x[0] != 1) { + ESP_LOGW("vanalign", "Einbaulage verworfen: %d Byte, Version %d", + (int) x.size(), x.empty() ? -1 : (int) x[0]); + return; + } + float t; + memcpy(&t, x.data() + 4, sizeof(float)); + if (!std::isfinite(t) || fabsf(t) > 180.0f) { + ESP_LOGW("vanalign", "Einbaulage verworfen: Verdrehung %.1f", t); + return; + } + id(orientation_version) = 1; + id(orientation_source) = x[1]; + id(orientation_invert_long) = x[2] != 0; + id(orientation_invert_lat) = x[3] != 0; + id(orientation_twist) = t; + ESP_LOGI("vanalign", "Einbaulage gespeichert: Quelle=%d laengs=%d quer=%d verdreht=%.1f", + (int) x[1], (int) x[2], (int) x[3], t); + - id: calib_ble uuid: cad48e28-7fbe-41cf-bae9-d77a6c233427 description: "Kalibriere Neigung"