1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package motej;
17
18 import java.io.IOException;
19
20 import javax.bluetooth.L2CAPConnection;
21 import javax.microedition.io.Connector;
22
23 import motej.request.ReportModeRequest;
24
25 import org.slf4j.Logger;
26 import org.slf4j.LoggerFactory;
27
28
29
30
31
32
33 class IncomingThread extends Thread {
34
35 private static final long THREAD_SLEEP = 1l;
36
37 private Logger log = LoggerFactory.getLogger(IncomingThread.class);
38
39 private Mote source;
40
41 private Extension extension;
42
43 private volatile boolean active;
44
45 private L2CAPConnection incoming;
46
47 private IrPoint[] interleavedIrCameraData;
48
49 private int[] interleavedAccelerometerData;
50
51 protected IncomingThread(Mote source, String btaddress)
52 throws IOException, InterruptedException {
53 super("in:" + btaddress);
54 this.source = source;
55
56 String l2cap = "btl2cap://" + btaddress
57 + ":13;authenticate=false;encrypt=false;master=false";
58
59 if (log.isDebugEnabled()) {
60 log.debug("Opening incoming connection: " + l2cap);
61 }
62
63 incoming = (L2CAPConnection) Connector.open(l2cap, Connector.READ, true);
64
65 if (log.isDebugEnabled()) {
66 log.debug("Incoming connection is " + incoming.toString());
67 }
68
69 Thread.sleep(THREAD_SLEEP);
70 active = true;
71 }
72
73 public void disconnect() {
74 active = false;
75 }
76
77 protected void parseAccelerometerData(byte[] bytes) {
78 int x = bytes[4] & 0xff;
79 int y = bytes[5] & 0xff;
80 int z = bytes[6] & 0xff;
81 source.fireAccelerometerEvent(x, y, z);
82 }
83
84 protected void parseBasicIrCameraData(byte[] bytes, int offset) {
85 int x0 = bytes[offset] & 0xff ^ (bytes[offset + 2] & 0x30) << 4;
86 int y0 = bytes[offset + 1] & 0xff ^ (bytes[offset + 2] & 0xc0) << 2;
87 IrPoint p0 = new IrPoint(x0, y0);
88
89 int x1 = bytes[offset + 3] & 0xff ^ (bytes[offset + 2] & 0x03) << 8;
90 int y1 = bytes[offset + 4] & 0xff ^ (bytes[offset + 2] & 0x0c) << 6;
91 IrPoint p1 = new IrPoint(x1, y1);
92
93 int x2 = bytes[offset + 5] & 0xff ^ (bytes[offset + 7] & 0x30) << 4;
94 int y2 = bytes[offset + 6] & 0xff ^ (bytes[offset + 7] & 0xc0) << 2;
95 IrPoint p2 = new IrPoint(x2, y2);
96
97 int x3 = bytes[offset + 8] & 0xff ^ (bytes[offset + 7] & 0x03) << 8;
98 int y3 = bytes[offset + 9] & 0xff ^ (bytes[offset + 7] & 0x0c) << 6;
99 IrPoint p3 = new IrPoint(x3, y3);
100
101 source.fireIrCameraEvent(IrCameraMode.BASIC, p0, p1, p2, p3);
102 }
103
104 protected void parseCoreButtonData(byte[] bytes) {
105 int modifiers = bytes[2] & 0xff ^ (bytes[3] & 0xff) << 8;
106 source.fireCoreButtonEvent(modifiers);
107 }
108
109 protected void parseExtendedIrCameraData(byte[] bytes, int offset) {
110 int x0 = bytes[7] & 0xff ^ (bytes[9] & 0x30) << 4;
111 int y0 = bytes[8] & 0xff ^ (bytes[9] & 0xc0) << 2;
112 int size0 = bytes[9] & 0x0f;
113 IrPoint p0 = new IrPoint(x0, y0, size0);
114
115 int x1 = bytes[10] & 0xff ^ (bytes[12] & 0x30) << 4;
116 int y1 = bytes[11] & 0xff ^ (bytes[12] & 0xc0) << 2;
117 int size1 = bytes[12] & 0x0f;
118 IrPoint p1 = new IrPoint(x1, y1, size1);
119
120 int x2 = bytes[13] & 0xff ^ (bytes[15] & 0x30) << 4;
121 int y2 = bytes[14] & 0xff ^ (bytes[15] & 0xc0) << 2;
122 int size2 = bytes[15] & 0x0f;
123 IrPoint p2 = new IrPoint(x2, y2, size2);
124
125 int x3 = bytes[16] & 0xff ^ (bytes[18] & 0x30) << 4;
126 int y3 = bytes[17] & 0xff ^ (bytes[18] & 0xc0) << 2;
127 int size3 = bytes[18] & 0x0f;
128 IrPoint p3 = new IrPoint(x3, y3, size3);
129
130 source.fireIrCameraEvent(IrCameraMode.EXTENDED, p0, p1, p2, p3);
131 }
132
133 protected void parseExtensionData(byte[] bytes, int offset, int length) {
134 if (extension == null) {
135 return;
136 }
137 byte[] extensionData = new byte[length];
138 System.arraycopy(bytes, offset, extensionData, 0, length);
139 extension.parseExtensionData(extensionData);
140 }
141
142 protected void parseFullIrCameraData(byte[] bytes, int reportMode) {
143 if (interleavedIrCameraData == null) {
144 interleavedIrCameraData = new IrPoint[4];
145 }
146 if (reportMode == ReportModeRequest.DATA_REPORT_0x3e) {
147 int x0 = (bytes[5] & 0xff) ^ ((bytes[7] & 0x30) << 4);
148 int y0 = (bytes[6] & 0xff) ^ ((bytes[7] & 0xc0) << 2);
149 int size0 = bytes[7] & 0x0f;
150 int xmin0 = bytes[8] & 0x7f;
151 int ymin0 = bytes[9] & 0x7f;
152 int xmax0 = bytes[10] & 0x7f;
153 int ymax0 = bytes[11] & 0x7f;
154 int intensity0 = bytes[13] & 0xff;
155 interleavedIrCameraData[0] = new IrPoint(x0, y0, size0, xmin0, ymin0, xmax0, ymax0, intensity0);
156
157 int x1 = (bytes[14] & 0xff) ^ ((bytes[16] & 0x30) << 4);
158 int y1 = (bytes[15] & 0xff) ^ ((bytes[16] & 0xc0) << 2);
159 int size1 = bytes[16] & 0x0f;
160 int xmin1 = bytes[17] & 0x7f;
161 int ymin1 = bytes[18] & 0x7f;
162 int xmax1 = bytes[19] & 0x7f;
163 int ymax1 = bytes[20] & 0x7f;
164 int intensity1 = bytes[22] & 0xff;
165 interleavedIrCameraData[1] = new IrPoint(x1, y1, size1, xmin1, ymin1, xmax1, ymax1, intensity1);
166 }
167
168 if (reportMode == ReportModeRequest.DATA_REPORT_0x3f) {
169 int x2 = (bytes[5] & 0xff) ^ ((bytes[7] & 0x30) << 4);
170 int y2 = (bytes[6] & 0xff) ^ ((bytes[7] & 0xc0) << 2);
171 int size2 = bytes[7] & 0x0f;
172 int xmin2 = bytes[8] & 0x7f;
173 int ymin2 = bytes[9] & 0x7f;
174 int xmax2 = bytes[10] & 0x7f;
175 int ymax2 = bytes[11] & 0x7f;
176 int intensity2 = bytes[13] & 0xff;
177 interleavedIrCameraData[2] = new IrPoint(x2, y2, size2, xmin2, ymin2, xmax2, ymax2, intensity2);
178
179 int x3 = (bytes[14] & 0xff) ^ ((bytes[16] & 0x30) << 4);
180 int y3 = (bytes[15] & 0xff) ^ ((bytes[16] & 0xc0) << 2);
181 int size3 = bytes[16] & 0x0f;
182 int xmin3 = bytes[17] & 0x7f;
183 int ymin3 = bytes[18] & 0x7f;
184 int xmax3 = bytes[19] & 0x7f;
185 int ymax3 = bytes[20] & 0x7f;
186 int intensity3 = bytes[22] & 0xff;
187 interleavedIrCameraData[3] = new IrPoint(x3, y3, size3, xmin3, ymin3, xmax3, ymax3, intensity3);
188 }
189
190 if (interleavedIrCameraData[0] != null &&
191 interleavedIrCameraData[2] != null) {
192 IrPoint p0 = interleavedIrCameraData[0];
193 IrPoint p1 = interleavedIrCameraData[1];
194 IrPoint p2 = interleavedIrCameraData[2];
195 IrPoint p3 = interleavedIrCameraData[3];
196 interleavedIrCameraData = null;
197 source.fireIrCameraEvent(IrCameraMode.FULL, p0, p1, p2, p3);
198 }
199 }
200
201 protected void parseInterleavedAccelerometerData(byte[] bytes, int reportMode) {
202 int x = 0;
203 int y = 0;
204 int z = 0;
205
206 if (reportMode == ReportModeRequest.DATA_REPORT_0x3e) {
207 x = bytes[4] & 0xff;
208 z = ((bytes[3] & 0x60) << 1) ^ ((bytes[2] & 0x60) >> 1);
209 }
210
211 if (reportMode == ReportModeRequest.DATA_REPORT_0x3f) {
212 y = bytes[4] & 0xff;
213 z = ((bytes[3] & 0x60) >> 3) ^ ((bytes[2] & 0x60) >> 5);
214 }
215
216 if (interleavedAccelerometerData == null) {
217 interleavedAccelerometerData = new int[3];
218 interleavedAccelerometerData[0] ^= x;
219 interleavedAccelerometerData[1] ^= y;
220 interleavedAccelerometerData[2] ^= z;
221 } else {
222 x ^= interleavedAccelerometerData[0];
223 y ^= interleavedAccelerometerData[1];
224 z ^= interleavedAccelerometerData[2];
225 interleavedAccelerometerData = null;
226 source.fireAccelerometerEvent(x, y, z);
227 }
228 }
229
230 protected void parseMemoryData(byte[] bytes) {
231 int size = ((bytes[4] >> 4) & 0x0f) + 1;
232 int error = bytes[4] & 0x0f;
233 byte[] address = new byte[] { bytes[5], bytes[6] };
234 byte[] payload = new byte[size];
235
236 System.arraycopy(bytes, 7, payload, 0, size);
237
238 source.fireReadDataEvent(address, payload, error);
239 }
240
241 protected void parseStatusInformation(byte[] bytes) {
242 boolean[] leds = new boolean[] { (bytes[4] & 0x10) == 0x10,
243 (bytes[4] & 0x20) == 0x20, (bytes[4] & 0x40) == 0x40,
244 (bytes[4] & 0x80) == 0x80 };
245 boolean extensionControllerConnected = (bytes[4] & 0x02) == 0x02;
246 boolean speakerEnabled = (bytes[4] & 0x04) == 0x04;
247 boolean continuousReportingEnabled = (bytes[4] & 0x08) == 0x08;
248 byte batteryLevel = bytes[7];
249
250 StatusInformationReport info = new StatusInformationReport(leds, speakerEnabled, continuousReportingEnabled, extensionControllerConnected, batteryLevel);
251 source.fireStatusInformationChangedEvent(info);
252 }
253
254 public void run() {
255 while (active) {
256 try {
257 byte[] buf = new byte[23];
258 incoming.receive(buf);
259
260 if (log.isTraceEnabled()) {
261 StringBuffer sb = new StringBuffer();
262 log.trace("received:");
263 for (int i = 0; i < 23; i++) {
264 String hex = Integer.toHexString(buf[i] & 0xff);
265 sb.append(hex.length() == 1 ? "0x0" : "0x").append(hex).append(" ");
266 if ((i + 1) % 8 == 0) {
267 log.trace(sb.toString());
268 sb.delete(0, sb.length());
269 }
270 }
271 if (sb.length() > 0) {
272 log.trace(sb.toString());
273 }
274 }
275
276 switch (buf[1]) {
277 case ReportModeRequest.DATA_REPORT_0x20:
278 parseStatusInformation(buf);
279 break;
280
281 case ReportModeRequest.DATA_REPORT_0x21:
282 parseCoreButtonData(buf);
283 parseMemoryData(buf);
284 break;
285
286 case ReportModeRequest.DATA_REPORT_0x30:
287 parseCoreButtonData(buf);
288 break;
289
290 case ReportModeRequest.DATA_REPORT_0x31:
291 parseCoreButtonData(buf);
292 parseAccelerometerData(buf);
293 break;
294
295 case ReportModeRequest.DATA_REPORT_0x32:
296 parseCoreButtonData(buf);
297 parseExtensionData(buf, 4, 8);
298 break;
299
300 case ReportModeRequest.DATA_REPORT_0x33:
301 parseCoreButtonData(buf);
302 parseAccelerometerData(buf);
303 parseExtendedIrCameraData(buf, 7);
304 break;
305
306 case ReportModeRequest.DATA_REPORT_0x34:
307 parseCoreButtonData(buf);
308 parseExtensionData(buf, 4, 19);
309 break;
310
311 case ReportModeRequest.DATA_REPORT_0x35:
312 parseCoreButtonData(buf);
313 parseAccelerometerData(buf);
314 parseExtensionData(buf, 7, 16);
315 break;
316
317 case ReportModeRequest.DATA_REPORT_0x36:
318 parseCoreButtonData(buf);
319 parseBasicIrCameraData(buf, 4);
320 parseExtensionData(buf, 14, 9);
321 break;
322
323 case ReportModeRequest.DATA_REPORT_0x37:
324 parseCoreButtonData(buf);
325 parseAccelerometerData(buf);
326 parseBasicIrCameraData(buf, 7);
327 parseExtensionData(buf, 17, 6);
328 break;
329
330 case ReportModeRequest.DATA_REPORT_0x3d:
331 parseExtensionData(buf, 2, 21);
332 break;
333
334 case ReportModeRequest.DATA_REPORT_0x3e:
335 parseCoreButtonData(buf);
336 parseInterleavedAccelerometerData(buf, ReportModeRequest.DATA_REPORT_0x3e);
337 parseFullIrCameraData(buf, ReportModeRequest.DATA_REPORT_0x3e);
338 break;
339
340 case ReportModeRequest.DATA_REPORT_0x3f:
341 parseCoreButtonData(buf);
342 parseInterleavedAccelerometerData(buf, ReportModeRequest.DATA_REPORT_0x3f);
343 parseFullIrCameraData(buf, ReportModeRequest.DATA_REPORT_0x3f);
344 break;
345
346 default:
347 if (log.isDebugEnabled()) {
348 String hex = Integer.toHexString(buf[1] & 0xff);
349 log.debug("Unknown or not yet implemented data report: " + (hex.length() == 1 ? "0x0" + hex : "0x" + hex));
350 }
351 }
352
353 Thread.sleep(THREAD_SLEEP);
354 } catch (InterruptedException ex) {
355 log.error("incoming wiimote thread interrupted.", ex);
356 } catch (IOException ex) {
357 log.error("connection closed?", ex);
358 active = false;
359
360
361 source.fireMoteDisconnectedEvent();
362 }
363 }
364 try {
365 incoming.close();
366 } catch (IOException ex) {
367 log.error(ex.getMessage(), ex);
368 }
369 }
370
371 public void setExtension(Extension extension) {
372 this.extension = extension;
373 }
374 }