1 | var Emitter = require("events").EventEmitter;
|
2 | var util = require("util");
|
3 |
|
4 | var Board = require("./board");
|
5 |
|
6 | var priv = new Map();
|
7 |
|
8 | var Controllers = {
|
9 |
|
10 | BNO055: {
|
11 | initialize: {
|
12 | value: function(opts, dataHandler) {
|
13 | var IMU = require("./imu"),
|
14 | driver = IMU.Drivers.get(this.board, "BNO055", opts);
|
15 |
|
16 | driver.on("data", function(data) {
|
17 | dataHandler(data);
|
18 | });
|
19 | }
|
20 | },
|
21 | toScaledEuler: {
|
22 | value: function(raw) {
|
23 |
|
24 | return {
|
25 | heading: raw.euler.heading / 16,
|
26 | roll: raw.euler.roll / 16,
|
27 | pitch: raw.euler.pitch / 16,
|
28 | };
|
29 | }
|
30 | },
|
31 | toScaledQuarternion: {
|
32 | value: function(raw) {
|
33 | return {
|
34 | w: raw.quarternion.w * (1 / (1 << 14)),
|
35 | x: raw.quarternion.x * (1 / (1 << 14)),
|
36 | y: raw.quarternion.y * (1 / (1 << 14)),
|
37 | z: raw.quarternion.z * (1 / (1 << 14)),
|
38 | };
|
39 | }
|
40 | },
|
41 | calibration: {
|
42 | get: function() {
|
43 | return priv.get(this).calibration;
|
44 | }
|
45 | },
|
46 | isCalibrated: {
|
47 | get: function() {
|
48 |
|
49 | return ((this.calibration >> 6) & 0x03) === 0x03;
|
50 | }
|
51 | }
|
52 | },
|
53 | };
|
54 |
|
55 |
|
56 |
|
57 |
|
58 |
|
59 |
|
60 |
|
61 |
|
62 |
|
63 |
|
64 |
|
65 |
|
66 |
|
67 |
|
68 |
|
69 |
|
70 |
|
71 |
|
72 |
|
73 |
|
74 |
|
75 |
|
76 |
|
77 | function Orientation(opts) {
|
78 |
|
79 | if (!(this instanceof Orientation)) {
|
80 | return new Orientation(opts);
|
81 | }
|
82 |
|
83 | Board.Component.call(
|
84 | this, opts = Board.Options(opts)
|
85 | );
|
86 |
|
87 | var freq = opts.freq || 25;
|
88 | var controller = null;
|
89 | var raw = null;
|
90 | var state = {
|
91 | euler: {
|
92 | heading: 0,
|
93 | roll: 0,
|
94 | pitch: 0,
|
95 | },
|
96 | quarternion: {
|
97 | w: 0,
|
98 | x: 0,
|
99 | y: 0,
|
100 | z: 0,
|
101 | },
|
102 | calibration: 0,
|
103 | };
|
104 |
|
105 | if (opts.controller && typeof opts.controller === "string") {
|
106 | controller = Controllers[opts.controller.toUpperCase()];
|
107 | } else {
|
108 | controller = opts.controller;
|
109 | }
|
110 |
|
111 | if (controller === null || typeof controller !== "object") {
|
112 | throw new Error("Missing valid Orientation controller");
|
113 | }
|
114 |
|
115 | Board.Controller.call(this, controller, opts);
|
116 |
|
117 |
|
118 | if (!this.toScaledQuarternion) {
|
119 | this.toScaledQuarternion = opts.toScaledQuarternion || function(raw) {
|
120 | return raw;
|
121 | };
|
122 | }
|
123 |
|
124 |
|
125 | if (!this.toScaledEuler) {
|
126 | this.toScaledEuler = opts.toScaledEuler || function(raw) {
|
127 | return raw;
|
128 | };
|
129 | }
|
130 |
|
131 | priv.set(this, state);
|
132 |
|
133 |
|
134 | if (typeof this.initialize === "function") {
|
135 | this.initialize(opts, function(data) {
|
136 | raw = data;
|
137 | });
|
138 | }
|
139 |
|
140 | setInterval(function() {
|
141 | if (raw === null) {
|
142 | return;
|
143 | }
|
144 | var didOrientationChange = false;
|
145 | var didCalibrationChange = false;
|
146 |
|
147 | ["heading", "roll", "pitch"].forEach(function(el) {
|
148 |
|
149 | if (state.euler[el] !== raw.orientation.euler[el]) {
|
150 | didOrientationChange = true;
|
151 | }
|
152 | state.euler[el] = raw.orientation.euler[el];
|
153 | });
|
154 |
|
155 | ["w", "x", "y", "z"].forEach(function(el) {
|
156 |
|
157 | if (state.quarternion[el] !== raw.orientation.quarternion[el]) {
|
158 | didOrientationChange = true;
|
159 | }
|
160 | state.quarternion[el] = raw.orientation.quarternion[el];
|
161 | });
|
162 |
|
163 |
|
164 |
|
165 | if (raw.calibration) {
|
166 |
|
167 | if (state.calibration !== raw.calibration) {
|
168 | didCalibrationChange = true;
|
169 | }
|
170 | state.calibration = raw.calibration;
|
171 | }
|
172 |
|
173 | var data = {
|
174 | euler: this.euler,
|
175 | quarternion: this.quarternion,
|
176 | calibration: this.calibration
|
177 | };
|
178 |
|
179 | this.emit("data", data);
|
180 |
|
181 | if (didOrientationChange) {
|
182 | this.emit("change", data);
|
183 | }
|
184 |
|
185 |
|
186 | if (didCalibrationChange) {
|
187 | this.emit("calibration", this.calibration);
|
188 | }
|
189 | }.bind(this), freq);
|
190 | }
|
191 |
|
192 |
|
193 | util.inherits(Orientation, Emitter);
|
194 |
|
195 | Object.defineProperties(Orientation.prototype, {
|
196 | euler: {
|
197 | get: function() {
|
198 | var state = priv.get(this);
|
199 | return this.toScaledEuler(state);
|
200 | }
|
201 | },
|
202 | quarternion: {
|
203 | get: function() {
|
204 | var state = priv.get(this);
|
205 | return this.toScaledQuarternion(state);
|
206 | }
|
207 | }
|
208 | });
|
209 |
|
210 |
|
211 | if (!!process.env.IS_TEST_MODE) {
|
212 | Orientation.Controllers = Controllers;
|
213 | Orientation.purge = function() {
|
214 | priv.clear();
|
215 | };
|
216 | }
|
217 |
|
218 |
|
219 | module.exports = Orientation;
|