di-sensors
Version:
Drivers and examples for using DI_Sensors in Node.js
466 lines (390 loc) • 16.7 kB
JavaScript
'use strict';
var _createClass = function () { function defineProperties(target, props) { for (var i = 0; i < props.length; i++) { var descriptor = props[i]; descriptor.enumerable = descriptor.enumerable || false; descriptor.configurable = true; if ("value" in descriptor) descriptor.writable = true; Object.defineProperty(target, descriptor.key, descriptor); } } return function (Constructor, protoProps, staticProps) { if (protoProps) defineProperties(Constructor.prototype, protoProps); if (staticProps) defineProperties(Constructor, staticProps); return Constructor; }; }();
function _classCallCheck(instance, Constructor) { if (!(instance instanceof Constructor)) { throw new TypeError("Cannot call a class as a function"); } }
function _possibleConstructorReturn(self, call) { if (!self) { throw new ReferenceError("this hasn't been initialised - super() hasn't been called"); } return call && (typeof call === "object" || typeof call === "function") ? call : self; }
function _inherits(subClass, superClass) { if (typeof superClass !== "function" && superClass !== null) { throw new TypeError("Super expression must either be null or a function, not " + typeof superClass); } subClass.prototype = Object.create(superClass && superClass.prototype, { constructor: { value: subClass, enumerable: false, writable: true, configurable: true } }); if (superClass) Object.setPrototypeOf ? Object.setPrototypeOf(subClass, superClass) : subClass.__proto__ = superClass; }
// https://www.dexterindustries.com/GoPiGo/
// https://github.com/DexterInd/DI_Sensors
//
// Copyright (c) 2017 Dexter Industries
// Released under the MIT license (http://choosealicense.com/licenses/mit/).
// For more information see https://github.com/DexterInd/GoPiGo3/blob/master/LICENSE.md
var Sensor = require('./base/sensor');
var BNO055 = function (_Sensor) {
_inherits(BNO055, _Sensor);
// Power modes
// Radius registers
// Gyroscope Offset registers
// Magnetometer Offset registers
// Accelerometer Offset registers
// SIC registers
// Unit selection register
// Status registers
// PAGE0 REGISTER DEFINITION START
// I2C addresses
function BNO055() {
var bus = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : 'RPI_1';
var address = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : BNO055.ADDRESS_A;
var mode = arguments.length > 2 && arguments[2] !== undefined ? arguments[2] : BNO055.OPERATION_MODE_NDOF;
var units = arguments.length > 3 && arguments[3] !== undefined ? arguments[3] : 0;
_classCallCheck(this, BNO055);
var _this = _possibleConstructorReturn(this, (BNO055.__proto__ || Object.getPrototypeOf(BNO055)).call(this, bus, address));
_this._mode = mode;
// Send a thow-away command and ignore any response or I2C errors
// just to make sure the BNO055 is in a good state and ready to accept
// commands (this seems to be necessary after a hard power down).
_this.i2c.writeReg8(BNO055.REG_PAGE_ID, 0);
// switch to config mode
_this._configMode();
_this.i2c.writeReg8(BNO055.REG_PAGE_ID, 0);
// check the chip ID
var bnoId = _this.i2c.readReg8u(BNO055.REG_CHIP_ID);
if (bnoId !== BNO055.ID) {
throw new Error('BNO055 failed to respond');
}
// reset the device using the reset command
_this.i2c.writeReg8(BNO055.REG_SYS_TRIGGER, 0x20);
// wait 650ms after reset for chip to be ready (recommended in datasheet)
_this.i2c.mwait(650);
// set to normal power mode
_this.i2c.writeReg8(BNO055.REG_PWR_MODE, BNO055.POWER_MODE_NORMAL);
// default to internal oscillator
_this.i2c.writeReg8(BNO055.REG_SYS_TRIGGER, 0x00);
// set the unit selection bits
_this.i2c.writeReg8(BNO055.REG_UNIT_SEL, units);
// set temperature source to gyroscope, as it seems to be more accurate.
_this.i2c.writeReg8(BNO055.REG_TEMP_SOURCE, 0x01);
// switch to normal operation mode
_this._operationMode();
return _this;
}
// Operation mode settings
// Axis remap values
// Axis remap registers
// Mode registers
// Temperature data register
// Gravity data registers
// Linear acceleration data registers
// Quaternion data registers
// Euler data registers
// Gyro data registers
// Mag data register
// Accel data register
// Page id register definition
_createClass(BNO055, [{
key: '_configMode',
value: function _configMode() {
this.setMode(BNO055.OPERATION_MODE_CONFIG);
}
}, {
key: '_operationMode',
value: function _operationMode() {
this.setMode(this._mode);
}
}, {
key: 'setMode',
value: function setMode(mode) {
this.i2c.writeReg8(BNO055.REG_OPR_MODE, mode & 0xFF);
// delay for 30ms according to datasheet
this.i2c.mwait(30);
}
}, {
key: 'getRevision',
value: function getRevision() {
// read revision values
var accel = this.i2c.readReg8u(BNO055.REG_ACCEL_REV_ID);
var mag = this.i2c.readReg8u(BNO055.REG_MAG_REV_ID);
var gyro = this.i2c.readReg8u(BNO055.REG_GYRO_REV_ID);
var bl = this.i2c.readReg8u(BNO055.REG_BL_REV_ID);
var swLsb = this.i2c.readReg8u(BNO055.REG_SW_REV_ID_LSB);
var swMsb = this.i2c.readReg8u(BNO055.REG_SW_REV_ID_MSB);
var sw = (swMsb << 8 | swLsb) & 0xFFFF;
return [sw, bl, accel, mag, gyro];
}
}, {
key: 'setExternalCrystal',
value: function setExternalCrystal(externalCrystal) {
this._configMode();
var regVal = externalCrystal ? 0x80 : 0x00;
this.i2c.writeReg8(BNO055.REG_SYS_TRIGGER, regVal);
this._operationMode();
}
}, {
key: 'getSystemStatus',
value: function getSystemStatus() {
var runSelfTest = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : true;
var selfTest = false;
if (runSelfTest) {
this._configMode();
var sysTrigger = this.i2c.readReg8u(BNO055.REG_SYS_TRIGGER);
this.i2c.writeReg8(BNO055.REG_SYS_TRIGGER, sysTrigger | 0x1);
this.i2c.wait(1);
selfTest = this.i2c.readReg8u(BNO055.REG_SELFTEST_RESULT);
this._operationMode();
}
var status = this.i2c.readReg8u(BNO055.REG_SYS_STAT);
var error = this.i2c.readReg8u(BNO055.REG_SYS_ERR);
return [status, selfTest, error];
}
}, {
key: 'getCalibrationStatus',
value: function getCalibrationStatus() {
var calStatus = this.i2c.readReg8u(BNO055.REG_CALIB_STAT);
var sys = calStatus >> 6 & 0x03;
var gyro = calStatus >> 4 & 0x03;
var accel = calStatus >> 2 & 0x03;
var mag = calStatus & 0x03;
return [sys, gyro, accel, mag];
}
}, {
key: 'getCalibration',
value: function getCalibration() {
this._configMode();
var calData = this.i2c.readRegList(BNO055.REG_ACCEL_OFFSET_X_LSB, 22);
this._operationMode();
return calData;
}
}, {
key: 'setCalibration',
value: function setCalibration(data) {
if (typeof data === 'undefined' || data.length !== 22) {
throw new Error('setCalibration Expects a list of 22 bytes of calibration data');
}
this._configMode();
this.i2c.writeRegList(BNO055.REG_ACCEL_OFFSET_X_LSB, data);
this._operationMode();
}
}, {
key: 'getAxisRemap',
value: function getAxisRemap() {
var mapConfig = this.i2c.readReg8u(BNO055.REG_AXIS_MAP_CONFIG);
var z = mapConfig >> 4 & 0x03;
var y = mapConfig >> 2 & 0x03;
var x = mapConfig & 0x03;
var signConfig = this.i2c.readReg8u(BNO055.REG_AXIS_MAP_SIGN);
var xSign = signConfig >> 2 & 0x01;
var ySign = signConfig >> 1 & 0x01;
var zSign = signConfig & 0x01;
return [x, y, z, xSign, ySign, zSign];
}
}, {
key: 'setAxisRemap',
value: function setAxisRemap(x, y, z) {
var xSign = arguments.length > 3 && arguments[3] !== undefined ? arguments[3] : BNO055.AXIS_REMAP_POSITIVE;
var ySign = arguments.length > 4 && arguments[4] !== undefined ? arguments[4] : BNO055.AXIS_REMAP_POSITIVE;
var zSign = arguments.length > 5 && arguments[5] !== undefined ? arguments[5] : BNO055.AXIS_REMAP_POSITIVE;
this._configMode();
var mapConfig = 0x00;
mapConfig |= (z & 0x03) << 4;
mapConfig |= (y & 0x03) << 2;
mapConfig |= x & 0x03;
this.i2c.writeReg8(BNO055.REG_AXIS_MAP_CONFIG, mapConfig);
var signConfig = 0x00;
signConfig |= (xSign & 0x01) << 2;
signConfig |= (ySign & 0x01) << 1;
signConfig |= zSign & 0x01;
this.i2c.writeReg8(BNO055.REG_AXIS_MAP_SIGN, signConfig);
this._operationMode();
}
}, {
key: '_readVector',
value: function _readVector(reg) {
var count = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : 3;
var data = this.i2c.readRegList(reg, count * 2);
var result = [];
for (var i = 0; i < count; i++) {
result[i] = ((data[i * 2 + 1] & 0xFF) << 8 | data[i * 2] & 0xFF) & 0xFFFF;
if (result[i] & 0x8000) {
result[i] -= 0x10000;
}
}
return result;
}
}, {
key: 'readEuler',
value: function readEuler() {
var vect = this._readVector(BNO055.REG_EULER_H_LSB);
return [vect[0] / 16.0, vect[1] / 16.0, vect[2] / 16.0];
}
}, {
key: 'readMagnetometer',
value: function readMagnetometer() {
var vect = this._readVector(BNO055.REG_MAG_DATA_X_LSB);
return [vect[0] / 16.0, vect[1] / 16.0, vect[2] / 16.0];
}
}, {
key: 'readGyroscope',
value: function readGyroscope() {
var vect = this._readVector(BNO055.REG_GYRO_DATA_X_LSB);
return [vect[0] / 16.0, vect[1] / 16.0, vect[2] / 16.0];
}
}, {
key: 'readAccelerometer',
value: function readAccelerometer() {
var vect = this._readVector(BNO055.REG_ACCEL_DATA_X_LSB);
return [vect[0] / 100.0, vect[1] / 100.0, vect[2] / 100.0];
}
}, {
key: 'readLinearAcceleration',
value: function readLinearAcceleration() {
var vect = this._readVector(BNO055.REG_LINEAR_ACCEL_DATA_X_LSB);
return [vect[0] / 100.0, vect[1] / 100.0, vect[2] / 100.0];
}
}, {
key: 'readGravity',
value: function readGravity() {
var vect = this._readVector(BNO055.REG_GRAVITY_DATA_X_LSB);
return [vect[0] / 100.0, vect[1] / 100.0, vect[2] / 100.0];
}
}, {
key: 'readQuaternion',
value: function readQuaternion() {
var vect = this._readVector(BNO055.REG_QUATERNION_DATA_W_LSB, 4);
var scale = 1.0 / (1 << 14);
return [vect[0] * scale, vect[1] * scale, vect[2] * scale, vect[3] * scale];
}
}, {
key: 'readTemp',
value: function readTemp() {
return this.i2c.readReg8s(BNO055.REG_TEMP);
}
}]);
return BNO055;
}(Sensor);
BNO055.ADDRESS_A = 0x28;
BNO055.ADDRESS_B = 0x29;
BNO055.ID = 0xA0;
BNO055.REG_PAGE_ID = 0x07;
BNO055.REG_CHIP_ID = 0x00;
BNO055.REG_ACCEL_REV_ID = 0x01;
BNO055.REG_MAG_REV_ID = 0x02;
BNO055.REG_GYRO_REV_ID = 0x03;
BNO055.REG_SW_REV_ID_LSB = 0x04;
BNO055.REG_SW_REV_ID_MSB = 0x05;
BNO055.REG_BL_REV_ID = 0x06;
BNO055.REG_ACCEL_DATA_X_LSB = 0x08;
BNO055.REG_ACCEL_DATA_X_MSB = 0x09;
BNO055.REG_ACCEL_DATA_Y_LSB = 0x0A;
BNO055.REG_ACCEL_DATA_Y_MSB = 0x0B;
BNO055.REG_ACCEL_DATA_Z_LSB = 0x0C;
BNO055.REG_ACCEL_DATA_Z_MSB = 0x0D;
BNO055.REG_MAG_DATA_X_LSB = 0x0E;
BNO055.REG_MAG_DATA_X_MSB = 0x0F;
BNO055.REG_MAG_DATA_Y_LSB = 0x10;
BNO055.REG_MAG_DATA_Y_MSB = 0x11;
BNO055.REG_MAG_DATA_Z_LSB = 0x12;
BNO055.REG_MAG_DATA_Z_MSB = 0x13;
BNO055.REG_GYRO_DATA_X_LSB = 0x14;
BNO055.REG_GYRO_DATA_X_MSB = 0x15;
BNO055.REG_GYRO_DATA_Y_LSB = 0x16;
BNO055.REG_GYRO_DATA_Y_MSB = 0x17;
BNO055.REG_GYRO_DATA_Z_LSB = 0x18;
BNO055.REG_GYRO_DATA_Z_MSB = 0x19;
BNO055.REG_EULER_H_LSB = 0x1A;
BNO055.REG_EULER_H_MSB = 0x1B;
BNO055.REG_EULER_R_LSB = 0x1C;
BNO055.REG_EULER_R_MSB = 0x1D;
BNO055.REG_EULER_P_LSB = 0x1E;
BNO055.REG_EULER_P_MSB = 0x1F;
BNO055.REG_QUATERNION_DATA_W_LSB = 0x20;
BNO055.REG_QUATERNION_DATA_W_MSB = 0x21;
BNO055.REG_QUATERNION_DATA_X_LSB = 0x22;
BNO055.REG_QUATERNION_DATA_X_MSB = 0x23;
BNO055.REG_QUATERNION_DATA_Y_LSB = 0x24;
BNO055.REG_QUATERNION_DATA_Y_MSB = 0x25;
BNO055.REG_QUATERNION_DATA_Z_LSB = 0x26;
BNO055.REG_QUATERNION_DATA_Z_MSB = 0x27;
BNO055.REG_LINEAR_ACCEL_DATA_X_LSB = 0x28;
BNO055.REG_LINEAR_ACCEL_DATA_X_MSB = 0x29;
BNO055.REG_LINEAR_ACCEL_DATA_Y_LSB = 0x2A;
BNO055.REG_LINEAR_ACCEL_DATA_Y_MSB = 0x2B;
BNO055.REG_LINEAR_ACCEL_DATA_Z_LSB = 0x2C;
BNO055.REG_LINEAR_ACCEL_DATA_Z_MSB = 0x2D;
BNO055.REG_GRAVITY_DATA_X_LSB = 0x2E;
BNO055.REG_GRAVITY_DATA_X_MSB = 0x2F;
BNO055.REG_GRAVITY_DATA_Y_LSB = 0x30;
BNO055.REG_GRAVITY_DATA_Y_MSB = 0x31;
BNO055.REG_GRAVITY_DATA_Z_LSB = 0x32;
BNO055.REG_GRAVITY_DATA_Z_MSB = 0x33;
BNO055.REG_TEMP = 0x34;
BNO055.REG_CALIB_STAT = 0x35;
BNO055.REG_SELFTEST_RESULT = 0x36;
BNO055.REG_INTR_STAT = 0x37;
BNO055.REG_SYS_CLK_STAT = 0x38;
BNO055.REG_SYS_STAT = 0x39;
BNO055.REG_SYS_ERR = 0x3A;
BNO055.REG_UNIT_SEL = 0x3B;
BNO055.UNIT_SEL_ACC = 0x01;
BNO055.UNIT_SEL_GYR = 0x02;
BNO055.UNIT_SEL_EUL = 0x04;
BNO055.UNIT_SEL_TEMP = 0x10;
BNO055.UNIT_SEL_ORI = 0x80;
BNO055.REG_DATA_SELECT = 0x3C;
BNO055.REG_OPR_MODE = 0x3D;
BNO055.REG_PWR_MODE = 0x3E;
BNO055.REG_SYS_TRIGGER = 0x3F;
BNO055.REG_TEMP_SOURCE = 0x40;
BNO055.REG_AXIS_MAP_CONFIG = 0x41;
BNO055.REG_AXIS_MAP_SIGN = 0x42;
BNO055.AXIS_REMAP_X = 0x00;
BNO055.AXIS_REMAP_Y = 0x01;
BNO055.AXIS_REMAP_Z = 0x02;
BNO055.AXIS_REMAP_POSITIVE = 0x00;
BNO055.AXIS_REMAP_NEGATIVE = 0x01;
BNO055.REG_SIC_MATRIX_0_LSB = 0x43;
BNO055.REG_SIC_MATRIX_0_MSB = 0x44;
BNO055.REG_SIC_MATRIX_1_LSB = 0x45;
BNO055.REG_SIC_MATRIX_1_MSB = 0x46;
BNO055.REG_SIC_MATRIX_2_LSB = 0x47;
BNO055.REG_SIC_MATRIX_2_MSB = 0x48;
BNO055.REG_SIC_MATRIX_3_LSB = 0x49;
BNO055.REG_SIC_MATRIX_3_MSB = 0x4A;
BNO055.REG_SIC_MATRIX_4_LSB = 0x4B;
BNO055.REG_SIC_MATRIX_4_MSB = 0x4C;
BNO055.REG_SIC_MATRIX_5_LSB = 0x4D;
BNO055.REG_SIC_MATRIX_5_MSB = 0x4E;
BNO055.REG_SIC_MATRIX_6_LSB = 0x4F;
BNO055.REG_SIC_MATRIX_6_MSB = 0x50;
BNO055.REG_SIC_MATRIX_7_LSB = 0x51;
BNO055.REG_SIC_MATRIX_7_MSB = 0x52;
BNO055.REG_SIC_MATRIX_8_LSB = 0x53;
BNO055.REG_SIC_MATRIX_8_MSB = 0x54;
BNO055.REG_ACCEL_OFFSET_X_LSB = 0x55;
BNO055.REG_ACCEL_OFFSET_X_MSB = 0x56;
BNO055.REG_ACCEL_OFFSET_Y_LSB = 0x57;
BNO055.REG_ACCEL_OFFSET_Y_MSB = 0x58;
BNO055.REG_ACCEL_OFFSET_Z_LSB = 0x59;
BNO055.REG_ACCEL_OFFSET_Z_MSB = 0x5A;
BNO055.REG_MAG_OFFSET_X_LSB = 0x5B;
BNO055.REG_MAG_OFFSET_X_MSB = 0x5C;
BNO055.REG_MAG_OFFSET_Y_LSB = 0x5D;
BNO055.REG_MAG_OFFSET_Y_MSB = 0x5E;
BNO055.REG_MAG_OFFSET_Z_LSB = 0x5F;
BNO055.REG_MAG_OFFSET_Z_MSB = 0x60;
BNO055.REG_GYRO_OFFSET_X_LSB = 0x61;
BNO055.REG_GYRO_OFFSET_X_MSB = 0x62;
BNO055.REG_GYRO_OFFSET_Y_LSB = 0x63;
BNO055.REG_GYRO_OFFSET_Y_MSB = 0x64;
BNO055.REG_GYRO_OFFSET_Z_LSB = 0x65;
BNO055.REG_GYRO_OFFSET_Z_MSB = 0x66;
BNO055.REG_ACCEL_RADIUS_LSB = 0x67;
BNO055.REG_ACCEL_RADIUS_MSB = 0x68;
BNO055.REG_MAG_RADIUS_LSB = 0x69;
BNO055.REG_MAG_RADIUS_MSB = 0x6A;
BNO055.POWER_MODE_NORMAL = 0x00;
BNO055.POWER_MODE_LOWPOWER = 0x01;
BNO055.POWER_MODE_SUSPEND = 0x02;
BNO055.OPERATION_MODE_CONFIG = 0x00;
BNO055.OPERATION_MODE_ACCONLY = 0x01;
BNO055.OPERATION_MODE_MAGONLY = 0x02;
BNO055.OPERATION_MODE_GYRONLY = 0x03;
BNO055.OPERATION_MODE_ACCMAG = 0x04;
BNO055.OPERATION_MODE_ACCGYRO = 0x05;
BNO055.OPERATION_MODE_MAGGYRO = 0x06;
BNO055.OPERATION_MODE_AMG = 0x07;
BNO055.OPERATION_MODE_IMUPLUS = 0x08;
BNO055.OPERATION_MODE_COMPASS = 0x09;
BNO055.OPERATION_MODE_M4G = 0x0A;
BNO055.OPERATION_MODE_NDOF_FMC_OFF = 0x0B;
BNO055.OPERATION_MODE_NDOF = 0x0C;
module.exports = BNO055;