OLD | NEW |
| (Empty) |
1 // Copyright (c) 2011 The Chromium Authors. All rights reserved. | |
2 // Use of this source code is governed by a BSD-style license that can be | |
3 // found in the LICENSE file. | |
4 // | |
5 // This file is based on the SMSLib library. | |
6 // | |
7 // SMSLib Sudden Motion Sensor Access Library | |
8 // Copyright (c) 2010 Suitable Systems | |
9 // All rights reserved. | |
10 // | |
11 // Developed by: Daniel Griscom | |
12 // Suitable Systems | |
13 // http://www.suitable.com | |
14 // | |
15 // Permission is hereby granted, free of charge, to any person obtaining a | |
16 // copy of this software and associated documentation files (the | |
17 // "Software"), to deal with the Software without restriction, including | |
18 // without limitation the rights to use, copy, modify, merge, publish, | |
19 // distribute, sublicense, and/or sell copies of the Software, and to | |
20 // permit persons to whom the Software is furnished to do so, subject to | |
21 // the following conditions: | |
22 // | |
23 // - Redistributions of source code must retain the above copyright notice, | |
24 // this list of conditions and the following disclaimers. | |
25 // | |
26 // - Redistributions in binary form must reproduce the above copyright | |
27 // notice, this list of conditions and the following disclaimers in the | |
28 // documentation and/or other materials provided with the distribution. | |
29 // | |
30 // - Neither the names of Suitable Systems nor the names of its | |
31 // contributors may be used to endorse or promote products derived from | |
32 // this Software without specific prior written permission. | |
33 // | |
34 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS | |
35 // OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF | |
36 // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. | |
37 // IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR | |
38 // ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, | |
39 // TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE | |
40 // SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE SOFTWARE. | |
41 // | |
42 // For more information about SMSLib, see | |
43 // <http://www.suitable.com/tools/smslib.html> | |
44 // or contact | |
45 // Daniel Griscom | |
46 // Suitable Systems | |
47 // 1 Centre Street, Suite 204 | |
48 // Wakefield, MA 01880 | |
49 // (781) 665-0053 | |
50 | |
51 #include "chrome/browser/device_orientation/accelerometer_mac.h" | |
52 | |
53 #include <math.h> | |
54 #include <sys/sysctl.h> | |
55 | |
56 #include "base/logging.h" | |
57 #include "base/scoped_ptr.h" | |
58 #include "chrome/browser/device_orientation/orientation.h" | |
59 | |
60 namespace device_orientation { | |
61 | |
62 struct AccelerometerMac::GenericMacbookSensor { | |
63 // Name of device to be read. | |
64 const char* service_name; | |
65 | |
66 // Number of bytes of the axis data. | |
67 int axis_size; | |
68 | |
69 // Default calibration value for zero g. | |
70 float zero_g; | |
71 | |
72 // Default calibration value for one g (negative when axis is inverted). | |
73 float one_g; | |
74 | |
75 // Kernel function index. | |
76 unsigned int function; | |
77 | |
78 // Size of the sensor record to be sent/received. | |
79 unsigned int record_size; | |
80 }; | |
81 | |
82 struct AccelerometerMac::AxisData { | |
83 // Location of the first byte representing the axis in the sensor data. | |
84 int index; | |
85 | |
86 // Axis inversion flag. The value changes often between models. | |
87 bool inverted; | |
88 }; | |
89 | |
90 // Sudden Motion Sensor descriptor. | |
91 struct AccelerometerMac::SensorDescriptor { | |
92 // Prefix of model to be tested. | |
93 const char* model_name; | |
94 | |
95 // Axis-specific data (x,y,z order). | |
96 AxisData axis[3]; | |
97 }; | |
98 | |
99 // Typical sensor parameters in MacBook models. | |
100 const AccelerometerMac::GenericMacbookSensor | |
101 AccelerometerMac::kGenericSensor = { | |
102 "SMCMotionSensor", 2, | |
103 0, 251, | |
104 5, 40 | |
105 }; | |
106 | |
107 // Supported sensor descriptors. Add entries here to enhance compatibility. | |
108 // All non-tested entries from SMSLib have been removed. | |
109 const AccelerometerMac::SensorDescriptor | |
110 AccelerometerMac::kSupportedSensors[] = { | |
111 // Tested by tommyw on a 13" MacBook. | |
112 { "MacBook1,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
113 | |
114 // Tested by S.Selz. (via avi) on a 13" MacBook. | |
115 { "MacBook2,1", { { 0, true }, { 2, false }, { 4, true } } }, | |
116 | |
117 // Tested by verhees on a 13" MacBook. | |
118 { "MacBook3,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
119 | |
120 // Tested by adlr on a 13" MacBook. | |
121 { "MacBook4,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
122 | |
123 // Tested by tommyw on a 13" MacBook. | |
124 { "MacBook6,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
125 | |
126 // Tested by avi on a 13" MacBook. | |
127 { "MacBook7,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
128 | |
129 // Tested by crc on a 13" MacBook Air. | |
130 { "MacBookAir1,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
131 | |
132 // Tested by sfiera, pjw on a 13" MacBook Air. | |
133 { "MacBookAir2,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
134 | |
135 // Note: MacBookAir3,1 (11" MacBook Air) and MacBookAir3,2 (13" MacBook Air) | |
136 // have no accelerometer sensors. | |
137 | |
138 // Tested by crc on a 15" MacBook Pro. | |
139 { "MacBookPro1,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
140 | |
141 // Tested by L.V. (via avi) on a 17" MacBook Pro. | |
142 { "MacBookPro2,1", { { 0, true }, { 2, false }, { 4, true } } }, | |
143 | |
144 // Tested by leandrogracia on a 15" MacBook Pro. | |
145 { "MacBookPro2,2", { { 0, true }, { 2, true }, { 4, false } } }, | |
146 | |
147 // Tested by leandrogracia on a 15" MacBook Pro. | |
148 // TODO(avi): this model name was also used for the 17" version; verify that | |
149 // these parameters are also valid for that model. | |
150 { "MacBookPro3,1", { { 0, false }, { 2, true }, { 4, true } } }, | |
151 | |
152 // Tested by leandrogracia on a 15" MacBook Pro. | |
153 // Tested by Eric Shapiro (via avi) on a 17" MacBook Pro. | |
154 { "MacBookPro4,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
155 | |
156 // Tested by leandrogracia on a 15" MacBook Pro. | |
157 { "MacBookPro5,1", { { 0, false }, { 2, false }, { 4, false } } }, | |
158 | |
159 // Tested by S.Selz. (via avi) on a 17" MacBook Pro. | |
160 { "MacBookPro5,2", { { 0, false }, { 2, false }, { 4, false } } }, | |
161 | |
162 // Tested by dmaclach on a 15" MacBook Pro. | |
163 { "MacBookPro5,3", { { 2, false }, { 0, false }, { 4, true } } }, | |
164 | |
165 // Tested by leandrogracia on a 15" MacBook Pro. | |
166 { "MacBookPro5,4", { { 0, false }, { 2, false }, { 4, false } } }, | |
167 | |
168 // Tested by leandrogracia on a 13" MacBook Pro. | |
169 { "MacBookPro5,5", { { 0, true }, { 2, true }, { 4, false } } }, | |
170 | |
171 // Tested by khom, leadpipe on a 17" MacBook Pro. | |
172 { "MacBookPro6,1", { { 0, false }, { 2, false }, { 4, false } } }, | |
173 | |
174 // Tested by leandrogracia on a 15" MacBook Pro. | |
175 { "MacBookPro6,2", { { 0, true }, { 2, false }, { 4, true } } }, | |
176 | |
177 // Tested by leandrogracia on a 13" MacBook Pro. | |
178 { "MacBookPro7,1", { { 0, true }, { 2, true }, { 4, false } } }, | |
179 | |
180 // Generic MacBook accelerometer sensor data, used for for both future models | |
181 // and past models for which there has been no testing. Note that this generic | |
182 // configuration may well have problems with inverted axes. | |
183 // TODO(avi): Find these past models and test on them; test on future models. | |
184 // MacBook5,1 | |
185 // MacBook5,2 | |
186 // MacBookPro1,2 | |
187 // MacBookPro3,1 (17" to compare to 15") | |
188 { "", { { 0, true }, { 2, true }, { 4, false } } } | |
189 }; | |
190 | |
191 // Create a AccelerometerMac object and return NULL if no valid sensor found. | |
192 DataFetcher* AccelerometerMac::Create() { | |
193 scoped_ptr<AccelerometerMac> accelerometer(new AccelerometerMac); | |
194 return accelerometer->Init() ? accelerometer.release() : NULL; | |
195 } | |
196 | |
197 AccelerometerMac::~AccelerometerMac() { | |
198 IOServiceClose(io_connection_); | |
199 } | |
200 | |
201 AccelerometerMac::AccelerometerMac() | |
202 : sensor_(NULL), | |
203 io_connection_(0) { | |
204 } | |
205 | |
206 // Retrieve per-axis accelerometer values. | |
207 // | |
208 // Axes and angles are defined according to the W3C DeviceOrientation Draft. | |
209 // See here: http://dev.w3.org/geo/api/spec-source-orientation.html | |
210 // | |
211 // Note: only beta and gamma angles are provided. Alpha is set to zero. | |
212 // | |
213 // Returns false in case of error or non-properly initialized object. | |
214 // | |
215 bool AccelerometerMac::GetOrientation(Orientation* orientation) { | |
216 DCHECK(sensor_); | |
217 | |
218 // Reset output record memory buffer. | |
219 std::fill(output_record_.begin(), output_record_.end(), 0x00); | |
220 | |
221 // Read record data from memory. | |
222 const size_t kInputSize = kGenericSensor.record_size; | |
223 size_t output_size = kGenericSensor.record_size; | |
224 | |
225 if (IOConnectCallStructMethod(io_connection_, kGenericSensor.function, | |
226 static_cast<const char *>(&input_record_[0]), kInputSize, | |
227 &output_record_[0], &output_size) != KERN_SUCCESS) { | |
228 return false; | |
229 } | |
230 | |
231 // Calculate per-axis calibrated values. | |
232 float axis_value[3]; | |
233 | |
234 for (int i = 0; i < 3; ++i) { | |
235 int sensor_value = 0; | |
236 int size = kGenericSensor.axis_size; | |
237 int index = sensor_->axis[i].index; | |
238 | |
239 // Important Note: Little endian is assumed as this code is Mac-only | |
240 // and PowerPC is currently not supported. | |
241 memcpy(&sensor_value, &output_record_[index], size); | |
242 | |
243 sensor_value = ExtendSign(sensor_value, size); | |
244 | |
245 // Correct value using the current calibration. | |
246 axis_value[i] = static_cast<float>(sensor_value - kGenericSensor.zero_g) / | |
247 kGenericSensor.one_g; | |
248 | |
249 // Make sure we reject any NaN or infinite values. | |
250 if (!isfinite(axis_value[i])) | |
251 return false; | |
252 | |
253 // Clamp value to the [-1, 1] range. | |
254 if (axis_value[i] < -1.0) | |
255 axis_value[i] = -1.0; | |
256 else if (axis_value[i] > 1.0) | |
257 axis_value[i] = 1.0; | |
258 | |
259 // Apply axis inversion. | |
260 if (sensor_->axis[i].inverted) | |
261 axis_value[i] = -axis_value[i]; | |
262 } | |
263 | |
264 // Transform the accelerometer values to W3C draft angles. | |
265 // | |
266 // Accelerometer values are just dot products of the sensor axes | |
267 // by the gravity vector 'g' with the result for the z axis inverted. | |
268 // | |
269 // To understand this transformation calculate the 3rd row of the z-x-y | |
270 // Euler angles rotation matrix (because of the 'g' vector, only 3rd row | |
271 // affects to the result). Note that z-x-y matrix means R = Ry * Rx * Rz. | |
272 // Then, assume alpha = 0 and you get this: | |
273 // | |
274 // x_acc = sin(gamma) | |
275 // y_acc = - cos(gamma) * sin(beta) | |
276 // z_acc = cos(beta) * cos(gamma) | |
277 // | |
278 // After that the rest is just a bit of trigonometry. | |
279 // | |
280 // Also note that alpha can't be provided but it's assumed to be always zero. | |
281 // This is necessary in order to provide enough information to solve | |
282 // the equations. | |
283 // | |
284 const double kRad2deg = 180.0 / M_PI; | |
285 | |
286 orientation->alpha_ = 0.0; | |
287 orientation->beta_ = kRad2deg * atan2(-axis_value[1], axis_value[2]); | |
288 orientation->gamma_ = kRad2deg * asin(axis_value[0]); | |
289 | |
290 // Make sure that the interval boundaries comply with the specification. At | |
291 // this point, beta is [-180, 180] and gamma is [-90, 90], but the spec has | |
292 // the upper bound open on both. | |
293 if (orientation->beta_ == 180.0) { | |
294 orientation->beta_ = -180.0; // -180 == 180 (upside-down) | |
295 } | |
296 if (orientation->gamma_ == 90.0) { | |
297 static double just_less_than_90 = nextafter(90, 0); | |
298 orientation->gamma_ = just_less_than_90; | |
299 } | |
300 | |
301 // At this point, DCHECKing is paranoia. Never hurts. | |
302 DCHECK_GE(orientation->beta_, -180.0); | |
303 DCHECK_LT(orientation->beta_, 180.0); | |
304 DCHECK_GE(orientation->gamma_, -90.0); | |
305 DCHECK_LT(orientation->gamma_, 90.0); | |
306 | |
307 orientation->can_provide_alpha_ = false; | |
308 orientation->can_provide_beta_ = true; | |
309 orientation->can_provide_gamma_ = true; | |
310 | |
311 return true; | |
312 } | |
313 | |
314 // Probe the local hardware looking for a supported sensor device | |
315 // and initialize an I/O connection to it. | |
316 bool AccelerometerMac::Init() { | |
317 // Allocate local variables for model name string (size from SMSLib). | |
318 static const int kNameSize = 32; | |
319 char local_model[kNameSize]; | |
320 | |
321 // Request model name to the kernel. | |
322 size_t name_size = kNameSize; | |
323 int params[2] = { CTL_HW, HW_MODEL }; | |
324 if (sysctl(params, 2, local_model, &name_size, NULL, 0) != 0) | |
325 return NULL; | |
326 | |
327 const SensorDescriptor* sensor_candidate = NULL; | |
328 | |
329 // Look for the current model in the supported sensor list. | |
330 const int kNumSensors = arraysize(kSupportedSensors); | |
331 | |
332 for (int i = 0; i < kNumSensors; ++i) { | |
333 // Check if the supported sensor model name is a prefix | |
334 // of the local hardware model (empty names are accepted). | |
335 const char* p1 = kSupportedSensors[i].model_name; | |
336 for (const char* p2 = local_model; *p1 != '\0' && *p1 == *p2; ++p1, ++p2) | |
337 continue; | |
338 if (*p1 != '\0') | |
339 continue; | |
340 | |
341 // Local hardware found in the supported sensor list. | |
342 sensor_candidate = &kSupportedSensors[i]; | |
343 | |
344 // Get a dictionary of the services matching to the one in the sensor. | |
345 CFMutableDictionaryRef dict = | |
346 IOServiceMatching(kGenericSensor.service_name); | |
347 if (dict == NULL) | |
348 continue; | |
349 | |
350 // Get an iterator for the matching services. | |
351 io_iterator_t device_iterator; | |
352 if (IOServiceGetMatchingServices(kIOMasterPortDefault, dict, | |
353 &device_iterator) != KERN_SUCCESS) { | |
354 continue; | |
355 } | |
356 | |
357 // Get the first device in the list. | |
358 io_object_t device = IOIteratorNext(device_iterator); | |
359 IOObjectRelease(device_iterator); | |
360 if (device == 0) | |
361 continue; | |
362 | |
363 // Try to open device. | |
364 kern_return_t result; | |
365 result = IOServiceOpen(device, mach_task_self(), 0, &io_connection_); | |
366 IOObjectRelease(device); | |
367 if (result != KERN_SUCCESS || io_connection_ == 0) | |
368 return false; | |
369 | |
370 // Local sensor service confirmed by IOKit. | |
371 sensor_ = sensor_candidate; | |
372 break; | |
373 } | |
374 | |
375 if (sensor_ == NULL) | |
376 return false; | |
377 | |
378 // Allocate and initialize input/output records. | |
379 input_record_.resize(kGenericSensor.record_size, 0x01); | |
380 output_record_.resize(kGenericSensor.record_size, 0x00); | |
381 | |
382 // Try to retrieve the current orientation. | |
383 Orientation test_orientation; | |
384 return GetOrientation(&test_orientation); | |
385 } | |
386 | |
387 // Extend the sign of an integer of less than 32 bits to a 32-bit integer. | |
388 int AccelerometerMac::ExtendSign(int value, size_t size) { | |
389 switch (size) { | |
390 case 1: | |
391 if (value & 0x00000080) | |
392 return value | 0xffffff00; | |
393 break; | |
394 | |
395 case 2: | |
396 if (value & 0x00008000) | |
397 return value | 0xffff0000; | |
398 break; | |
399 | |
400 case 3: | |
401 if (value & 0x00800000) | |
402 return value | 0xff000000; | |
403 break; | |
404 | |
405 default: | |
406 LOG(FATAL) << "Invalid integer size for sign extension: " << size; | |
407 } | |
408 | |
409 return value; | |
410 } | |
411 | |
412 } // namespace device_orientation | |
OLD | NEW |