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 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 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 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 211 static void clearPredictedRotation() 212 { 213 mTask.predicted_rotation = -1; 214 mTask.predicted_rotation_time = 0; 215 } 216 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 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 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 252 static bool isAccelerating(float magnitude) 253 { 254 return ((magnitude < MIN_ACCELERATION_MAGNITUDE) 255 || (magnitude > MAX_ACCELERATION_MAGNITUDE)); 256 } 257 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 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 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 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 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 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 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 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 537 static bool windowOrientationFlush(void *cookie) 538 { 539 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_WIN_ORIENTATION), SENSOR_DATA_EVENT_FLUSH, NULL); 540 } 541 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 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 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
