witmotion-react-native
Version:
React Native SDK for WitMotion BLE IMU sensors (WT901, WT901BLECL, etc.)
218 lines (217 loc) • 7.69 kB
JavaScript
"use strict";
/**
* WitMotion BLE Parser for React Native
*
* Parses packets according to the official WIT Standard Protocol.
* Supports:
* - Standard packets (0x50–0x5A, 11 bytes with checksum)
* - Combined packets 0x61 (20 bytes without checksum, or fragmented)
* - Register packets 0x71 (20 or 22 bytes, depending on firmware version)
*
* Reference: WitMotion SDK / WT901BLECL Datasheet
*/
Object.defineProperty(exports, "__esModule", { value: true });
exports.WitStreamParser = exports.WitReg = void 0;
/**
* Register codes for 0x71 packets (mirroring the official SDK).
* Extend this enum if your device firmware provides additional registers.
*/
var WitReg;
(function (WitReg) {
WitReg[WitReg["Quaternion"] = 0] = "Quaternion";
WitReg[WitReg["Magnetometer"] = 1] = "Magnetometer";
WitReg[WitReg["Temperature"] = 2] = "Temperature";
// Add more registers here if needed
})(WitReg || (exports.WitReg = WitReg = {}));
class WitStreamParser {
buf = [];
comb61 = [];
onData;
push(bytes) {
for (const b of bytes)
this.buf.push(b);
while (this.buf.length >= 3) {
if (this.buf[0] !== 0x55) {
this.buf.shift();
continue;
}
const id = this.buf[1];
// --- Combined packet 0x61 (20 bytes total, no checksum) ---
if (id === 0x61) {
if (this.buf.length >= 20) {
const pkt = this.buf.slice(0, 20);
this.decode61(pkt.slice(2, 20));
this.buf.splice(0, 20);
continue;
}
// Handle fragmented BLE notifications
if (this.buf.length > 2) {
let take = 0;
for (let i = 2; i < this.buf.length; i++) {
if (this.buf[i] === 0x55)
break; // new header
take++;
}
if (take > 0) {
const chunk = this.buf.slice(2, 2 + take);
this.comb61.push(...chunk);
this.buf.splice(0, 2 + take);
while (this.comb61.length >= 18) {
const body = this.comb61.slice(0, 18);
this.decode61(body);
this.comb61 = this.comb61.slice(18);
}
continue;
}
break;
}
break;
}
// --- Standard packets 0x50–0x5A (11 bytes with checksum) ---
if (id >= 0x50 && id <= 0x5A) {
if (this.buf.length < 11)
break;
const pkt = this.buf.slice(0, 11);
const sum = pkt.slice(0, 10).reduce((a, v) => (a + v) & 0xff, 0);
if (sum !== pkt[10]) {
this.buf.shift();
continue;
}
this.decodeStd(pkt);
this.buf.splice(0, 11);
continue;
}
// --- Register packets 0x71 (20 or 22 bytes) ---
if (id === 0x71) {
// Case with checksum (22 bytes)
if (this.buf.length >= 22) {
const pkt = this.buf.slice(0, 22);
const sum = pkt.slice(0, 21).reduce((a, v) => (a + v) & 0xff, 0);
if (sum === pkt[21]) {
this.decode71(pkt.slice(2, 20)); // pass register+16 data bytes
this.buf.splice(0, 22);
continue;
}
}
// Case without checksum (20 bytes, some BLE firmwares)
if (this.buf.length >= 20) {
const pkt = this.buf.slice(0, 20);
this.decode71(pkt.slice(2, 20));
this.buf.splice(0, 20);
continue;
}
break;
}
// Unknown ID → resync
this.buf.shift();
}
}
i16(lo, hi) {
let v = (hi << 8) | lo;
if (v & 0x8000)
v -= 0x10000;
return v;
}
decode61(body18) {
const i16 = (i) => this.i16(body18[i], body18[i + 1]);
const g = 9.80665;
const ax = (i16(0) / 32768) * 16 * g;
const ay = (i16(2) / 32768) * 16 * g;
const az = (i16(4) / 32768) * 16 * g;
const gx = (i16(6) / 32768) * 2000;
const gy = (i16(8) / 32768) * 2000;
const gz = (i16(10) / 32768) * 2000;
const roll = (i16(12) / 32768) * 180;
const pitch = (i16(14) / 32768) * 180;
const yaw = (i16(16) / 32768) * 180;
const out = {
acc: { x: ax, y: ay, z: az },
gyro: { x: gx, y: gy, z: gz },
angle: { roll, pitch, yaw },
};
this.onData?.(out);
}
decodeStd(pkt11) {
const id = pkt11[1];
const p = (i) => pkt11[2 + i];
const i16 = (i) => this.i16(p(i), p(i + 1));
const out = {};
switch (id) {
case 0x51: { // ACC
const g = 9.80665;
out.acc = {
x: (i16(0) / 32768) * 16 * g,
y: (i16(2) / 32768) * 16 * g,
z: (i16(4) / 32768) * 16 * g,
};
break;
}
case 0x52: { // GYRO
out.gyro = {
x: (i16(0) / 32768) * 2000,
y: (i16(2) / 32768) * 2000,
z: (i16(4) / 32768) * 2000,
};
break;
}
case 0x53: { // ANGLE
out.angle = {
roll: (i16(0) / 32768) * 180,
pitch: (i16(2) / 32768) * 180,
yaw: (i16(4) / 32768) * 180,
};
break;
}
case 0x54: { // MAG
out.mag = { x: i16(0), y: i16(2), z: i16(4) };
break;
}
case 0x59: { // QUAT
out.quat = {
w: i16(0) / 32768,
x: i16(2) / 32768,
y: i16(4) / 32768,
z: i16(6) / 32768,
};
break;
}
default: break;
}
if (out.acc || out.gyro || out.angle || out.mag || out.quat) {
this.onData?.(out);
}
}
decode71(data18) {
const reg = data18[0];
const i16 = (i) => this.i16(data18[i], data18[i + 1]);
const out = {};
switch (reg) {
case WitReg.Quaternion:
out.quat = {
w: i16(2) / 32768,
x: i16(4) / 32768,
y: i16(6) / 32768,
z: i16(8) / 32768,
};
break;
case WitReg.Magnetometer:
out.mag = {
x: i16(2),
y: i16(4),
z: i16(6),
};
break;
case WitReg.Temperature:
out.temp = i16(2) / 100;
break;
default:
if (__DEV__)
console.log('Unhandled 0x71 register:', reg, data18);
break;
}
if (out.acc || out.gyro || out.angle || out.mag || out.quat || out.temp !== undefined) {
this.onData?.(out);
}
}
}
exports.WitStreamParser = WitStreamParser;