bno055-imu-node
Version:
Interact with a BNO055 IMU from your Node.js app
221 lines (220 loc) • 9.25 kB
JavaScript
"use strict";
Object.defineProperty(exports, "__esModule", { value: true });
const i2c = require("i2c-bus");
const constants_1 = require("./constants");
const wait = (t) => new Promise(ok => setTimeout(ok, t));
class BNO055 {
constructor(bus, address) {
this.bus = bus;
this.address = address;
this.mode = constants_1.OpMode.Config;
this.units = {
accel: 'mps2',
euler: 'deg',
gyro: 'degps',
temp: 'c',
};
}
static async begin(address, mode = constants_1.OpMode.FullFusion, busNumber = 1) {
const bus = await i2c.openPromisified(busNumber);
const device = new BNO055(bus, address);
await device.verifyConnection();
await device.setMode(mode);
await device.useExternalClock();
await device.getUnits();
return device;
}
async getAxisMapping() {
const axisMaps = await this.bus.readByte(this.address, constants_1.Reg.AXIS_MAP_CONFIG);
const axisSigns = await this.bus.readByte(this.address, constants_1.Reg.AXIS_MAP_SIGN);
return {
X: {
axis: axisMaps & 0x3,
sign: axisSigns >> 2 & 0x1,
},
Y: {
axis: axisMaps >> 2 & 0x3,
sign: axisSigns >> 1 & 0x1,
},
Z: {
axis: axisMaps >> 4 & 0x3,
sign: axisSigns & 0x1,
},
};
}
async getCalibrationStatuses() {
const calByte = await this.bus.readByte(this.address, constants_1.Reg.CALIB_STAT);
return {
sys: (calByte >> 6) & 0x03,
gyro: (calByte >> 4) & 0x03,
accel: (calByte >> 2) & 0x03,
mag: calByte & 0x03,
};
}
async getEuler() {
const buffer = await this.readBlock(constants_1.Reg.EULER_H_LSB, 6);
const scale = this.units.euler === 'deg'
? 1 / constants_1.EulerUnitScale.Degs
: 1 / constants_1.EulerUnitScale.Rads;
return {
h: scale * buffer.readInt16LE(0),
r: scale * buffer.readInt16LE(2),
p: scale * buffer.readInt16LE(4),
};
}
async getMode() {
const mode = (await this.bus.readByte(this.address, constants_1.Reg.OPR_MODE)) & 0xF;
this.mode = mode;
return mode;
}
async getPage() {
return await this.bus.readByte(this.address, constants_1.Reg.PAGE_ID) & 0x1;
}
async getQuat() {
const buffer = await this.readBlock(constants_1.Reg.QUATERNION_DATA_W_LSB, 8);
const scale = (1.0 / (1 << 14));
return {
w: scale * buffer.readInt16LE(0),
x: scale * buffer.readInt16LE(2),
y: scale * buffer.readInt16LE(4),
z: scale * buffer.readInt16LE(6),
};
}
async getSelfTestResults() {
const selfTest = await this.bus.readByte(this.address, constants_1.Reg.SELFTEST_RESULT);
return {
mcuPassed: (selfTest >> 3 & 0x1) === 1,
magPassed: (selfTest >> 2 & 0x1) === 1,
accelPassed: (selfTest >> 1 & 0x1) === 1,
gyroPassed: (selfTest & 0x1) === 1,
};
}
async getSensorOffsets() {
if (await this.isFullyCalibrated()) {
const savedMode = this.mode;
await this.setMode(constants_1.OpMode.Config);
const offsets = {
accelX: await this.readDoubleByte(constants_1.Reg.ACCEL_OFFSET_X_LSB),
accelY: await this.bus.readByte(this.address, constants_1.Reg.ACCEL_OFFSET_Y_LSB),
accelZ: await this.bus.readByte(this.address, constants_1.Reg.ACCEL_OFFSET_Z_LSB),
magX: await this.bus.readByte(this.address, constants_1.Reg.MAG_OFFSET_X_LSB),
magY: await this.bus.readByte(this.address, constants_1.Reg.MAG_OFFSET_Y_LSB),
magZ: await this.bus.readByte(this.address, constants_1.Reg.MAG_OFFSET_Z_LSB),
gyroX: await this.bus.readByte(this.address, constants_1.Reg.GYRO_OFFSET_X_LSB),
gyroY: await this.bus.readByte(this.address, constants_1.Reg.GYRO_OFFSET_Y_LSB),
gyroZ: await this.bus.readByte(this.address, constants_1.Reg.GYRO_OFFSET_Z_LSB),
accelRadius: await this.bus.readByte(this.address, constants_1.Reg.ACCEL_RADIUS_LSB),
magRadius: await this.bus.readByte(this.address, constants_1.Reg.MAG_RADIUS_LSB),
};
await this.setMode(savedMode);
return offsets;
}
else {
return;
}
}
async getSystemError() {
return await this.bus.readByte(this.address, constants_1.Reg.SYS_ERR) & 0xF;
}
async getSystemStatus() {
return await this.bus.readByte(this.address, constants_1.Reg.SYS_STAT) & 0x7;
}
async getTemperature() {
const tempByte = await this.bus.readByte(this.address, constants_1.Reg.TEMP);
const temp = Buffer.of(tempByte).readInt8(0);
return temp * (this.units.temp === 'c' ? constants_1.TempUnitScale.C : constants_1.TempUnitScale.F);
}
async getUnits() {
const unitByte = await this.bus.readByte(this.address, constants_1.Reg.UNIT_SEL);
this.units = {
accel: unitByte & 0x1 ? 'mg' : 'mps2',
euler: (unitByte >> 2) & 0x1 ? 'rad' : 'deg',
gyro: (unitByte >> 1) & 0x1 ? 'radps' : 'degps',
temp: (unitByte >> 4) & 0x1 ? 'f' : 'c',
};
return { ...this.units };
}
async getVersions() {
const [device, accel, mag, gyro, swLsb, swMsb, bootloader,] = await this.readBlock(constants_1.Reg.DEVICE_ID, 7);
const software = `${swMsb >> 4}.${swMsb & 0xF}.${swLsb >> 4}.${swLsb & 0xF}`;
return { device, accel, mag, gyro, software, bootloader };
}
/**
* Checks that all relevant calibration status values are set to 3 (fully calibrated)
*/
async isFullyCalibrated() {
const { sys, accel, gyro, mag } = await this.getCalibrationStatuses();
switch (this.mode) {
case constants_1.OpMode.AccelOnly:
return accel === 3;
case constants_1.OpMode.MagOnly:
return mag === 3;
case constants_1.OpMode.GyroOnly:
case constants_1.OpMode.ImuMagForGyro:
return gyro === 3;
case constants_1.OpMode.AccelMag:
case constants_1.OpMode.Compass:
return accel === 3 && mag === 3;
case constants_1.OpMode.AccelGyro:
case constants_1.OpMode.Imu:
return accel === 3 && gyro === 3;
case constants_1.OpMode.MagGyro:
return mag === 3 && gyro === 3;
default:
return sys === 3 && gyro === 3 && accel === 3 && mag === 3;
}
}
async resetSystem() {
const savedMode = this.mode;
await this.setMode(constants_1.OpMode.Config);
await this.bus.writeByte(this.address, constants_1.Reg.SYS_TRIGGER, 0x20);
await wait(2000);
await this.setMode(savedMode);
}
async setAxisMapping({ X, Y, Z }) {
const savedMode = this.mode;
await this.setMode(constants_1.OpMode.Config);
const axisMaps = (Z.axis << 4) | (Y.axis << 2) | X.axis;
const axisSigns = (X.sign << 2) | (Y.sign << 1) | Z.sign;
await this.bus.writeByte(this.address, constants_1.Reg.AXIS_MAP_CONFIG, axisMaps);
await this.bus.writeByte(this.address, constants_1.Reg.AXIS_MAP_SIGN, axisSigns & 0x7);
await this.setMode(savedMode);
}
async setMode(mode) {
await this.bus.writeByte(this.address, constants_1.Reg.OPR_MODE, mode);
await wait(mode === constants_1.OpMode.Config ? constants_1.BNO055_CONFIG_MODE_WAIT : constants_1.BNO055_MODE_SWITCH_WAIT);
this.mode = mode;
}
async setPowerLevel(level = constants_1.PowerLevel.Normal) {
const savedMode = this.mode;
await this.setMode(constants_1.OpMode.Config);
await this.bus.writeByte(this.address, constants_1.Reg.PWR_MODE, level);
await this.bus.writeByte(this.address, constants_1.Reg.PAGE_ID, 0);
await this.setMode(savedMode);
}
async useExternalClock() {
const savedMode = this.mode;
await this.setMode(constants_1.OpMode.Config);
await this.bus.writeByte(this.address, constants_1.Reg.PAGE_ID, 0);
await this.bus.writeByte(this.address, constants_1.Reg.SYS_TRIGGER, 0x80);
await this.setMode(savedMode);
}
async verifyConnection() {
if (await this.bus.readByte(this.address, constants_1.Reg.DEVICE_ID) !== constants_1.BNO055_ID) {
throw new Error(`Device does not seem to be connected`);
}
}
/**
* I2C Helper Methods
*/
async readDoubleByte(reg) {
const [lsb, msb] = await this.readBlock(reg, 2);
return (msb << 8) | lsb;
}
async readBlock(reg, length = 1) {
const buffer = Buffer.alloc(length);
await this.bus.readI2cBlock(this.address, reg, length, buffer);
return buffer;
}
}
exports.BNO055 = BNO055;