1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <stdlib.h>
18 #include <string.h>
19 #include <timer.h>
20 #include <heap.h>
21 #include <plat/inc/rtc.h>
22 #include <plat/inc/syscfg.h>
23 #include <hostIntf.h>
24 #include <nanohubPacket.h>
25 #include <floatRt.h>
26
27 #include <seos.h>
28
29 #include <nanohub_math.h>
30 #include <sensors.h>
31 #include <limits.h>
32
33 #define ACCEL_MIN_RATE_HZ SENSOR_HZ(15) // 15 HZ
34 #define ACCEL_MAX_LATENCY_NS 40000000ull // 40 ms in nsec
35
36 // all time units in usec, angles in degrees
37 #define RADIANS_TO_DEGREES (180.0f / M_PI)
38
39 #define NS2US(x) (x >> 10) // convert nsec to approx usec
40
41 #define PROPOSAL_SETTLE_TIME NS2US(40000000ull) // 40 ms
42 #define PROPOSAL_MIN_TIME_SINCE_FLAT_ENDED NS2US(500000000ull) // 500 ms
43 #define PROPOSAL_MIN_TIME_SINCE_SWING_ENDED NS2US(300000000ull) // 300 ms
44 #define PROPOSAL_MIN_TIME_SINCE_ACCELERATION_ENDED NS2US(500000000ull) // 500 ms
45
46 #define FLAT_ANGLE 80
47 #define FLAT_TIME NS2US(1000000000ull) // 1 sec
48
49 #define SWING_AWAY_ANGLE_DELTA 20
50 #define SWING_TIME NS2US(300000000ull) // 300 ms
51
52 #define MAX_FILTER_DELTA_TIME NS2US(1000000000ull) // 1 sec
53 #define FILTER_TIME_CONSTANT NS2US(200000000ull) // 200 ms
54
55 #define NEAR_ZERO_MAGNITUDE 1.0f // m/s^2
56 #define ACCELERATION_TOLERANCE 4.0f
57 #define STANDARD_GRAVITY 9.8f
58 #define MIN_ACCELERATION_MAGNITUDE (STANDARD_GRAVITY - ACCELERATION_TOLERANCE)
59 #define MAX_ACCELERATION_MAGNITUDE (STANDARD_GRAVITY + ACCELERATION_TOLERANCE)
60
61 #define MAX_TILT 80
62 #define TILT_OVERHEAD_ENTER -40
63 #define TILT_OVERHEAD_EXIT -15
64
65 #define ADJACENT_ORIENTATION_ANGLE_GAP 45
66
67 #define TILT_HISTORY_SIZE 200
68 #define TILT_REFERENCE_PERIOD NS2US(1800000000000ull) // 30 min
69 #define TILT_REFERENCE_BACKOFF NS2US(300000000000ull) // 5 min
70
71 #define MIN_ACCEL_INTERVAL NS2US(33333333ull) // 33.3 ms for 30 Hz
72
73 #define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
74 #define EVT_SENSOR_WIN_ORIENTATION_DATA_RDY sensorGetMyEventType(SENS_TYPE_WIN_ORIENTATION)
75
76 static int8_t Tilt_Tolerance[4][2] = {
77 /* ROTATION_0 */ { -25, 70 },
78 /* ROTATION_90 */ { -25, 65 },
79 /* ROTATION_180 */ { -25, 60 },
80 /* ROTATION_270 */ { -25, 65 }
81 };
82
83 struct WindowOrientationTask {
84 uint32_t tid;
85 uint32_t handle;
86 uint32_t accelHandle;
87
88 uint64_t last_filtered_time;
89 struct TripleAxisDataPoint last_filtered_sample;
90
91 uint64_t tilt_reference_time;
92 uint64_t accelerating_time;
93 uint64_t predicted_rotation_time;
94 uint64_t flat_time;
95 uint64_t swinging_time;
96
97 uint32_t tilt_history_time[TILT_HISTORY_SIZE];
98 int tilt_history_index;
99 int8_t tilt_history[TILT_HISTORY_SIZE];
100
101 int8_t current_rotation;
102 int8_t prev_valid_rotation;
103 int8_t proposed_rotation;
104 int8_t predicted_rotation;
105
106 bool flat;
107 bool swinging;
108 bool accelerating;
109 bool overhead;
110 };
111
112 static struct WindowOrientationTask mTask;
113
114 static const struct SensorInfo mSi =
115 {
116 .sensorName = "Window Orientation",
117 .sensorType = SENS_TYPE_WIN_ORIENTATION,
118 .numAxis = NUM_AXIS_EMBEDDED,
119 .interrupt = NANOHUB_INT_NONWAKEUP,
120 .minSamples = 20
121 };
122
isTiltAngleAcceptable(int rotation,int8_t tilt_angle)123 static bool isTiltAngleAcceptable(int rotation, int8_t tilt_angle)
124 {
125 return ((tilt_angle >= Tilt_Tolerance[rotation][0])
126 && (tilt_angle <= Tilt_Tolerance[rotation][1]));
127 }
128
isOrientationAngleAcceptable(int current_rotation,int rotation,int orientation_angle)129 static bool isOrientationAngleAcceptable(int current_rotation, int rotation,
130 int orientation_angle)
131 {
132 // If there is no current rotation, then there is no gap.
133 // The gap is used only to introduce hysteresis among advertised orientation
134 // changes to avoid flapping.
135 int lower_bound, upper_bound;
136
137 if (current_rotation >= 0) {
138 // If the specified rotation is the same or is counter-clockwise
139 // adjacent to the current rotation, then we set a lower bound on the
140 // orientation angle.
141 // For example, if currentRotation is ROTATION_0 and proposed is
142 // ROTATION_90, then we want to check orientationAngle > 45 + GAP / 2.
143 if ((rotation == current_rotation)
144 || (rotation == (current_rotation + 1) % 4)) {
145 lower_bound = rotation * 90 - 45
146 + ADJACENT_ORIENTATION_ANGLE_GAP / 2;
147 if (rotation == 0) {
148 if ((orientation_angle >= 315)
149 && (orientation_angle < lower_bound + 360)) {
150 return false;
151 }
152 } else {
153 if (orientation_angle < lower_bound) {
154 return false;
155 }
156 }
157 }
158
159 // If the specified rotation is the same or is clockwise adjacent,
160 // then we set an upper bound on the orientation angle.
161 // For example, if currentRotation is ROTATION_0 and rotation is
162 // ROTATION_270, then we want to check orientationAngle < 315 - GAP / 2.
163 if ((rotation == current_rotation)
164 || (rotation == (current_rotation + 3) % 4)) {
165 upper_bound = rotation * 90 + 45
166 - ADJACENT_ORIENTATION_ANGLE_GAP / 2;
167 if (rotation == 0) {
168 if ((orientation_angle <= 45)
169 && (orientation_angle > upper_bound)) {
170 return false;
171 }
172 } else {
173 if (orientation_angle > upper_bound) {
174 return false;
175 }
176 }
177 }
178 }
179 return true;
180 }
181
isPredictedRotationAcceptable(uint64_t now)182 static bool isPredictedRotationAcceptable(uint64_t now)
183 {
184 // The predicted rotation must have settled long enough.
185 if (now < mTask.predicted_rotation_time + PROPOSAL_SETTLE_TIME) {
186 return false;
187 }
188
189 // The last flat state (time since picked up) must have been sufficiently
190 // long ago.
191 if (now < mTask.flat_time + PROPOSAL_MIN_TIME_SINCE_FLAT_ENDED) {
192 return false;
193 }
194
195 // The last swing state (time since last movement to put down) must have
196 // been sufficiently long ago.
197 if (now < mTask.swinging_time + PROPOSAL_MIN_TIME_SINCE_SWING_ENDED) {
198 return false;
199 }
200
201 // The last acceleration state must have been sufficiently long ago.
202 if (now < mTask.accelerating_time
203 + PROPOSAL_MIN_TIME_SINCE_ACCELERATION_ENDED) {
204 return false;
205 }
206
207 // Looks good!
208 return true;
209 }
210
clearPredictedRotation()211 static void clearPredictedRotation()
212 {
213 mTask.predicted_rotation = -1;
214 mTask.predicted_rotation_time = 0;
215 }
216
clearTiltHistory()217 static void clearTiltHistory()
218 {
219 mTask.tilt_history_time[0] = 0;
220 mTask.tilt_history_index = 1;
221 mTask.tilt_reference_time = 0;
222 }
223
reset()224 static void reset()
225 {
226 mTask.last_filtered_time = 0;
227 mTask.proposed_rotation = -1;
228
229 mTask.flat_time = 0;
230 mTask.flat = false;
231
232 mTask.swinging_time = 0;
233 mTask.swinging = false;
234
235 mTask.accelerating_time = 0;
236 mTask.accelerating = false;
237
238 mTask.overhead = false;
239
240 clearPredictedRotation();
241 clearTiltHistory();
242 }
243
updatePredictedRotation(uint64_t now,int rotation)244 static void updatePredictedRotation(uint64_t now, int rotation)
245 {
246 if (mTask.predicted_rotation != rotation) {
247 mTask.predicted_rotation = rotation;
248 mTask.predicted_rotation_time = now;
249 }
250 }
251
isAccelerating(float magnitude)252 static bool isAccelerating(float magnitude)
253 {
254 return ((magnitude < MIN_ACCELERATION_MAGNITUDE)
255 || (magnitude > MAX_ACCELERATION_MAGNITUDE));
256 }
257
addTiltHistoryEntry(uint64_t now,int8_t tilt)258 static void addTiltHistoryEntry(uint64_t now, int8_t tilt)
259 {
260 uint64_t old_reference_time, delta;
261 size_t i;
262 int index;
263
264 if (mTask.tilt_reference_time == 0) {
265 // set reference_time after reset()
266
267 mTask.tilt_reference_time = now - 1;
268 } else if (mTask.tilt_reference_time + TILT_REFERENCE_PERIOD < now) {
269 // uint32_t tilt_history_time[] is good up to 71 min (2^32 * 1e-6 sec).
270 // proactively shift reference_time every 30 min,
271 // all history entries are within 5 min interval (15Hz x 200 samples)
272
273 old_reference_time = mTask.tilt_reference_time;
274 mTask.tilt_reference_time = now - TILT_REFERENCE_BACKOFF;
275
276 delta = mTask.tilt_reference_time - old_reference_time;
277 for (i = 0; i < TILT_HISTORY_SIZE; ++i) {
278 mTask.tilt_history_time[i] = (mTask.tilt_history_time[i] > delta)
279 ? (mTask.tilt_history_time[i] - delta) : 0;
280 }
281 }
282
283 index = mTask.tilt_history_index;
284 mTask.tilt_history[index] = tilt;
285 mTask.tilt_history_time[index] = now - mTask.tilt_reference_time;
286
287 index = ((index + 1) == TILT_HISTORY_SIZE) ? 0 : (index + 1);
288 mTask.tilt_history_index = index;
289 mTask.tilt_history_time[index] = 0;
290 }
291
nextTiltHistoryIndex(int index)292 static int nextTiltHistoryIndex(int index)
293 {
294 int next = (index == 0) ? (TILT_HISTORY_SIZE - 1): (index - 1);
295 return ((mTask.tilt_history_time[next] != 0) ? next : -1);
296 }
297
isFlat(uint64_t now)298 static bool isFlat(uint64_t now)
299 {
300 int i = mTask.tilt_history_index;
301 for (; (i = nextTiltHistoryIndex(i)) >= 0;) {
302 if (mTask.tilt_history[i] < FLAT_ANGLE) {
303 break;
304 }
305 if (mTask.tilt_reference_time + mTask.tilt_history_time[i] + FLAT_TIME <= now) {
306 // Tilt has remained greater than FLAT_ANGLE for FLAT_TIME.
307 return true;
308 }
309 }
310 return false;
311 }
312
isSwinging(uint64_t now,int8_t tilt)313 static bool isSwinging(uint64_t now, int8_t tilt)
314 {
315 int i = mTask.tilt_history_index;
316 for (; (i = nextTiltHistoryIndex(i)) >= 0;) {
317 if (mTask.tilt_reference_time + mTask.tilt_history_time[i] + SWING_TIME
318 < now) {
319 break;
320 }
321 if (mTask.tilt_history[i] + SWING_AWAY_ANGLE_DELTA <= tilt) {
322 // Tilted away by SWING_AWAY_ANGLE_DELTA within SWING_TIME.
323 return true;
324 }
325 }
326 return false;
327 }
328
add_samples(struct TripleAxisDataEvent * ev)329 static bool add_samples(struct TripleAxisDataEvent *ev)
330 {
331 int i, tilt_tmp;
332 int orientation_angle, nearest_rotation;
333 float x, y, z, alpha, magnitude;
334 uint64_t now_nsec = ev->referenceTime, now;
335 uint64_t then, time_delta;
336 struct TripleAxisDataPoint *last_sample;
337 size_t sampleCnt = ev->samples[0].firstSample.numSamples;
338 bool skip_sample;
339 bool accelerating, flat, swinging;
340 bool change_detected;
341 int8_t old_proposed_rotation, proposed_rotation;
342 int8_t tilt_angle;
343
344 for (i = 0; i < sampleCnt; i++) {
345
346 x = ev->samples[i].x;
347 y = ev->samples[i].y;
348 z = ev->samples[i].z;
349
350 // Apply a low-pass filter to the acceleration up vector in cartesian space.
351 // Reset the orientation listener state if the samples are too far apart in time.
352
353 now_nsec += i > 0 ? ev->samples[i].deltaTime : 0;
354 now = NS2US(now_nsec); // convert to ~usec
355
356 last_sample = &mTask.last_filtered_sample;
357 then = mTask.last_filtered_time;
358 time_delta = now - then;
359
360 if ((now < then) || (now > then + MAX_FILTER_DELTA_TIME)) {
361 reset();
362 skip_sample = true;
363 } else {
364 // alpha is the weight on the new sample
365 alpha = floatFromUint64(time_delta) / floatFromUint64(FILTER_TIME_CONSTANT + time_delta);
366 x = alpha * (x - last_sample->x) + last_sample->x;
367 y = alpha * (y - last_sample->y) + last_sample->y;
368 z = alpha * (z - last_sample->z) + last_sample->z;
369
370 skip_sample = false;
371 }
372
373 // drop samples when input sampling rate is 2x higher than requested
374 if (!skip_sample && (time_delta < MIN_ACCEL_INTERVAL)) {
375 skip_sample = true;
376 } else {
377 mTask.last_filtered_time = now;
378 mTask.last_filtered_sample.x = x;
379 mTask.last_filtered_sample.y = y;
380 mTask.last_filtered_sample.z = z;
381 }
382
383 accelerating = false;
384 flat = false;
385 swinging = false;
386
387 if (!skip_sample) {
388 // Calculate the magnitude of the acceleration vector.
389 magnitude = sqrtf(x * x + y * y + z * z);
390
391 if (magnitude < NEAR_ZERO_MAGNITUDE) {
392 clearPredictedRotation();
393 } else {
394 // Determine whether the device appears to be undergoing
395 // external acceleration.
396 if (isAccelerating(magnitude)) {
397 accelerating = true;
398 mTask.accelerating_time = now;
399 }
400
401 // Calculate the tilt angle.
402 // This is the angle between the up vector and the x-y plane
403 // (the plane of the screen) in a range of [-90, 90] degrees.
404 // -90 degrees: screen horizontal and facing the ground (overhead)
405 // 0 degrees: screen vertical
406 // 90 degrees: screen horizontal and facing the sky (on table)
407 tilt_tmp = (int)(asinf(z / magnitude) * RADIANS_TO_DEGREES);
408 tilt_tmp = (tilt_tmp > 127) ? 127 : tilt_tmp;
409 tilt_tmp = (tilt_tmp < -128) ? -128 : tilt_tmp;
410 tilt_angle = tilt_tmp;
411 addTiltHistoryEntry(now, tilt_angle);
412
413 // Determine whether the device appears to be flat or swinging.
414 if (isFlat(now)) {
415 flat = true;
416 mTask.flat_time = now;
417 }
418 if (isSwinging(now, tilt_angle)) {
419 swinging = true;
420 mTask.swinging_time = now;
421 }
422
423 // If the tilt angle is too close to horizontal then we cannot
424 // determine the orientation angle of the screen.
425 if (tilt_angle <= TILT_OVERHEAD_ENTER) {
426 mTask.overhead = true;
427 } else if (tilt_angle >= TILT_OVERHEAD_EXIT) {
428 mTask.overhead = false;
429 }
430
431 if (mTask.overhead) {
432 clearPredictedRotation();
433 } else if (fabsf(tilt_angle) > MAX_TILT) {
434 clearPredictedRotation();
435 } else {
436 // Calculate the orientation angle.
437 // This is the angle between the x-y projection of the up
438 // vector onto the +y-axis, increasing clockwise in a range
439 // of [0, 360] degrees.
440 orientation_angle = (int)(-atan2f(-x, y) * RADIANS_TO_DEGREES);
441 if (orientation_angle < 0) {
442 // atan2 returns [-180, 180]; normalize to [0, 360]
443 orientation_angle += 360;
444 }
445
446 // Find the nearest rotation.
447 nearest_rotation = (orientation_angle + 45) / 90;
448 if (nearest_rotation == 4) {
449 nearest_rotation = 0;
450 }
451 // Determine the predicted orientation.
452 if (isTiltAngleAcceptable(nearest_rotation, tilt_angle)
453 && isOrientationAngleAcceptable(mTask.current_rotation,
454 nearest_rotation,
455 orientation_angle)) {
456 updatePredictedRotation(now, nearest_rotation);
457 } else {
458 clearPredictedRotation();
459 }
460 }
461 }
462 }
463
464 mTask.flat = flat;
465 mTask.swinging = swinging;
466 mTask.accelerating = accelerating;
467
468 // Determine new proposed rotation.
469 old_proposed_rotation = mTask.proposed_rotation;
470 if ((mTask.predicted_rotation < 0)
471 || isPredictedRotationAcceptable(now)) {
472
473 mTask.proposed_rotation = mTask.predicted_rotation;
474 }
475 proposed_rotation = mTask.proposed_rotation;
476
477 if ((proposed_rotation != old_proposed_rotation)
478 && (proposed_rotation >= 0)) {
479 mTask.current_rotation = proposed_rotation;
480
481 change_detected = (proposed_rotation != mTask.prev_valid_rotation);
482 mTask.prev_valid_rotation = proposed_rotation;
483
484 if (change_detected) {
485 return true;
486 }
487 }
488 }
489
490 return false;
491 }
492
493
windowOrientationPower(bool on,void * cookie)494 static bool windowOrientationPower(bool on, void *cookie)
495 {
496 if (on == false && mTask.accelHandle != 0) {
497 sensorRelease(mTask.tid, mTask.accelHandle);
498 mTask.accelHandle = 0;
499 osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
500 }
501
502 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
503
504 return true;
505 }
506
windowOrientationSetRate(uint32_t rate,uint64_t latency,void * cookie)507 static bool windowOrientationSetRate(uint32_t rate, uint64_t latency, void *cookie)
508 {
509 int i;
510
511 if (mTask.accelHandle == 0) {
512 for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) {
513 if (sensorRequest(mTask.tid, mTask.accelHandle, ACCEL_MIN_RATE_HZ, ACCEL_MAX_LATENCY_NS)) {
514 // clear hysteresis
515 mTask.current_rotation = -1;
516 mTask.prev_valid_rotation = -1;
517 reset();
518 osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
519 break;
520 }
521 }
522 }
523
524 if (mTask.accelHandle != 0)
525 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
526
527 return true;
528 }
529
windowOrientationFirmwareUpload(void * cookie)530 static bool windowOrientationFirmwareUpload(void *cookie)
531 {
532 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG,
533 1, 0);
534 return true;
535 }
536
windowOrientationFlush(void * cookie)537 static bool windowOrientationFlush(void *cookie)
538 {
539 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_WIN_ORIENTATION), SENSOR_DATA_EVENT_FLUSH, NULL);
540 }
541
windowOrientationHandleEvent(uint32_t evtType,const void * evtData)542 static void windowOrientationHandleEvent(uint32_t evtType, const void* evtData)
543 {
544 struct TripleAxisDataEvent *ev;
545 union EmbeddedDataPoint sample;
546 bool rotation_changed;
547
548 if (evtData == SENSOR_DATA_EVENT_FLUSH)
549 return;
550
551 switch (evtType) {
552 case EVT_SENSOR_ACC_DATA_RDY:
553 ev = (struct TripleAxisDataEvent *)evtData;
554 rotation_changed = add_samples(ev);
555
556 if (rotation_changed) {
557 //osLog(LOG_INFO, "WO: ********** rotation changed to ********: %d\n", (int)mTask.proposed_rotation);
558
559 // send a single int32 here so no memory alloc/free needed.
560 sample.idata = mTask.proposed_rotation;
561 osEnqueueEvt(EVT_SENSOR_WIN_ORIENTATION_DATA_RDY, sample.vptr, NULL);
562 }
563 break;
564 }
565 }
566
567 static const struct SensorOps mSops =
568 {
569 .sensorPower = windowOrientationPower,
570 .sensorFirmwareUpload = windowOrientationFirmwareUpload,
571 .sensorSetRate = windowOrientationSetRate,
572 .sensorFlush = windowOrientationFlush,
573 };
574
window_orientation_start(uint32_t tid)575 static bool window_orientation_start(uint32_t tid)
576 {
577 osLog(LOG_INFO, " WINDOW ORIENTATION: %ld\n", tid);
578
579 mTask.tid = tid;
580
581 mTask.current_rotation = -1;
582 mTask.prev_valid_rotation = -1;
583 reset();
584
585 mTask.handle = sensorRegister(&mSi, &mSops, NULL, true);
586
587 return true;
588 }
589
windowOrientationEnd()590 static void windowOrientationEnd()
591 {
592 }
593
594 INTERNAL_APP_INIT(
595 APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 3),
596 0,
597 window_orientation_start,
598 windowOrientationEnd,
599 windowOrientationHandleEvent);
600
601