| 99 | // ============================================================================ |
| 100 | |
| 101 | function decodeTelemetry(data: ArrayBuffer): DecodedTelemetryOutput | null { |
| 102 | const view = new DataView(data); |
| 103 | |
| 104 | if (data.byteLength < 120) { |
| 105 | return null; |
| 106 | } |
| 107 | |
| 108 | const msgType = view.getUint8(0); |
| 109 | if (msgType !== MSG_TELEMETRY) { |
| 110 | return null; |
| 111 | } |
| 112 | |
| 113 | const mode = view.getUint8(1); |
| 114 | const sequence = view.getUint16(2, true); |
| 115 | const cpu_percent = view.getUint8(4); |
| 116 | const mem_percent = view.getUint8(5); |
| 117 | const disk_percent = view.getUint8(6); |
| 118 | |
| 119 | const poseX = view.getFloat64(8, true); |
| 120 | const poseY = view.getFloat64(16, true); |
| 121 | const poseTheta = view.getFloat64(24, true); |
| 122 | |
| 123 | // Detect old (f64 voltage) vs new (f32 voltage + f32 current) format |
| 124 | const voltageF32 = view.getFloat32(32, true); |
| 125 | const isNewFormat = voltageF32 > 0 && voltageF32 < 100; |
| 126 | const batteryVoltage = isNewFormat ? voltageF32 : view.getFloat64(32, true); |
| 127 | const systemCurrent = isNewFormat ? view.getFloat32(36, true) : 0; |
| 128 | |
| 129 | const timestampLow = view.getUint32(40, true); |
| 130 | const timestampHigh = view.getUint32(44, true); |
| 131 | const timestamp_us = timestampLow + timestampHigh * 0x100000000; |
| 132 | |
| 133 | const cmdLinear = view.getFloat32(48, true); |
| 134 | const cmdAngular = view.getFloat32(52, true); |
| 135 | |
| 136 | const measLinear = view.getFloat32(56, true); |
| 137 | const measAngular = view.getFloat32(60, true); |
| 138 | |
| 139 | const accelLinear = view.getFloat32(64, true); |
| 140 | const accelAngular = view.getFloat32(68, true); |
| 141 | |
| 142 | const motor_temps: [number, number, number, number] = [ |
| 143 | view.getFloat32(72, true), |
| 144 | view.getFloat32(76, true), |
| 145 | view.getFloat32(80, true), |
| 146 | view.getFloat32(84, true), |
| 147 | ]; |
| 148 | |
| 149 | const motor_currents: [number, number, number, number] = [ |
| 150 | view.getFloat32(88, true), |
| 151 | view.getFloat32(92, true), |
| 152 | view.getFloat32(96, true), |
| 153 | view.getFloat32(100, true), |
| 154 | ]; |
| 155 | |
| 156 | const healthBits = view.getUint16(104, true); |
| 157 | const health = { |
| 158 | can_healthy: (healthBits & (1 << 0)) !== 0, |