diff --git a/modules/drivers/mpu6050/mpu6050.js b/modules/drivers/mpu6050/mpu6050.js index df08327658..51f148994f 100644 --- a/modules/drivers/mpu6050/mpu6050.js +++ b/modules/drivers/mpu6050/mpu6050.js @@ -27,6 +27,10 @@ import SMBus from "pins/smbus"; import Timer from "timer"; const REGISTERS = { + I2C_MST_CNTRL: 0x24, + I2C_SLV0_ADDR: 0x25, + I2C_SLV0_REG: 0x26, + I2C_SLV0_CTRL: 0x27, INT_BYPASS: 0x37, ACCEL_XOUT: 0x3B, //big endian ACCEL_YOUT: 0x3D, @@ -35,6 +39,9 @@ const REGISTERS = { GYRO_XOUT: 0x43, GYRO_YOUT: 0x45, GYRO_ZOUT: 0x47, + EXT_SENS_DATA_00: 0x49, + I2C_SLV0_DO: 0x63, + USER_CNTRL: 0x6A, PWR_MGMT_1: 0x6B, PWR_MGMT_2: 0x6C, WHO_AM_I: 0x75 @@ -44,6 +51,9 @@ Object.freeze(REGISTERS); const EXPECTED_WHO_AM_I = 0x68; const GYRO_SCALER = (1 / 131); //Datasheet Section 6.1 const ACCEL_SCALER = (1 / 16384); //Datasheet Section 6.2 +const I2C_MST_EN = 0x20; +const I2C_MST_CLK = 0x0D; +const I2C_SLV0_EN = 0x80; class SMBHold extends SMBus { //SMBus implementation that holds the i2c bus between the i2c.read and i2c.write on read operations. constructor(dictionary) { @@ -94,7 +104,9 @@ class Gyro_Accelerometer extends SMBHold { this.writeByte(REGISTERS.PWR_MGMT_1, 0b10000000); Timer.delay(150); this.writeByte(REGISTERS.PWR_MGMT_1, 0); - this.writeByte(REGISTERS.INT_BYPASS, 0b00000010); + this.writeByte(REGISTERS.INT_BYPASS, 0); + this.writeByte(REGISTERS.USER_CNTRL, I2C_MST_EN); + this.writeByte(REGISTERS.I2C_MST_CNTRL, I2C_MST_CLK); Timer.delay(150); } @@ -127,6 +139,84 @@ class Gyro_Accelerometer extends SMBHold { throw ("Invalid operation for MPU-6050."); } } + + /** + * Access to I2C Master Bus for the MPU for accessing connected slave devices + * Usage is similar to regular SMBus + * (using bypass will cause problems for devices not implementing SMBus) + */ + get I2CMasterBus() { + let that = this; + return class { + constructor(dictionary) { + this.config = dictionary; + } + + calcLength(value) { + let len = 0; + for(let i in value) { + switch(typeof(value[i])) { + case "number": + len++; + break; + case "string": + len += value[i].length; + break; + default: + if(value[i] instanceof(ArrayBuffer)) + len += value[i].byteLength; + else if(value[i].length !== undefined) + len += value[i].length; + else + throw "MPU I2CMasterBus unsupported type"; + } + } + + return len; + } + + readBlock(register, count, buffer) { + if(count > 24) + throw "MPU I2CMasterBus readBlock maximum length is 24 bytes"; + that.writeByte(REGISTERS.I2C_SLV0_ADDR, this.config.address | 0x80); + that.writeByte(REGISTERS.I2C_SLV0_REG, register); + that.writeByte(REGISTERS.I2C_SLV0_CTRL, I2C_SLV0_EN | count); + Timer.delay(1); + return that.readBlock(REGISTERS.EXT_SENS_DATA_00, count, buffer); + } + + writeBlock(register, ...value) { + let count = this.calcLength(value); + if(count > 4) + throw "MPU I2CMasterBus writeBlock maximum length is 4 bytes"; + that.writeByte(REGISTERS.I2C_SLV0_ADDR, this.config.address); + that.writeByte(REGISTERS.I2C_SLV0_REG, register); + that.writeBlock(REGISTERS.I2C_SLV0_DO, ...value); + return that.writeByte(REGISTERS.I2C_SLV0_CTRL, I2C_SLV0_EN | count); + } + + writeByte(register, value) { + return this.writeBlock(register, value & 0xFF); + } + + writeWord(register, value, endian) { + if (endian) + return this.write(register, (value >> 8) & 255, value & 255); + else + return this.write(register, value & 255, (value >> 8) & 255); + } + + readByte(register) { + return this.readBlock(register, 1)[0]; + } + + readWord(register, endian) { + let value = this.readBlock(register, 2); + return endian ? (value[1] | (value[0] << 8)) : (value[0] | (value[1] << 8)); + } + }; + } + } Object.freeze(Gyro_Accelerometer.prototype);