Skip to content

Instantly share code, notes, and snippets.

@cat-haines

cat-haines/imu.device.nut

Last active August 29, 2015 14:16
Show Gist options
  • Save cat-haines/63a3b895abcfbddf4ee9 to your computer and use it in GitHub Desktop.
Save cat-haines/63a3b895abcfbddf4ee9 to your computer and use it in GitHub Desktop.
Download ZIP
IMU Tail
Raw
imu.device.nut
// Copyright (c) 2014 Electric Imp
// This file is licensed under the MIT License
// http://opensource.org/licenses/MIT
class LSM9DS0TR {
static WHO_AM_I_G = 0x0F;
static CTRL_REG1_G = 0x20;
static CTRL_REG2_G = 0x21;
static CTRL_REG3_G = 0x22;
static CTRL_REG4_G = 0x23;
static CTRL_REG5_G = 0x24;
static REF_DATACAP_G = 0x25;
static STATUS_REG_G = 0x27;
static OUT_X_L_G = 0x28;
static OUT_X_H_G = 0x29;
static OUT_Y_L_G = 0x2A;
static OUT_Y_H_G = 0x2B;
static OUT_Z_L_G = 0x2C;
static OUT_Z_H_G = 0x2D;
static FIFO_CTRL_REG_G = 0x2E;
static FIFO_SRC_REG_G = 0x2F;
static INT1_CFG_G = 0x30;
static INT1_SRC_G = 0x31;
static INT1_THS_XH_G = 0x32;
static INT1_THS_XL_G = 0x33;
static INT1_THS_YH_G = 0x34;
static INT1_THS_YL_G = 0x35;
static INT1_THS_ZH_G = 0x36;
static INT1_THS_ZL_G = 0x37;
static INT1_DURATION_G = 0x38;
static OUT_TEMP_L_XM = 0x05;
static OUT_TEMP_H_XM = 0x06;
static STATUS_REG_M = 0x07;
static OUT_X_L_M = 0x08;
static OUT_X_H_M = 0x09;
static OUT_Y_L_M = 0x0A;
static OUT_Y_H_M = 0x0B;
static OUT_Z_L_M = 0x0C;
static OUT_Z_H_M = 0x0D;
static WHO_AM_I_XM = 0x0F;
static INT_CTRL_REG_M = 0x12;
static INT_SRC_REG_M = 0x13;
static INT_THS_L_M = 0x14;
static INT_THS_H_M = 0x15;
static OFFSET_X_L_M = 0x16;
static OFFSET_X_H_M = 0x17;
static OFFSET_Y_L_M = 0x18;
static OFFSET_Y_H_M = 0x19;
static OFFSET_Z_L_M = 0x1A;
static OFFSET_Z_H_M = 0x1B;
static REFERENCE_X = 0x1C;
static REFERENCE_Y = 0x1D;
static REFERENCE_Z = 0x1E;
static CTRL_REG0_XM = 0x1F;
static CTRL_REG1_XM = 0x20;
static CTRL_REG2_XM = 0x21;
static CTRL_REG3_XM = 0x22;
static CTRL_REG4_XM = 0x23;
static CTRL_REG5_XM = 0x24;
static CTRL_REG6_XM = 0x25;
static CTRL_REG7_XM = 0x26;
static STATUS_REG_A = 0x27;
static OUT_X_L_A = 0x28;
static OUT_X_H_A = 0x29;
static OUT_Y_L_A = 0x2A;
static OUT_Y_H_A = 0x2B;
static OUT_Z_L_A = 0x2C;
static OUT_Z_H_A = 0x2D;
static FIFO_CTRL_REG = 0x2E;
static FIFO_SRC_REG = 0x2F;
static INT_GEN_1_REG = 0x30;
static INT_GEN_1_SRC = 0x31;
static INT_GEN_1_THS = 0x32;
static INT_GEN_1_DURATION = 0x33;
static INT_GEN_2_REG = 0x34;
static INT_GEN_2_SRC = 0x35;
static INT_GEN_2_THS = 0x36;
static INT_GEN_2_DURATION = 0x37;
static CLICK_CFG = 0x38;
static CLICK_SRC = 0x39;
static CLICK_THS = 0x3A;
static TIME_LIMIT = 0x3B;
static TIME_LATENCY = 0x3C;
static TIME_WINDOW = 0x3D;
static Act_THS = 0x3E;
static Act_DUR = 0x3F;
_i2c = null;
_xm_addr = null;
_g_addr = null;
_temp_enabled = null;
// -------------------------------------------------------------------------
// 0x3C = 8-bit I2C Student Address for Accel / Magnetometer
// 0xD4 = 8-bit I2C Student Address for Angular Rate Sensor
constructor(i2c, xm_addr = 0x3C, g_addr = 0xD4) {
_i2c = i2c;
_xm_addr = xm_addr;
_g_addr = g_addr;
_temp_enabled = false;
imp.sleep(0.1);
}
// for testng
function getDeviceId_G() {
return _i2c.read(_g_addr, format("%c",WHO_AM_I_G), 1)[0];
}
// set power state of the gyro device
// note that if individual axes were previously disabled, they still will be
function enableGyro(state) {
_setRegBit(_g_addr, CTRL_REG1_G, 3, state);
}
// Put magnetometer into continuous-conversion mode
// IMU comes up with magnetometer powered down
function enableMag(state) {
local val = _i2c.read(_xm_addr, format("%c",CTRL_REG7_XM), 1)[0] & 0xFC;
if (state == 0) val = val | 0x01;
_i2c.write(_xm_addr, format("%c%c",CTRL_REG7_XM, val));
}
// Enable temperature sensor
function enableTemp(state) {
_setRegBit(_xm_addr, CTRL_REG5_XM, 7, state);
if (state == 0) {
_temp_enabled = false;
} else {
_temp_enabled = true;
}
}
// -------------------------------------------------------------------------
// Set Accelerometer Data Rate in Hz
// IMU comes up with accelerometer disabled; rate must be set to enable
function enableAccel(state) {
local val = _i2c.read(_xm_addr, format("%c",CTRL_REG1_XM), 1)[0] & 0x0F;
if (state == true) val = val | 0x10;
_i2c.write(_xm_addr, format("%c%c",CTRL_REG1_XM, val));
}
// -------------------------------------------------------------------------
// Set Magnetometer Data Rate in Hz
// IMU comes up with magnetometer data rate set to 3.125 Hz
function setMagDataRate(rate) {
local val = _i2c.read(_xm_addr, format("%c",CTRL_REG5_XM), 1)[0] & 0xE3;
if (rate <= 3.125) {
// rate already set
} else if (rate <= 6.25) {
val = val | 0x04;
} else if (rate <= 12.5) {
val = val | 0x08;
} else if (rate <= 25) {
val = val | 0x0C;
} else if (rate <= 50) {
val = val | 0x10;
} else {
// rate = 100 Hz
val = val | 0x14;
}
_i2c.write(_xm_addr, format("%c%c",CTRL_REG5_XM, val));
}
// -------------------------------------------------------------------------
// read the internal temperature sensor (C) in the accelerometer / magnetometer
function readTemp() {
if (!_temp_enabled) { enableTemp(1) };
local temp = (_i2c.read(_xm_addr, format("%c", OUT_TEMP_H_XM), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_TEMP_L_XM), 1)[0];
temp = temp & 0x0fff; // temp data is 12 bits, 2's comp, right-justified
if (temp & 0x0800) {
return (-1.0) * _twosComp(temp, 0x0fff);
} else {
return temp;
}
}
// -------------------------------------------------------------------------
// Read data from the Gyro
// Returns a table {x: <data>, y: <data>, z: <data>}
function readGyro() {
local x_raw = (_i2c.read(_g_addr, format("%c", OUT_X_H_G), 1)[0] << 8) + _i2c.read(_g_addr, format("%c", OUT_X_L_G), 1)[0];
local y_raw = (_i2c.read(_g_addr, format("%c", OUT_Y_H_G), 1)[0] << 8) + _i2c.read(_g_addr, format("%c", OUT_Y_L_G), 1)[0];
local z_raw = (_i2c.read(_g_addr, format("%c", OUT_Z_H_G), 1)[0] << 8) + _i2c.read(_g_addr, format("%c", OUT_Z_L_G), 1)[0];
local result = {};
if (x_raw & 0x8000) {
result.x <- (-1.0) * _twosComp(x_raw, 0xffff);
} else {
result.x <- x_raw;
}
if (y_raw & 0x8000) {
result.y <- (-1.0) * _twosComp(y_raw, 0xffff);
} else {
result.y <- y_raw;
}
if (z_raw & 0x8000) {
result.z <- (-1.0) * _twosComp(z_raw, 0xffff);
} else {
result.z <- z_raw;
}
return result;
}
// Read data from the Magnetometer
// Returns a table {x: <data>, y: <data>, z: <data>}
function readMag() {
local x_raw = (_i2c.read(_xm_addr, format("%c", OUT_X_H_M), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_X_L_M), 1)[0];
local y_raw = (_i2c.read(_xm_addr, format("%c", OUT_Y_H_M), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_Y_L_M), 1)[0];
local z_raw = (_i2c.read(_xm_addr, format("%c", OUT_Z_H_M), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_Z_L_M), 1)[0];
local result = {};
if (x_raw & 0x8000) {
result.x <- (-1.0) * _twosComp(x_raw, 0xffff);
} else {
result.x <- x_raw;
}
if (y_raw & 0x8000) {
result.y <- (-1.0) * _twosComp(y_raw, 0xffff);
} else {
result.y <- y_raw;
}
if (z_raw & 0x8000) {
result.z <- (-1.0) * _twosComp(z_raw, 0xffff);
} else {
result.z <- z_raw;
}
return result;
}
// Read data from the Accelerometer
// Returns a table {x: <data>, y: <data>, z: <data>}
function readAccel() {
local x_raw = (_i2c.read(_xm_addr, format("%c", OUT_X_H_A), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_X_L_A), 1)[0];
local y_raw = (_i2c.read(_xm_addr, format("%c", OUT_Y_H_A), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_Y_L_A), 1)[0];
local z_raw = (_i2c.read(_xm_addr, format("%c", OUT_Z_H_A), 1)[0] << 8) + _i2c.read(_xm_addr, format("%c", OUT_Z_L_A), 1)[0];
//server.log(format("%02X, %02X, %02X",x_raw, y_raw, z_raw));
local result = {};
if (x_raw & 0x8000) {
result.x <- (-1.0) * _twosComp(x_raw, 0xffff);
} else {
result.x <- x_raw;
}
if (y_raw & 0x8000) {
result.y <- (-1.0) * _twosComp(y_raw, 0xffff);
} else {
result.y <- y_raw;
}
if (z_raw & 0x8000) {
result.z <- (-1.0) * _twosComp(z_raw, 0xffff);
} else {
result.z <- z_raw;
}
return result;
}
/******************** PRIVATE FUNCTIONS ********************/
function _twosComp(value, mask) {
value = ~(value & mask) + 1;
return value & mask;
}
function _setRegBit(addr, reg, bit, state) {
local val = _i2c.read(addr, format("%c",reg), 1)[0];
if (state == 0) {
val = val & ~(0x01 << bit);
} else {
val = val | (0x01 << bit);
}
_i2c.write(addr, format("%c%c", reg, val));
}
}
function logloop() {
imp.wakeup(1,logloop);
local acc = imu.readAccel();
local mag = imu.readMag();
local gyr = imu.readGyro();
local temp = imu.readTemp();
server.log(format("Accel: (%0.2f, %0.2f, %0.2f)", acc.x, acc.y, acc.z));
server.log(format("Mag: (%0.2f, %0.2f, %0.2f)", mag.x, mag.y, mag.z));
server.log(format("Gyro: (%0.2f, %0.2f, %0.2f)", gyr.x, gyr.y, gyr.z));
server.log(format("Temp: %d C", temp));
}
/* RUNTIME START ------------------------------------------------------------ */
xm_int1 <- hardware.pin1; // accel / magnetometer interrupt 1
xm_int2 <- hardware.pin2; // accel / magnetometer interrupt 2
g_drdy <- hardware.pin5; // angular rate Data Ready
g_int <- hardware.pin7; // angular rate Interrupt
i2c <- hardware.i2c89;
i2c.configure(CLOCK_SPEED_400_KHZ);
imu <- LSM9DS0TR(i2c);
imu.enableGyro(1); // enable the gyro
imu.enableAccel(1); // enable accel @ 3.125 Hz
imu.enableMag(1); // enable magnometer
logloop();
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment