98 lines
4.4 KiB
Swift
98 lines
4.4 KiB
Swift
import Foundation
|
||
|
||
/// Neigungsmesser „VanAlign Pro“ – ein ESP32 mit MPU6050, der Längs- und
|
||
/// Querneigung des Fahrzeugs über Bluetooth bereitstellt.
|
||
///
|
||
/// Anders als die übrigen Geräte gibt es hier kein Rahmenprotokoll: Jede
|
||
/// Messgrösse liegt in einer eigenen Charakteristik als 32-Bit-Float.
|
||
enum VanAlignProtocol {
|
||
|
||
/// Wird vom Gerät beworben, das Gerät ist darüber auffindbar.
|
||
static let serviceUUID = "2A24B789-7AAB-4535-AF3E-EE76A35CC42D"
|
||
|
||
static let pitchUUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233424"
|
||
static let rollUUID = "CAD48E28-7FBE-41CF-BAE9-D77A6C233425"
|
||
/// Zwei Floats: die gespeicherten Kalibrier-Offsets.
|
||
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])
|
||
|
||
/// Liest einen Winkel aus vier Bytes, little-endian.
|
||
///
|
||
/// Die Firmware legt den Float per `memcpy` ab, und der ESP32 ist
|
||
/// little-endian – die Reihenfolge steht also fest. Die Web-Oberfläche des
|
||
/// Ursprungsprojekts probiert zusätzlich die umgekehrte Reihenfolge, falls
|
||
/// die erste unplausibel aussieht. Das ist nicht nur unnötig, sondern
|
||
/// schädlich: ein vertauschter Float von 4,25° ergibt gelesen etwa 0,0 und
|
||
/// wirkt damit völlig plausibel. Ein Vorzeichen- oder Wertfehler bliebe so
|
||
/// unbemerkt.
|
||
static func angle(from data: Data) -> Double? {
|
||
guard data.count >= 4 else { return nil }
|
||
var raw: UInt32 = 0
|
||
for (index, byte) in data.prefix(4).enumerated() {
|
||
raw |= UInt32(byte) << UInt32(8 * index)
|
||
}
|
||
let value = Float(bitPattern: raw)
|
||
// NAN meldet die Firmware, solange der Sensor nichts liefert.
|
||
guard value.isFinite, abs(value) <= 180 else { return nil }
|
||
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,
|
||
let pitch = angle(from: data.prefix(4)),
|
||
let roll = angle(from: data.dropFirst(4).prefix(4)) else { return nil }
|
||
return (pitch, roll)
|
||
}
|
||
}
|