johnny-five
Version:
The JavaScript Robotics and Hardware Programming Framework. Use with: Arduino (all models), Electric Imp, Beagle Bone, Intel Galileo & Edison, Linino One, Pinoccio, pcDuino3, Raspberry Pi, Particle/Spark Core & Photon, Tessel 2, TI Launchpad and more!
909 lines (779 loc) • 21.4 kB
JavaScript
const Board = require("./board");
const Emitter = require("./mixins/emitter");
const {int16, RAD_TO_DEG, TAU} = require("./fn");
const priv = new Map();
const Controllers = {
HMC5883L: {
REGISTER: {
value: {
// Page 11
// Table 2: Register List
//
// Configuration Register A
CRA: 0x00,
// Configuration Register B
// This may change, depending on gauss
CRB: 0x01,
// Mode Register
MODE: 0x02,
// Data Output X MSB Register
READ: 0x03,
}
},
initialize: {
value(opts, dataHandler) {
const state = priv.get(this);
const address = opts.address || 0x1E;
const READLENGTH = 6;
state.scale = 1;
Object.assign(state, new Compass.Scale(opts.gauss || 0.88));
opts.address = address;
this.io.i2cConfig(opts);
// Page 18
// OPERATIONAL EXAMPLES...
//
// 1. Write CRA (00) – send 0x3C 0x00 0x70 (8-average, 15 Hz default, normal measurement)
//
// Set CRA
// Page 12
this.io.i2cWrite(address, this.REGISTER.CRA, 0x70);
// Set CRB
// Page 13
this.io.i2cWrite(address, this.REGISTER.CRB, 0x40);
// Page 14
// Measurement: Continuous
this.io.i2cWrite(address, this.REGISTER.MODE, 0x00);
this.io.i2cRead(address, this.REGISTER.READ, READLENGTH, bytes => {
dataHandler({
x: int16(bytes[0], bytes[1]),
y: int16(bytes[4], bytes[5]),
z: int16(bytes[2], bytes[3]),
});
});
}
},
toScaledHeading: {
value({x, y}) {
const state = priv.get(this);
return ToHeading(x * state.scale, y * state.scale);
}
}
},
/**
* HMC6352: 2-Axis Compass Module
* 0x42
*
* http://bildr.org/2011/01/hmc6352/
*/
HMC6352: {
REGISTER: {
value: {
READ: 0x41
}
},
initialize: {
value(opts, dataHandler) {
const state = priv.get(this);
const address = opts.address || 0x21;
const READLENGTH = 2;
state.scale = 1;
opts.delay = 10;
opts.address = address;
this.io.i2cConfig(opts);
this.io.i2cWrite(address, this.REGISTER.READ);
// Initialize continuous read
this.io.i2cRead(address, this.REGISTER.READ, READLENGTH, bytes => {
dataHandler({
x: (((bytes[0] << 8) + bytes[1]) / 10) | 0,
y: null,
z: null,
});
});
}
},
toScaledHeading: {
value({x}) {
const state = priv.get(this);
return x * state.scale;
},
},
},
BNO055: {
initialize: {
value(opts, dataHandler) {
const IMU = require("./sip");
const driver = IMU.Drivers.get(this.board, "BNO055", opts);
const state = priv.get(this);
// AF p.32, Table 3-19: Magnetometer Unit settings
state.sensitivity = 16;
driver.on("data", ({magnetometer}) => {
dataHandler(magnetometer);
});
}
},
toScaledHeading: {
value(raw) {
const state = priv.get(this);
const x = raw.x / state.sensitivity;
const y = raw.y / state.sensitivity;
return ToHeading(x, y);
},
},
},
// http://www.nxp.com/files/sensors/doc/data_sheet/MAG3110.pdf
MAG3110: {
REGISTER: {
value: {
// Page 15
// Table 11 Register Address Map
// DR_STATUS
STATUS: 0x00,
// OUT_X_MSB
READ: 0x01,
// OFF_X_MSB
OFFSETS: 0x09,
// CTRL_REG1
CTRL_REG1: 0x10,
// CTRL_REG2
CTRL_REG2: 0x11,
}
},
initialize: {
value(opts, dataHandler) {
const state = priv.get(this);
// MAG3110 has only one possible address
const address = 0x0E;
let isDataPending = false;
let temp;
state.isCalibrated = false;
state.isPreCalibrated = false;
state.hasEmittedCalibration = false;
state.measurements = 20;
state.offsets = {
x: 0,
y: 0,
z: 0,
};
state.accum = {
x: { offset: null, high: 0, low: 0 },
y: { offset: null, high: 0, low: 0 },
z: { offset: null, high: 0, low: 0 },
};
opts.delay = 2;
opts.address = address;
if (opts.offsets) {
state.isCalibrated = true;
state.isPreCalibrated = true;
if (Array.isArray(opts.offsets)) {
temp = opts.offsets.slice();
opts.offsets = {
x: temp[0],
y: temp[1],
z: temp[2],
};
}
state.accum.x.low = opts.offsets.x[0];
state.accum.x.high = opts.offsets.x[1];
state.accum.x.offset = (state.accum.x.low + state.accum.x.high) / 2;
state.accum.y.low = opts.offsets.y[0];
state.accum.y.high = opts.offsets.y[1];
state.accum.y.offset = (state.accum.y.low + state.accum.y.high) / 2;
state.accum.z.low = opts.offsets.z[0];
state.accum.z.high = opts.offsets.z[1];
state.accum.z.offset = (state.accum.z.low + state.accum.z.high) / 2;
}
/*
Page 14
4.2.7 MAG3110 Setup Examples
Continuous measurements with ODR = 80 Hz, OSR = 1
1. Enable automatic magnetic sensor resets by setting bit AUTO_MRST_EN in CTRL_REG2.
(CTRL_REG2 = 0x80)
2. Put MAG3110 in active mode 80 Hz ODR with OSR = 1 by writing 0x01 to CTRL_REG1
(CTRL_REG1 = 0x01)
3. At this point it is possible to sync with MAG3110 utilizing INT1 pin or
using polling of the DR_STATUS register as explained in section 4.2.5.
*/
this.io.i2cConfig(opts);
/*
Page 21
5.5.2 CTRL_REG2 (0x11)
Table 33.
CTRL_REG2 Register
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
| A | | R | M | | | | |
A: Automatic Magnetic Sensor Reset. Default value: 0.
R: Data output correction. Default value: 0.
M: Magnetic Sensor Reset (One-Shot). Default value: 0.
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
| 1 | | 0 | 0 | | | | |
0b10000000 = 128 = 0x80
RAW
0b10100000 = 160 = 0xA0
*/
this.io.i2cWrite(address, this.REGISTER.CTRL_REG2, 0x80);
// this.io.i2cWrite(address, this.REGISTER.CTRL_REG2, 0xA0);
/*
Page 20
5.5.1 CTRL_REG1 (0x10)
Table 30. CTRL_REG1 Register
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
|DR2|DR1|DR0|OS1|OS0|FR |TM |AC |
See Table 31. CTRL_REG1 Description for complete descriptions
(Active mode, 80Hz)
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
| 0 | 0 | 0 | 0 | 0 | 0 | 0 | 1 |
0b00000001 = 1 = 0x01
*/
this.io.i2cWrite(address, this.REGISTER.CTRL_REG1, 0x01);
const measured = {
x: 0,
y: 0,
z: 0,
};
const readCycle = () => {
this.io.i2cReadOnce(address, this.REGISTER.STATUS, 1, data => {
/*
Page 16
5.1.1 DR_STATUS (0x00)
Table 12
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
|OVR|ZOW|XOW|YOW|DR |ZDR|YDR|XDR|
Table 13
(Contains Complete descriptions)
OVR (ZYXOW) (X, Y, Z-axis Data Overwrite. Default value: 0.)
0: No Data overwritten
1: Previous X, Y, Z has been overwritten
ZOW, YOW, XOW:
0: No Data overwritten
1: Previous X, Y, Z has been overwritten
DR (ZYXDR) (X or Y or Z-axis new Data Ready. Default value: 0.)
0: No new data is ready
1: New full set of data is ready
ZDR, YDR, XDR:
0: No new data is ready
1: New X, Y, Z data is ready
| 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|---|---|---|---|---|---|---|---|
| 0 | 0 | 0 | 0 | 1 | 1 | 1 | 1 |
0b00001111 = 15 = 0x0F: A complete set of axis data is available
0b11111111 = 255 = 0xFF: All data is newly written
*/
if (!isDataPending && (data[0] === 0x0F || data[0] === 0xFF)) {
isDataPending = true;
this.io.i2cReadOnce(address, this.REGISTER.READ, 6, bytes => {
let timeout = 0;
isDataPending = false;
measured.x = int16(bytes[0], bytes[1]);
measured.y = int16(bytes[2], bytes[3]);
measured.z = int16(bytes[4], bytes[5]);
if (!state.isCalibrated) {
if (state.accum.x.offset === null) {
state.accum.x.offset = measured.x;
state.accum.x.low = measured.x;
state.accum.x.high = measured.x;
}
if (state.accum.y.offset === null) {
state.accum.y.offset = measured.y;
state.accum.y.low = measured.y;
state.accum.y.high = measured.y;
}
state.accum.x.low = Math.min(state.accum.x.low, measured.x);
state.accum.x.high = Math.max(state.accum.x.high, measured.x);
state.accum.x.offset = Math.trunc((state.accum.x.low + state.accum.x.high) / 2);
state.accum.y.low = Math.min(state.accum.y.low, measured.y);
state.accum.y.high = Math.max(state.accum.y.high, measured.y);
state.accum.y.offset = Math.trunc((state.accum.y.low + state.accum.y.high) / 2);
state.accum.z.low = Math.min(state.accum.z.low, measured.z);
state.accum.z.high = Math.max(state.accum.z.high, measured.z);
state.accum.z.offset = Math.trunc((state.accum.z.low + state.accum.z.high) / 2);
--state.measurements;
if (!state.measurements) {
state.isCalibrated = true;
}
}
if (state.isCalibrated) {
if (!state.hasEmittedCalibration) {
state.hasEmittedCalibration = true;
state.offsets.x = state.accum.x.offset;
state.offsets.y = state.accum.y.offset;
state.offsets.z = state.accum.z.offset;
this.io.i2cWrite(address, this.REGISTER.OFFSETS, [
state.offsets.x >> 7, (state.offsets.x << 1) & 0xFF,
state.offsets.y >> 7, (state.offsets.y << 1) & 0xFF,
state.offsets.z >> 7, (state.offsets.z << 1) & 0xFF,
]);
this.emit("calibrated", {
x: [state.accum.x.low, state.accum.x.high],
y: [state.accum.y.low, state.accum.y.high],
z: [state.accum.z.low, state.accum.z.high],
});
}
timeout = Math.floor(1000 / 80);
dataHandler(measured);
}
// MAG3110 is set to read at 80Hz (do this after calibration)
setTimeout(readCycle, timeout);
});
} else {
readCycle();
}
});
};
readCycle();
}
},
calibrate: {
value(measurements) {
const state = priv.get(this);
state.isCalibrated = false;
state.measurements = measurements;
}
},
toScaledHeading: {
value({y, x}) {
const state = priv.get(this);
const scale = {
x: 1 / (state.accum.x.high - state.accum.x.low),
y: 1 / (state.accum.y.high - state.accum.y.low),
};
let heading = Math.atan2(-y * scale.y, x * scale.x);
if (heading < 0) {
heading += TAU;
}
return Math.trunc(heading * RAD_TO_DEG);
},
},
},
/**
* LSM303C: 6Dof 3-Axis Magnetometer & Accelerometer
*
* https://learn.sparkfun.com/tutorials/lsm303c-6dof-hookup-guide
* https://github.com/sparkfun/LSM303C_6_DOF_IMU_Breakout
*/
LSM303C: {
initialize: {
value(opts, dataHandler) {
const IMU = require("./sip");
const driver = IMU.Drivers.get(this.board, "LSM303C", opts);
driver.on("data", ({magnetometer}) => {
dataHandler(magnetometer);
});
}
},
toScaledHeading: {
value({x, y}) {
return ToHeading(x, y);
},
},
},
};
/**
* Compass
* @constructor
*
* five.Compass();
*
* five.Compass({
* controller: "HMC5883L",
* freq: 50,
* });
*
*
* Device Shorthands:
*
* "HMC5883L": new five.Magnetometer()
*
*
* @param {Object} opts [description]
*
*/
class Compass extends Emitter {
constructor(opts) {
super();
Board.Component.call(
this, opts = Board.Options(opts)
);
const freq = opts.freq || 25;
let raw = {
x: null,
y: null,
z: null,
};
const state = {
x: 0,
y: 0,
z: 0,
scale: 0,
register: 0,
heading: 0
};
Board.Controller.call(this, Controllers, opts);
if (!this.toScaledHeading) {
this.toScaledHeading = opts.toScaledHeading || (raw => raw);
}
priv.set(this, state);
if (typeof this.initialize === "function") {
this.initialize(opts, data => raw = data);
}
setInterval(() => {
if (raw.x === null) {
return;
}
let isChange = false;
state.x = raw.x;
state.y = raw.y;
state.z = raw.z;
const heading = this.heading;
if (heading !== state.heading) {
state.heading = heading;
isChange = true;
}
this.emit("data", {heading});
if (isChange) {
this.emit("change", {heading});
}
}, freq);
Object.defineProperties(this, {
/**
* [read-only] Bearing information
* @name bearing
* @property
* @type Object
*
*
name
abbr
low
mid
high
heading
*
*/
bearing: {
get() {
const length = Compass.Points.length;
const heading = this.heading;
let point;
for (let i = 0; i < length; i++) {
point = Compass.Points[i];
if (heading >= point.low && heading <= point.high) {
// Specify fields to return to avoid returning the
// range array (too much noisy data)
return {
name: point.name,
abbr: point.abbr,
low: point.low,
high: point.high,
heading
};
}
}
}
},
/**
* [read-only] Raw X/Y/Z
* @name raw
* @property
* @type Object
*
x
y
z
*/
raw: {
get() {
return {
x: raw.x,
y: raw.y,
z: raw.z
};
}
},
/**
* [read-only] Heading (azimuth)
* @name heading
* @property
* @type number
*/
heading: {
get() {
return this.toScaledHeading(raw);
}
}
});
}
/**
* Compass.scale Set the scale gauss for compass readings
* @param {Number} gauss [description]
* @return {register} [description]
*
* Ported from:
* http://bildr.org/2012/02/hmc5883l_arduino/
*/
}
Compass.Scale = class {
constructor(gauss) {
if (gauss === 0.88) {
this.register = 0x00;
this.scale = 0.73;
} else if (gauss === 1.3) {
this.register = 0x01;
this.scale = 0.92;
} else if (gauss === 1.9) {
this.register = 0x02;
this.scale = 1.22;
} else if (gauss === 2.5) {
this.register = 0x03;
this.scale = 1.52;
} else if (gauss === 4.0) {
this.register = 0x04;
this.scale = 2.27;
} else if (gauss === 4.7) {
this.register = 0x05;
this.scale = 2.56;
} else if (gauss === 5.6) {
this.register = 0x06;
this.scale = 3.03;
} else if (gauss === 8.1) {
this.register = 0x07;
this.scale = 4.35;
} else {
this.register = 0x00;
this.scale = 1;
}
// Setting is in the top 3 bits of the register.
this.register = this.register << 5;
}
};
function ToHeading(x, y) {
/**
*
* Applications of Magnetoresistive Sensors in Navigation Systems
* by Michael J. Caruso of Honeywell Inc.
* http://www.ssec.honeywell.com/position-sensors/datasheets/sae.pdf
*
*
* Azimuth (x=0, y<0) = 90.0 (3)
* Azimuth (x=0, y>0) = 270.0
* Azimuth (x<0) = 180 - [arcTan(y/x)]*180/PI
* Azimuth (x>0, y<0) = - [arcTan(y/x)]*180/PI
* Azimuth (x>0, y>0) = 360 - [arcTan(y/x)]*180/PI
*/
/**
* http://bildr.org/2012/02/hmc5883l_arduino/
* @type {[type]}
* Copyright (C) 2011 Love Electronics (loveelectronics.co.uk)
This program is free software: you can redistribute it and/or modify it under the terms of the version 3 GNU General Public License as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
let radians = Math.atan2(y, x);
if (radians < 0) {
radians += TAU;
}
if (radians > TAU) {
radians -= TAU;
}
return radians * RAD_TO_DEG;
}
/**
* Compass.Points
*
* 32 Point Compass
* +1 for North
*
*/
Compass.Points = [{
name: "North",
abbr: "N",
low: 354.38,
high: 360
}, {
name: "North",
abbr: "N",
low: 0,
high: 5.62
}, {
name: "North by East",
abbr: "NbE",
low: 5.63,
high: 16.87
}, {
name: "North-NorthEast",
abbr: "NNE",
low: 16.88,
high: 28.12
}, {
name: "NorthEast by North",
abbr: "NEbN",
low: 28.13,
high: 39.37
}, {
name: "NorthEast",
abbr: "NE",
low: 39.38,
high: 50.62
}, {
name: "NorthEast by East",
abbr: "NEbE",
low: 50.63,
high: 61.87
}, {
name: "East-NorthEast",
abbr: "ENE",
low: 61.88,
high: 73.12
}, {
name: "East by North",
abbr: "EbN",
low: 73.13,
high: 84.37
}, {
name: "East",
abbr: "E",
low: 84.38,
high: 95.62
}, {
name: "East by South",
abbr: "EbS",
low: 95.63,
high: 106.87
}, {
name: "East-SouthEast",
abbr: "ESE",
low: 106.88,
high: 118.12
}, {
name: "SouthEast by East",
abbr: "SEbE",
low: 118.13,
high: 129.37
}, {
name: "SouthEast",
abbr: "SE",
low: 129.38,
high: 140.62
}, {
name: "SouthEast by South",
abbr: "SEbS",
low: 140.63,
high: 151.87
}, {
name: "South-SouthEast",
abbr: "SSE",
low: 151.88,
high: 163.12
}, {
name: "South by East",
abbr: "SbE",
low: 163.13,
high: 174.37
}, {
name: "South",
abbr: "S",
low: 174.38,
high: 185.62
}, {
name: "South by West",
abbr: "SbW",
low: 185.63,
high: 196.87
}, {
name: "South-SouthWest",
abbr: "SSW",
low: 196.88,
high: 208.12
}, {
name: "SouthWest by South",
abbr: "SWbS",
low: 208.13,
high: 219.37
}, {
name: "SouthWest",
abbr: "SW",
low: 219.38,
high: 230.62
}, {
name: "SouthWest by West",
abbr: "SWbW",
low: 230.63,
high: 241.87
}, {
name: "West-SouthWest",
abbr: "WSW",
low: 241.88,
high: 253.12
}, {
name: "West by South",
abbr: "WbS",
low: 253.13,
high: 264.37
}, {
name: "West",
abbr: "W",
low: 264.38,
high: 275.62
}, {
name: "West by North",
abbr: "WbN",
low: 275.63,
high: 286.87
}, {
name: "West-NorthWest",
abbr: "WNW",
low: 286.88,
high: 298.12
}, {
name: "NorthWest by West",
abbr: "NWbW",
low: 298.13,
high: 309.37
}, {
name: "NorthWest",
abbr: "NW",
low: 309.38,
high: 320.62
}, {
name: "NorthWest by North",
abbr: "NWbN",
low: 320.63,
high: 331.87
}, {
name: "North-NorthWest",
abbr: "NNW",
low: 331.88,
high: 343.12
}, {
name: "North by West",
abbr: "NbW",
low: 343.13,
high: 354.37
}];
Object.freeze(Compass.Points);
/**
* Fires once every N ms, equal to value of `freq`. Defaults to 66ms
*
* @event
* @name read
* @memberOf Compass
*/
/**
* Fires when the calculated heading has changed
*
* @event
* @name headingchange
* @memberOf Compass
*/
/* istanbul ignore else */
if (!!process.env.IS_TEST_MODE) {
Compass.Controllers = Controllers;
Compass.purge = () => {
priv.clear();
};
}
module.exports = Compass;