1 /* 2 * Copyright (C) 2008-2014 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 <ctype.h> 18 #include <dirent.h> 19 #include <errno.h> 20 #include <fcntl.h> 21 #include <inttypes.h> 22 #include <math.h> 23 #include <poll.h> 24 #include <pthread.h> 25 #include <stdlib.h> 26 #include <sys/select.h> 27 #include <unistd.h> 28 29 #define LOG_TAG "CwMcuSensor" 30 #include <cutils/log.h> 31 #include <cutils/properties.h> 32 33 #include "CwMcuSensor.h" 34 35 36 #define REL_Significant_Motion REL_WHEEL 37 #define LIGHTSENSOR_LEVEL 10 38 #define DEBUG_DATA 0 39 #define COMPASS_CALIBRATION_DATA_SIZE 26 40 #define G_SENSOR_CALIBRATION_DATA_SIZE 3 41 #define NS_PER_MS 1000000LL 42 #define EXHAUSTED_MAGIC 0x77 43 44 /*****************************************************************************/ 45 #define IIO_MAX_BUFF_SIZE 4096 46 #define IIO_MAX_DATA_SIZE 24 47 #define IIO_MAX_NAME_LENGTH 30 48 #define IIO_BUF_SIZE_RETRY 8 49 #define INT32_CHAR_LEN 12 50 51 #define INIT_TRIGGER_RETRY 5 52 53 static const char iio_dir[] = "/sys/bus/iio/devices/"; 54 55 static int min(int a, int b) { 56 return (a < b) ? a : b; 57 } 58 59 static int chomp(char *buf, size_t len) { 60 if (buf == NULL) 61 return -1; 62 63 while (len > 0 && isspace(buf[len-1])) { 64 buf[len - 1] = '\0'; 65 len--; 66 } 67 68 return 0; 69 } 70 71 int CwMcuSensor::sysfs_set_input_attr(const char *attr, char *value, size_t len) { 72 char fname[PATH_MAX]; 73 int fd; 74 int rc; 75 76 snprintf(fname, sizeof(fname), "%s/%s", mDevPath, attr); 77 fname[sizeof(fname) - 1] = '\0'; 78 79 fd = open(fname, O_WRONLY); 80 if (fd < 0) { 81 ALOGE("%s: fname = %s, fd = %d, failed: %s\n", __func__, fname, fd, strerror(errno)); 82 return -EACCES; 83 } 84 85 rc = write(fd, value, (size_t)len); 86 if (rc < 0) { 87 ALOGE("%s: write failed: fd = %d, rc = %d, strerr = %s\n", __func__, fd, rc, strerror(errno)); 88 close(fd); 89 return -EIO; 90 } 91 92 close(fd); 93 94 return 0; 95 } 96 97 int CwMcuSensor::sysfs_set_input_attr_by_int(const char *attr, int value) { 98 char buf[INT32_CHAR_LEN]; 99 100 size_t n = snprintf(buf, sizeof(buf), "%d", value); 101 if (n > sizeof(buf)) { 102 return -1; 103 } 104 105 return sysfs_set_input_attr(attr, buf, n); 106 } 107 108 static inline int find_type_by_name(const char *name, const char *type) { 109 const struct dirent *ent; 110 int number, numstrlen; 111 112 DIR *dp; 113 char thisname[IIO_MAX_NAME_LENGTH]; 114 char *filename; 115 size_t size; 116 size_t typeLen = strlen(type); 117 size_t nameLen = strlen(name); 118 119 if (nameLen >= sizeof(thisname) - 1) { 120 return -ERANGE; 121 } 122 123 dp = opendir(iio_dir); 124 if (dp == NULL) { 125 return -ENODEV; 126 } 127 128 while (ent = readdir(dp), ent != NULL) { 129 if (strcmp(ent->d_name, ".") != 0 && 130 strcmp(ent->d_name, "..") != 0 && 131 strlen(ent->d_name) > typeLen && 132 strncmp(ent->d_name, type, typeLen) == 0) { 133 numstrlen = sscanf(ent->d_name + typeLen, 134 "%d", &number); 135 136 /* verify the next character is not a colon */ 137 if (ent->d_name[strlen(type) + numstrlen] != ':') { 138 size = sizeof(iio_dir) - 1 + typeLen + numstrlen + 6; 139 filename = (char *)malloc(size); 140 141 if (filename == NULL) 142 return -ENOMEM; 143 144 snprintf(filename, size, 145 "%s%s%d/name", 146 iio_dir, type, number); 147 148 int fd = open(filename, O_RDONLY); 149 free(filename); 150 if (fd < 0) { 151 continue; 152 } 153 size = read(fd, thisname, sizeof(thisname) - 1); 154 close(fd); 155 if (size < nameLen) { 156 continue; 157 } 158 thisname[size] = '\0'; 159 if (strncmp(name, thisname, nameLen)) { 160 continue; 161 } 162 // check for termination or whitespace 163 if (!thisname[nameLen] || isspace(thisname[nameLen])) { 164 return number; 165 } 166 } 167 } 168 } 169 return -ENODEV; 170 } 171 172 int fill_block_debug = 0; 173 174 pthread_mutex_t sys_fs_mutex = PTHREAD_MUTEX_INITIALIZER; 175 pthread_mutex_t sync_timestamp_algo_mutex = PTHREAD_MUTEX_INITIALIZER; 176 pthread_mutex_t last_timestamp_mutex = PTHREAD_MUTEX_INITIALIZER; 177 178 void CwMcuSensor::sync_time_thread_in_class(void) { 179 int fd; 180 char buf[24]; 181 int err; 182 uint64_t mcu_current_time; 183 uint64_t cpu_current_time; 184 int open_errno; 185 186 ALOGV("sync_time_thread_in_class++:\n"); 187 188 pthread_mutex_lock(&sys_fs_mutex); 189 190 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "batch_enable"); 191 192 fd = open(fixed_sysfs_path, O_RDWR); 193 open_errno = errno; 194 pthread_mutex_unlock(&sys_fs_mutex); 195 if (fd >= 0) { 196 err = read(fd, buf, sizeof(buf) - 1); 197 cpu_current_time = getTimestamp(); 198 if (err < 0) { 199 ALOGE("sync_time_thread_in_class: read fail, err = %d\n", err); 200 } else { 201 buf[err] = '\0'; 202 mcu_current_time = strtoull(buf, NULL, 10) * NS_PER_US; 203 if (errno == ERANGE) { 204 ALOGE("sync_time_thread_in_class: strtoll fails, strerr = %s, buf = %s\n", 205 strerror(errno), buf); 206 } else { 207 pthread_mutex_lock(&sync_timestamp_algo_mutex); 208 209 if (mcu_current_time == 0) { 210 // Do a recovery mechanism of timestamp estimation when the sensor_hub reset happened 211 ALOGE("Sync: sensor hub is on reset\n"); 212 time_slope = 1; 213 memset(last_mcu_timestamp, 0, sizeof(last_mcu_timestamp)); 214 memset(last_cpu_timestamp, 0, sizeof(last_cpu_timestamp)); 215 for (int i=0; i<numSensors; i++) { 216 offset_reset[i] = true; 217 } 218 } else if ((mcu_current_time <= last_mcu_sync_time) || (last_mcu_sync_time == 0)) { 219 ALOGV("Sync: time_slope was not estimated yet\n"); 220 time_slope = 1; 221 time_offset = cpu_current_time - mcu_current_time; 222 for (int i=0; i<numSensors; i++) { 223 offset_reset[i] = true; 224 } 225 } else { 226 time_slope = (float)(cpu_current_time - last_cpu_sync_time) / 227 (float)(mcu_current_time - last_mcu_sync_time); 228 time_offset = cpu_current_time - mcu_current_time; 229 } 230 ALOGV("Sync: time_offset = %" PRId64 ", time_slope = %f\n", time_offset, time_slope); 231 ALOGV("Sync: mcu_current_time = %" PRId64 ", last_mcu_sync_time = %" PRId64 "\n", mcu_current_time, last_mcu_sync_time); 232 ALOGV("Sync: cpu_current_time = %" PRId64 ", last_cpu_sync_time = %" PRId64 "\n", cpu_current_time, last_cpu_sync_time); 233 234 last_mcu_sync_time = mcu_current_time; 235 last_cpu_sync_time = cpu_current_time; 236 237 pthread_mutex_unlock(&sync_timestamp_algo_mutex); 238 } 239 } 240 close(fd); 241 } else { 242 ALOGE("sync_time_thread_in_class: open failed, path = .../batch_enable, fd = %d," 243 " strerr = %s\n", fd, strerror(open_errno)); 244 } 245 246 ALOGV("sync_time_thread_in_class--:\n"); 247 } 248 249 void *sync_time_thread_run(void *context) { 250 CwMcuSensor *myClass = (CwMcuSensor *)context; 251 252 while (1) { 253 ALOGV("sync_time_thread_run++:\n"); 254 myClass->sync_time_thread_in_class(); 255 sleep(PERIODIC_SYNC_TIME_SEC); 256 ALOGV("sync_time_thread_run--:\n"); 257 } 258 return NULL; 259 } 260 261 CwMcuSensor::CwMcuSensor() 262 : SensorBase(NULL, "CwMcuSensor") 263 , mEnabled(0) 264 , mInputReader(IIO_MAX_BUFF_SIZE) 265 , time_slope(1) 266 , time_offset(0) 267 , init_trigger_done(false) { 268 269 int rc; 270 271 memset(last_mcu_timestamp, 0, sizeof(last_mcu_timestamp)); 272 memset(last_cpu_timestamp, 0, sizeof(last_cpu_timestamp)); 273 for (int i=0; i<numSensors; i++) { 274 offset_reset[i] = true; 275 } 276 277 mPendingEvents[CW_ACCELERATION].version = sizeof(sensors_event_t); 278 mPendingEvents[CW_ACCELERATION].sensor = ID_A; 279 mPendingEvents[CW_ACCELERATION].type = SENSOR_TYPE_ACCELEROMETER; 280 mPendingEvents[CW_ACCELERATION].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 281 282 mPendingEvents[CW_MAGNETIC].version = sizeof(sensors_event_t); 283 mPendingEvents[CW_MAGNETIC].sensor = ID_M; 284 mPendingEvents[CW_MAGNETIC].type = SENSOR_TYPE_MAGNETIC_FIELD; 285 286 mPendingEvents[CW_GYRO].version = sizeof(sensors_event_t); 287 mPendingEvents[CW_GYRO].sensor = ID_GY; 288 mPendingEvents[CW_GYRO].type = SENSOR_TYPE_GYROSCOPE; 289 mPendingEvents[CW_GYRO].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 290 291 mPendingEvents[CW_LIGHT].version = sizeof(sensors_event_t); 292 mPendingEvents[CW_LIGHT].sensor = ID_L; 293 mPendingEvents[CW_LIGHT].type = SENSOR_TYPE_LIGHT; 294 memset(mPendingEvents[CW_LIGHT].data, 0, sizeof(mPendingEvents[CW_LIGHT].data)); 295 296 mPendingEvents[CW_PRESSURE].version = sizeof(sensors_event_t); 297 mPendingEvents[CW_PRESSURE].sensor = ID_PS; 298 mPendingEvents[CW_PRESSURE].type = SENSOR_TYPE_PRESSURE; 299 memset(mPendingEvents[CW_PRESSURE].data, 0, sizeof(mPendingEvents[CW_PRESSURE].data)); 300 301 mPendingEvents[CW_ORIENTATION].version = sizeof(sensors_event_t); 302 mPendingEvents[CW_ORIENTATION].sensor = ID_O; 303 mPendingEvents[CW_ORIENTATION].type = SENSOR_TYPE_ORIENTATION; 304 mPendingEvents[CW_ORIENTATION].orientation.status = SENSOR_STATUS_ACCURACY_HIGH; 305 306 mPendingEvents[CW_ROTATIONVECTOR].version = sizeof(sensors_event_t); 307 mPendingEvents[CW_ROTATIONVECTOR].sensor = ID_RV; 308 mPendingEvents[CW_ROTATIONVECTOR].type = SENSOR_TYPE_ROTATION_VECTOR; 309 310 mPendingEvents[CW_LINEARACCELERATION].version = sizeof(sensors_event_t); 311 mPendingEvents[CW_LINEARACCELERATION].sensor = ID_LA; 312 mPendingEvents[CW_LINEARACCELERATION].type = SENSOR_TYPE_LINEAR_ACCELERATION; 313 314 mPendingEvents[CW_GRAVITY].version = sizeof(sensors_event_t); 315 mPendingEvents[CW_GRAVITY].sensor = ID_G; 316 mPendingEvents[CW_GRAVITY].type = SENSOR_TYPE_GRAVITY; 317 318 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].version = sizeof(sensors_event_t); 319 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].sensor = ID_CW_MAGNETIC_UNCALIBRATED; 320 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; 321 322 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].version = sizeof(sensors_event_t); 323 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].sensor = ID_CW_GYROSCOPE_UNCALIBRATED; 324 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; 325 326 mPendingEvents[CW_GAME_ROTATION_VECTOR].version = sizeof(sensors_event_t); 327 mPendingEvents[CW_GAME_ROTATION_VECTOR].sensor = ID_CW_GAME_ROTATION_VECTOR; 328 mPendingEvents[CW_GAME_ROTATION_VECTOR].type = SENSOR_TYPE_GAME_ROTATION_VECTOR; 329 330 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].version = sizeof(sensors_event_t); 331 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].sensor = ID_CW_GEOMAGNETIC_ROTATION_VECTOR; 332 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; 333 334 mPendingEvents[CW_SIGNIFICANT_MOTION].version = sizeof(sensors_event_t); 335 mPendingEvents[CW_SIGNIFICANT_MOTION].sensor = ID_CW_SIGNIFICANT_MOTION; 336 mPendingEvents[CW_SIGNIFICANT_MOTION].type = SENSOR_TYPE_SIGNIFICANT_MOTION; 337 338 mPendingEvents[CW_STEP_DETECTOR].version = sizeof(sensors_event_t); 339 mPendingEvents[CW_STEP_DETECTOR].sensor = ID_CW_STEP_DETECTOR; 340 mPendingEvents[CW_STEP_DETECTOR].type = SENSOR_TYPE_STEP_DETECTOR; 341 342 mPendingEvents[CW_STEP_COUNTER].version = sizeof(sensors_event_t); 343 mPendingEvents[CW_STEP_COUNTER].sensor = ID_CW_STEP_COUNTER; 344 mPendingEvents[CW_STEP_COUNTER].type = SENSOR_TYPE_STEP_COUNTER; 345 346 347 mPendingEvents[CW_ACCELERATION_W].version = sizeof(sensors_event_t); 348 mPendingEvents[CW_ACCELERATION_W].sensor = ID_A_W; 349 mPendingEvents[CW_ACCELERATION_W].type = SENSOR_TYPE_ACCELEROMETER; 350 mPendingEvents[CW_ACCELERATION_W].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 351 352 mPendingEvents[CW_MAGNETIC_W].version = sizeof(sensors_event_t); 353 mPendingEvents[CW_MAGNETIC_W].sensor = ID_M_W; 354 mPendingEvents[CW_MAGNETIC_W].type = SENSOR_TYPE_MAGNETIC_FIELD; 355 356 mPendingEvents[CW_GYRO_W].version = sizeof(sensors_event_t); 357 mPendingEvents[CW_GYRO_W].sensor = ID_GY_W; 358 mPendingEvents[CW_GYRO_W].type = SENSOR_TYPE_GYROSCOPE; 359 mPendingEvents[CW_GYRO_W].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 360 361 mPendingEvents[CW_PRESSURE_W].version = sizeof(sensors_event_t); 362 mPendingEvents[CW_PRESSURE_W].sensor = ID_PS_W; 363 mPendingEvents[CW_PRESSURE_W].type = SENSOR_TYPE_PRESSURE; 364 memset(mPendingEvents[CW_PRESSURE_W].data, 0, sizeof(mPendingEvents[CW_PRESSURE_W].data)); 365 366 mPendingEvents[CW_ORIENTATION_W].version = sizeof(sensors_event_t); 367 mPendingEvents[CW_ORIENTATION_W].sensor = ID_O_W; 368 mPendingEvents[CW_ORIENTATION_W].type = SENSOR_TYPE_ORIENTATION; 369 mPendingEvents[CW_ORIENTATION_W].orientation.status = SENSOR_STATUS_ACCURACY_HIGH; 370 371 mPendingEvents[CW_ROTATIONVECTOR_W].version = sizeof(sensors_event_t); 372 mPendingEvents[CW_ROTATIONVECTOR_W].sensor = ID_RV_W; 373 mPendingEvents[CW_ROTATIONVECTOR_W].type = SENSOR_TYPE_ROTATION_VECTOR; 374 375 mPendingEvents[CW_LINEARACCELERATION_W].version = sizeof(sensors_event_t); 376 mPendingEvents[CW_LINEARACCELERATION_W].sensor = ID_LA_W; 377 mPendingEvents[CW_LINEARACCELERATION_W].type = SENSOR_TYPE_LINEAR_ACCELERATION; 378 379 mPendingEvents[CW_GRAVITY_W].version = sizeof(sensors_event_t); 380 mPendingEvents[CW_GRAVITY_W].sensor = ID_G_W; 381 mPendingEvents[CW_GRAVITY_W].type = SENSOR_TYPE_GRAVITY; 382 383 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].version = sizeof(sensors_event_t); 384 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].sensor = ID_CW_MAGNETIC_UNCALIBRATED_W; 385 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; 386 387 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].version = sizeof(sensors_event_t); 388 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].sensor = ID_CW_GYROSCOPE_UNCALIBRATED_W; 389 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; 390 391 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].version = sizeof(sensors_event_t); 392 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].sensor = ID_CW_GAME_ROTATION_VECTOR_W; 393 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].type = SENSOR_TYPE_GAME_ROTATION_VECTOR; 394 395 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].version = sizeof(sensors_event_t); 396 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].sensor = ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W; 397 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; 398 399 mPendingEvents[CW_STEP_DETECTOR_W].version = sizeof(sensors_event_t); 400 mPendingEvents[CW_STEP_DETECTOR_W].sensor = ID_CW_STEP_DETECTOR_W; 401 mPendingEvents[CW_STEP_DETECTOR_W].type = SENSOR_TYPE_STEP_DETECTOR; 402 403 mPendingEvents[CW_STEP_COUNTER_W].version = sizeof(sensors_event_t); 404 mPendingEvents[CW_STEP_COUNTER_W].sensor = ID_CW_STEP_COUNTER_W; 405 mPendingEvents[CW_STEP_COUNTER_W].type = SENSOR_TYPE_STEP_COUNTER; 406 407 408 mPendingEventsFlush.version = META_DATA_VERSION; 409 mPendingEventsFlush.sensor = 0; 410 mPendingEventsFlush.type = SENSOR_TYPE_META_DATA; 411 412 char buffer_access[PATH_MAX]; 413 const char *device_name = "CwMcuSensor"; 414 int rate = 20, dev_num, enabled = 0, i; 415 416 dev_num = find_type_by_name(device_name, "iio:device"); 417 if (dev_num < 0) 418 dev_num = 0; 419 420 snprintf(buffer_access, sizeof(buffer_access), 421 "/dev/iio:device%d", dev_num); 422 423 data_fd = open(buffer_access, O_RDWR); 424 if (data_fd < 0) { 425 ALOGE("CwMcuSensor::CwMcuSensor: open file '%s' failed: %s\n", 426 buffer_access, strerror(errno)); 427 } 428 429 if (data_fd >= 0) { 430 int i; 431 int fd; 432 int iio_buf_size; 433 434 ALOGV("%s: 11 Before pthread_mutex_lock()\n", __func__); 435 pthread_mutex_lock(&sys_fs_mutex); 436 ALOGV("%s: 11 Acquired pthread_mutex_lock()\n", __func__); 437 438 strcpy(fixed_sysfs_path,"/sys/class/htc_sensorhub/sensor_hub/"); 439 fixed_sysfs_path_len = strlen(fixed_sysfs_path); 440 441 snprintf(mDevPath, sizeof(mDevPath), "%s%s", fixed_sysfs_path, "iio"); 442 443 snprintf(mTriggerName, sizeof(mTriggerName), "%s-dev%d", 444 device_name, dev_num); 445 ALOGV("CwMcuSensor::CwMcuSensor: mTriggerName = %s\n", mTriggerName); 446 447 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) { 448 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed00: %s\n", 449 strerror(errno)); 450 } 451 452 // This is a piece of paranoia that retry for current_trigger 453 for (i = 0; i < INIT_TRIGGER_RETRY; i++) { 454 rc = sysfs_set_input_attr("trigger/current_trigger", 455 mTriggerName, strlen(mTriggerName)); 456 if (rc < 0) { 457 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) { 458 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed11: %s\n", 459 strerror(errno)); 460 } 461 ALOGE("CwMcuSensor::CwMcuSensor: set current trigger failed: rc = %d, strerr() = %s" 462 ", i = %d\n", 463 rc, strerror(errno), i); 464 } else { 465 init_trigger_done = true; 466 break; 467 } 468 } 469 470 iio_buf_size = IIO_MAX_BUFF_SIZE; 471 for (i = 0; i < IIO_BUF_SIZE_RETRY; i++) { 472 if (sysfs_set_input_attr_by_int("buffer/length", iio_buf_size) < 0) { 473 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer length (%d) failed: %s\n", 474 iio_buf_size, strerror(errno)); 475 } else { 476 if (sysfs_set_input_attr_by_int("buffer/enable", 1) < 0) { 477 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed22: %s, " 478 "i = %d, iio_buf_size = %d\n", strerror(errno), i, iio_buf_size); 479 } else { 480 ALOGI("CwMcuSensor::CwMcuSensor: set IIO buffer length success: %d\n", iio_buf_size); 481 break; 482 } 483 } 484 iio_buf_size /= 2; 485 } 486 487 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_en"); 488 fd = open(fixed_sysfs_path, O_RDWR); 489 if (fd >= 0) { 490 static const char buf[] = "12"; 491 492 rc = write(fd, buf, sizeof(buf) - 1); 493 if (rc < 0) { 494 ALOGE("%s: write buf = %s, failed: %s", __func__, buf, strerror(errno)); 495 } 496 497 close(fd); 498 } else { 499 ALOGE("%s open %s failed: %s", __func__, fixed_sysfs_path, strerror(errno)); 500 } 501 502 pthread_mutex_unlock(&sys_fs_mutex); 503 504 ALOGV("%s: data_fd = %d", __func__, data_fd); 505 ALOGV("%s: iio_device_path = %s", __func__, buffer_access); 506 ALOGV("%s: ctrl sysfs_path = %s", __func__, fixed_sysfs_path); 507 508 setEnable(0, 1); // Inside this function call, we use sys_fs_mutex 509 } 510 511 int gs_temp_data[G_SENSOR_CALIBRATION_DATA_SIZE] = {0}; 512 int compass_temp_data[COMPASS_CALIBRATION_DATA_SIZE] = {0}; 513 514 515 ALOGV("%s: 22 Before pthread_mutex_lock()\n", __func__); 516 pthread_mutex_lock(&sys_fs_mutex); 517 ALOGV("%s: 22 Acquired pthread_mutex_lock()\n", __func__); 518 519 //Sensor Calibration init . Waiting for firmware ready 520 rc = cw_read_calibrator_file(CW_MAGNETIC, SAVE_PATH_MAG, compass_temp_data); 521 if (rc == 0) { 522 ALOGD("Get compass calibration data from data/misc/ x is %d ,y is %d ,z is %d\n", 523 compass_temp_data[0], compass_temp_data[1], compass_temp_data[2]); 524 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_mag"); 525 cw_save_calibrator_file(CW_MAGNETIC, fixed_sysfs_path, compass_temp_data); 526 } else { 527 ALOGI("Compass calibration data does not exist\n"); 528 } 529 530 rc = cw_read_calibrator_file(CW_ACCELERATION, SAVE_PATH_ACC, gs_temp_data); 531 if (rc == 0) { 532 ALOGD("Get g-sensor user calibration data from data/misc/ x is %d ,y is %d ,z is %d\n", 533 gs_temp_data[0],gs_temp_data[1],gs_temp_data[2]); 534 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_acc"); 535 if(!(gs_temp_data[0] == 0 && gs_temp_data[1] == 0 && gs_temp_data[2] == 0 )) { 536 cw_save_calibrator_file(CW_ACCELERATION, fixed_sysfs_path, gs_temp_data); 537 } 538 } else { 539 ALOGI("G-Sensor user calibration data does not exist\n"); 540 } 541 542 pthread_mutex_unlock(&sys_fs_mutex); 543 544 pthread_create(&sync_time_thread, (const pthread_attr_t *) NULL, 545 sync_time_thread_run, (void *)this); 546 547 } 548 549 CwMcuSensor::~CwMcuSensor() { 550 if (!mEnabled.isEmpty()) { 551 setEnable(0, 0); 552 } 553 } 554 555 float CwMcuSensor::indexToValue(size_t index) const { 556 static const float luxValues[LIGHTSENSOR_LEVEL] = { 557 0.0, 10.0, 40.0, 90.0, 160.0, 558 225.0, 320.0, 640.0, 1280.0, 559 2600.0 560 }; 561 562 const size_t maxIndex = (LIGHTSENSOR_LEVEL - 1); 563 if (index > maxIndex) { 564 index = maxIndex; 565 } 566 return luxValues[index]; 567 } 568 569 int CwMcuSensor::find_handle(int32_t sensors_id) { 570 switch (sensors_id) { 571 case CW_ACCELERATION: 572 return ID_A; 573 case CW_MAGNETIC: 574 return ID_M; 575 case CW_GYRO: 576 return ID_GY; 577 case CW_PRESSURE: 578 return ID_PS; 579 case CW_ORIENTATION: 580 return ID_O; 581 case CW_ROTATIONVECTOR: 582 return ID_RV; 583 case CW_LINEARACCELERATION: 584 return ID_LA; 585 case CW_GRAVITY: 586 return ID_G; 587 case CW_MAGNETIC_UNCALIBRATED: 588 return ID_CW_MAGNETIC_UNCALIBRATED; 589 case CW_GYROSCOPE_UNCALIBRATED: 590 return ID_CW_GYROSCOPE_UNCALIBRATED; 591 case CW_GAME_ROTATION_VECTOR: 592 return ID_CW_GAME_ROTATION_VECTOR; 593 case CW_GEOMAGNETIC_ROTATION_VECTOR: 594 return ID_CW_GEOMAGNETIC_ROTATION_VECTOR; 595 case CW_LIGHT: 596 return ID_L; 597 case CW_SIGNIFICANT_MOTION: 598 return ID_CW_SIGNIFICANT_MOTION; 599 case CW_STEP_DETECTOR: 600 return ID_CW_STEP_DETECTOR; 601 case CW_STEP_COUNTER: 602 return ID_CW_STEP_COUNTER; 603 case CW_ACCELERATION_W: 604 return ID_A_W; 605 case CW_MAGNETIC_W: 606 return ID_M_W; 607 case CW_GYRO_W: 608 return ID_GY_W; 609 case CW_PRESSURE_W: 610 return ID_PS_W; 611 case CW_ORIENTATION_W: 612 return ID_O_W; 613 case CW_ROTATIONVECTOR_W: 614 return ID_RV_W; 615 case CW_LINEARACCELERATION_W: 616 return ID_LA_W; 617 case CW_GRAVITY_W: 618 return ID_G_W; 619 case CW_MAGNETIC_UNCALIBRATED_W: 620 return ID_CW_MAGNETIC_UNCALIBRATED_W; 621 case CW_GYROSCOPE_UNCALIBRATED_W: 622 return ID_CW_GYROSCOPE_UNCALIBRATED_W; 623 case CW_GAME_ROTATION_VECTOR_W: 624 return ID_CW_GAME_ROTATION_VECTOR_W; 625 case CW_GEOMAGNETIC_ROTATION_VECTOR_W: 626 return ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W; 627 case CW_STEP_DETECTOR_W: 628 return ID_CW_STEP_DETECTOR_W; 629 case CW_STEP_COUNTER_W: 630 return ID_CW_STEP_COUNTER_W; 631 default: 632 return 0xFF; 633 } 634 } 635 636 bool CwMcuSensor::is_batch_wake_sensor(int32_t handle) { 637 switch (handle) { 638 case ID_A_W: 639 case ID_M_W: 640 case ID_GY_W: 641 case ID_PS_W: 642 case ID_O_W: 643 case ID_RV_W: 644 case ID_LA_W: 645 case ID_G_W: 646 case ID_CW_MAGNETIC_UNCALIBRATED_W: 647 case ID_CW_GYROSCOPE_UNCALIBRATED_W: 648 case ID_CW_GAME_ROTATION_VECTOR_W: 649 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W: 650 case ID_CW_STEP_DETECTOR_W: 651 case ID_CW_STEP_COUNTER_W: 652 return true; 653 default: 654 return false; 655 } 656 } 657 658 int CwMcuSensor::find_sensor(int32_t handle) { 659 int what = -1; 660 661 switch (handle) { 662 case ID_A: 663 what = CW_ACCELERATION; 664 break; 665 case ID_A_W: 666 what = CW_ACCELERATION_W; 667 break; 668 case ID_M: 669 what = CW_MAGNETIC; 670 break; 671 case ID_M_W: 672 what = CW_MAGNETIC_W; 673 break; 674 case ID_GY: 675 what = CW_GYRO; 676 break; 677 case ID_GY_W: 678 what = CW_GYRO_W; 679 break; 680 case ID_PS: 681 what = CW_PRESSURE; 682 break; 683 case ID_PS_W: 684 what = CW_PRESSURE_W; 685 break; 686 case ID_O: 687 what = CW_ORIENTATION; 688 break; 689 case ID_O_W: 690 what = CW_ORIENTATION_W; 691 break; 692 case ID_RV: 693 what = CW_ROTATIONVECTOR; 694 break; 695 case ID_RV_W: 696 what = CW_ROTATIONVECTOR_W; 697 break; 698 case ID_LA: 699 what = CW_LINEARACCELERATION; 700 break; 701 case ID_LA_W: 702 what = CW_LINEARACCELERATION_W; 703 break; 704 case ID_G: 705 what = CW_GRAVITY; 706 break; 707 case ID_G_W: 708 what = CW_GRAVITY_W; 709 break; 710 case ID_CW_MAGNETIC_UNCALIBRATED: 711 what = CW_MAGNETIC_UNCALIBRATED; 712 break; 713 case ID_CW_MAGNETIC_UNCALIBRATED_W: 714 what = CW_MAGNETIC_UNCALIBRATED_W; 715 break; 716 case ID_CW_GYROSCOPE_UNCALIBRATED: 717 what = CW_GYROSCOPE_UNCALIBRATED; 718 break; 719 case ID_CW_GYROSCOPE_UNCALIBRATED_W: 720 what = CW_GYROSCOPE_UNCALIBRATED_W; 721 break; 722 case ID_CW_GAME_ROTATION_VECTOR: 723 what = CW_GAME_ROTATION_VECTOR; 724 break; 725 case ID_CW_GAME_ROTATION_VECTOR_W: 726 what = CW_GAME_ROTATION_VECTOR_W; 727 break; 728 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR: 729 what = CW_GEOMAGNETIC_ROTATION_VECTOR; 730 break; 731 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W: 732 what = CW_GEOMAGNETIC_ROTATION_VECTOR_W; 733 break; 734 case ID_CW_SIGNIFICANT_MOTION: 735 what = CW_SIGNIFICANT_MOTION; 736 break; 737 case ID_CW_STEP_DETECTOR: 738 what = CW_STEP_DETECTOR; 739 break; 740 case ID_CW_STEP_DETECTOR_W: 741 what = CW_STEP_DETECTOR_W; 742 break; 743 case ID_CW_STEP_COUNTER: 744 what = CW_STEP_COUNTER; 745 break; 746 case ID_CW_STEP_COUNTER_W: 747 what = CW_STEP_COUNTER_W; 748 break; 749 case ID_L: 750 what = CW_LIGHT; 751 break; 752 } 753 754 return what; 755 } 756 757 int CwMcuSensor::getEnable(int32_t handle) { 758 ALOGV("CwMcuSensor::getEnable: handle = %d\n", handle); 759 return 0; 760 } 761 762 int CwMcuSensor::setEnable(int32_t handle, int en) { 763 764 int what; 765 int err = 0; 766 int flags = !!en; 767 int fd; 768 char buf[10]; 769 int temp_data[COMPASS_CALIBRATION_DATA_SIZE]; 770 char value[PROPERTY_VALUE_MAX] = {0}; 771 int rc; 772 773 ALOGV("%s: Before pthread_mutex_lock()\n", __func__); 774 pthread_mutex_lock(&sys_fs_mutex); 775 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__); 776 777 property_get("debug.sensorhal.fill.block", value, "0"); 778 ALOGV("CwMcuSensor::setEnable: debug.sensorhal.fill.block= %s", value); 779 fill_block_debug = atoi(value) == 1; 780 781 what = find_sensor(handle); 782 783 ALOGV("CwMcuSensor::setEnable: " 784 "[v13-Dynamic adjust the IIO buffer], handle = %d, en = %d, what = %d\n", 785 handle, en, what); 786 787 if (uint32_t(what) >= numSensors) { 788 pthread_mutex_unlock(&sys_fs_mutex); 789 return -EINVAL; 790 } 791 792 offset_reset[what] = !!flags; 793 794 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "enable"); 795 fd = open(fixed_sysfs_path, O_RDWR); 796 if (fd >= 0) { 797 int n = snprintf(buf, sizeof(buf), "%d %d\n", what, flags); 798 err = write(fd, buf, min(n, sizeof(buf))); 799 if (err < 0) { 800 ALOGE("%s: write failed: %s", __func__, strerror(errno)); 801 } 802 803 close(fd); 804 805 if (flags) { 806 mEnabled.markBit(what); 807 } else { 808 mEnabled.clearBit(what); 809 } 810 811 if (mEnabled.isEmpty()) { 812 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) { 813 ALOGE("CwMcuSensor::setEnable: set buffer disable failed: %s\n", strerror(errno)); 814 } else { 815 ALOGV("CwMcuSensor::setEnable: set IIO buffer enable = 0\n"); 816 } 817 } 818 } else { 819 ALOGE("%s open failed: %s", __func__, strerror(errno)); 820 } 821 822 823 // Sensor Calibration init. Waiting for firmware ready 824 if (!flags && 825 ((what == CW_MAGNETIC) || 826 (what == CW_ORIENTATION) || 827 (what == CW_ROTATIONVECTOR))) { 828 ALOGV("Save Compass calibration data"); 829 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_mag"); 830 rc = cw_read_calibrator_file(CW_MAGNETIC, fixed_sysfs_path, temp_data); 831 if (rc== 0) { 832 cw_save_calibrator_file(CW_MAGNETIC, SAVE_PATH_MAG, temp_data); 833 } else { 834 ALOGI("Compass calibration data from driver fails\n"); 835 } 836 } 837 838 pthread_mutex_unlock(&sys_fs_mutex); 839 return 0; 840 } 841 842 int CwMcuSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout) 843 { 844 int what; 845 int fd; 846 char buf[32] = {0}; 847 int err; 848 int delay_ms; 849 int timeout_ms; 850 bool dryRun = false; 851 852 ALOGV("CwMcuSensor::batch++: handle = %d, flags = %d, period_ns = %" PRId64 ", timeout = %" PRId64 "\n", 853 handle, flags, period_ns, timeout); 854 855 what = find_sensor(handle); 856 delay_ms = period_ns/NS_PER_MS; // int64_t is being dropped to an int type 857 timeout_ms = timeout/NS_PER_MS; // int64_t is being dropped to an int type 858 859 if(flags & SENSORS_BATCH_DRY_RUN) { 860 dryRun = true; 861 } 862 863 if (uint32_t(what) >= CW_SENSORS_ID_END) { 864 return -EINVAL; 865 } 866 867 if(is_batch_wake_sensor(handle)) { 868 flags |= SENSORS_BATCH_WAKE_UPON_FIFO_FULL; 869 ALOGV("CwMcuSensor::batch: SENSORS_BATCH_WAKE_UPON_FIFO_FULL~!!\n"); 870 } else 871 flags &= ~SENSORS_BATCH_WAKE_UPON_FIFO_FULL; 872 873 switch (what) { 874 case CW_LIGHT: 875 case CW_SIGNIFICANT_MOTION: 876 if (timeout > 0) { 877 ALOGI("CwMcuSensor::batch: handle = %d, not support batch mode", handle); 878 return -EINVAL; 879 } 880 break; 881 default: 882 break; 883 } 884 885 if (dryRun == true) { 886 ALOGV("CwMcuSensor::batch: SENSORS_BATCH_DRY_RUN is set\n"); 887 return 0; 888 } 889 890 ALOGV("%s: Before pthread_mutex_lock()\n", __func__); 891 pthread_mutex_lock(&sys_fs_mutex); 892 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__); 893 894 if (mEnabled.isEmpty()) { 895 int i; 896 int iio_buf_size; 897 898 if (!init_trigger_done) { 899 err = sysfs_set_input_attr("trigger/current_trigger", 900 mTriggerName, strlen(mTriggerName)); 901 if (err < 0) { 902 ALOGE("CwMcuSensor::batch: set current trigger failed: err = %d, strerr() = %s\n", 903 err, strerror(errno)); 904 } else { 905 init_trigger_done = true; 906 } 907 } 908 909 iio_buf_size = IIO_MAX_BUFF_SIZE; 910 for (i = 0; i < IIO_BUF_SIZE_RETRY; i++) { 911 if (sysfs_set_input_attr_by_int("buffer/length", iio_buf_size) < 0) { 912 ALOGE("CwMcuSensor::batch: set IIO buffer length (%d) failed: %s\n", 913 iio_buf_size, strerror(errno)); 914 } else { 915 if (sysfs_set_input_attr_by_int("buffer/enable", 1) < 0) { 916 ALOGE("CwMcuSensor::batch: set IIO buffer enable failed: %s, i = %d, " 917 "iio_buf_size = %d\n", strerror(errno), i , iio_buf_size); 918 } else { 919 ALOGI("CwMcuSensor::batch: set IIO buffer length = %d, success\n", iio_buf_size); 920 break; 921 } 922 } 923 iio_buf_size /= 2; 924 } 925 } 926 927 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "batch_enable"); 928 929 fd = open(fixed_sysfs_path, O_RDWR); 930 if (fd < 0) { 931 err = -errno; 932 } else { 933 int n = snprintf(buf, sizeof(buf), "%d %d %d %d\n", what, flags, delay_ms, timeout_ms); 934 err = write(fd, buf, min(n, sizeof(buf))); 935 if (err < 0) { 936 err = -errno; 937 } else { 938 err = 0; 939 } 940 close(fd); 941 } 942 pthread_mutex_unlock(&sys_fs_mutex); 943 944 ALOGV("CwMcuSensor::batch: fd = %d, sensors_id = %d, flags = %d, delay_ms= %d," 945 " timeout_ms = %d, path = %s, err = %d\n", 946 fd , what, flags, delay_ms, timeout_ms, fixed_sysfs_path, err); 947 948 return err; 949 } 950 951 952 int CwMcuSensor::flush(int handle) 953 { 954 int what; 955 int fd; 956 char buf[10] = {0}; 957 int err; 958 959 what = find_sensor(handle); 960 961 if (uint32_t(what) >= CW_SENSORS_ID_END) { 962 return -EINVAL; 963 } 964 965 ALOGV("%s: Before pthread_mutex_lock()\n", __func__); 966 pthread_mutex_lock(&sys_fs_mutex); 967 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__); 968 969 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "flush"); 970 971 fd = open(fixed_sysfs_path, O_RDWR); 972 if (fd >= 0) { 973 int n = snprintf(buf, sizeof(buf), "%d\n", what); 974 err = write(fd, buf, min(n, sizeof(buf))); 975 if (err < 0) { 976 err = -errno; 977 } else { 978 err = 0; 979 } 980 close(fd); 981 } else { 982 ALOGI("CwMcuSensor::flush: flush not supported\n"); 983 err = -EINVAL; 984 } 985 986 pthread_mutex_unlock(&sys_fs_mutex); 987 ALOGI("CwMcuSensor::flush: fd = %d, sensors_id = %d, path = %s, err = %d\n", 988 fd, what, fixed_sysfs_path, err); 989 return err; 990 } 991 992 993 bool CwMcuSensor::hasPendingEvents() const { 994 return !mPendingMask.isEmpty(); 995 } 996 997 int CwMcuSensor::setDelay(int32_t handle, int64_t delay_ns) { 998 char buf[80]; 999 int fd; 1000 int what; 1001 int rc; 1002 1003 ALOGV("%s: Before pthread_mutex_lock()\n", __func__); 1004 pthread_mutex_lock(&sys_fs_mutex); 1005 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__); 1006 1007 ALOGV("CwMcuSensor::setDelay: handle = %" PRId32 ", delay_ns = %" PRId64 "\n", 1008 handle, delay_ns); 1009 1010 what = find_sensor(handle); 1011 if (uint32_t(what) >= numSensors) { 1012 pthread_mutex_unlock(&sys_fs_mutex); 1013 return -EINVAL; 1014 } 1015 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "delay_ms"); 1016 fd = open(fixed_sysfs_path, O_RDWR); 1017 if (fd >= 0) { 1018 size_t n = snprintf(buf, sizeof(buf), "%d %lld\n", what, (delay_ns/NS_PER_MS)); 1019 write(fd, buf, min(n, sizeof(buf))); 1020 close(fd); 1021 } 1022 1023 pthread_mutex_unlock(&sys_fs_mutex); 1024 return 0; 1025 1026 } 1027 1028 void CwMcuSensor::calculate_rv_4th_element(int sensors_id) { 1029 switch (sensors_id) { 1030 case CW_ROTATIONVECTOR: 1031 case CW_GAME_ROTATION_VECTOR: 1032 case CW_GEOMAGNETIC_ROTATION_VECTOR: 1033 case CW_ROTATIONVECTOR_W: 1034 case CW_GAME_ROTATION_VECTOR_W: 1035 case CW_GEOMAGNETIC_ROTATION_VECTOR_W: 1036 float q0, q1, q2, q3; 1037 1038 q1 = mPendingEvents[sensors_id].data[0]; 1039 q2 = mPendingEvents[sensors_id].data[1]; 1040 q3 = mPendingEvents[sensors_id].data[2]; 1041 1042 q0 = 1 - q1*q1 - q2*q2 - q3*q3; 1043 q0 = (q0 > 0) ? (float)sqrt(q0) : 0; 1044 1045 mPendingEvents[sensors_id].data[3] = q0; 1046 break; 1047 default: 1048 break; 1049 } 1050 } 1051 1052 int CwMcuSensor::readEvents(sensors_event_t* data, int count) { 1053 uint64_t mtimestamp; 1054 1055 if (count < 1) { 1056 return -EINVAL; 1057 } 1058 1059 ALOGD_IF(fill_block_debug == 1, "CwMcuSensor::readEvents: Before fill\n"); 1060 ssize_t n = mInputReader.fill(data_fd); 1061 ALOGD_IF(fill_block_debug == 1, "CwMcuSensor::readEvents: After fill, n = %zd\n", n); 1062 if (n < 0) { 1063 return n; 1064 } 1065 1066 cw_event const* event; 1067 uint8_t data_temp[24]; 1068 int id; 1069 int numEventReceived = 0; 1070 1071 while (count && mInputReader.readEvent(&event)) { 1072 1073 memcpy(data_temp, event->data, sizeof(data_temp)); 1074 1075 id = processEvent(data_temp); 1076 if (id == CW_META_DATA) { 1077 *data++ = mPendingEventsFlush; 1078 count--; 1079 numEventReceived++; 1080 ALOGV("CwMcuSensor::readEvents: metadata = %d\n", mPendingEventsFlush.meta_data.sensor); 1081 } else if ((id == TIME_DIFF_EXHAUSTED) || (id == CW_TIME_BASE)) { 1082 ALOGV("readEvents: id = %d\n", id); 1083 } else { 1084 /*** The algorithm which parsed mcu_time into cpu_time for each event ***/ 1085 uint64_t event_mcu_time = mPendingEvents[id].timestamp; 1086 uint64_t event_cpu_time; 1087 1088 if (event_mcu_time < last_mcu_timestamp[id]) { 1089 ALOGE("Do syncronization due to wrong delta mcu_timestamp\n"); 1090 ALOGE("curr_ts = %" PRIu64 " ns, last_ts = %" PRIu64 " ns", 1091 event_mcu_time, last_mcu_timestamp[id]); 1092 sync_time_thread_in_class(); 1093 } 1094 1095 pthread_mutex_lock(&sync_timestamp_algo_mutex); 1096 1097 if (offset_reset[id]) { 1098 ALOGV("offset changed, id = %d, offset = %" PRId64 "\n", id, time_offset); 1099 offset_reset[id] = false; 1100 event_cpu_time = event_mcu_time + time_offset; 1101 } else { 1102 int64_t event_mcu_diff = (event_mcu_time - last_mcu_timestamp[id]); 1103 int64_t event_cpu_diff = event_mcu_diff * time_slope; 1104 event_cpu_time = last_cpu_timestamp[id] + event_cpu_diff; 1105 } 1106 pthread_mutex_unlock(&sync_timestamp_algo_mutex); 1107 1108 pthread_mutex_lock(&last_timestamp_mutex); 1109 1110 mtimestamp = getTimestamp(); 1111 ALOGV("readEvents: id = %d, accuracy = %d\n" 1112 , id 1113 , mPendingEvents[id].acceleration.status); 1114 ALOGV("readEvents: id = %d," 1115 " mcu_time = %" PRId64 " ms," 1116 " cpu_time = %" PRId64 " ns," 1117 " delta = %" PRId64 " us," 1118 " HALtime = %" PRId64 " ns\n", 1119 id, 1120 event_mcu_time / NS_PER_MS, 1121 event_cpu_time, 1122 (event_cpu_time - last_cpu_timestamp[id]) / NS_PER_US, 1123 mtimestamp); 1124 event_cpu_time = (mtimestamp > event_cpu_time) ? event_cpu_time : mtimestamp; 1125 last_mcu_timestamp[id] = event_mcu_time; 1126 last_cpu_timestamp[id] = event_cpu_time; 1127 pthread_mutex_unlock(&last_timestamp_mutex); 1128 /*** The algorithm which parsed mcu_time into cpu_time for each event ***/ 1129 1130 mPendingEvents[id].timestamp = event_cpu_time; 1131 1132 if (mEnabled.hasBit(id)) { 1133 if (id == CW_SIGNIFICANT_MOTION) { 1134 setEnable(ID_CW_SIGNIFICANT_MOTION, 0); 1135 } 1136 calculate_rv_4th_element(id); 1137 *data++ = mPendingEvents[id]; 1138 count--; 1139 numEventReceived++; 1140 } 1141 } 1142 1143 mInputReader.next(); 1144 } 1145 return numEventReceived; 1146 } 1147 1148 1149 int CwMcuSensor::processEvent(uint8_t *event) { 1150 int sensorsid = 0; 1151 int16_t data[3]; 1152 int16_t bias[3]; 1153 int64_t time; 1154 1155 sensorsid = (int)event[0]; 1156 memcpy(data, &event[1], 6); 1157 memcpy(bias, &event[7], 6); 1158 memcpy(&time, &event[13], 8); 1159 1160 mPendingEvents[sensorsid].timestamp = time * NS_PER_MS; 1161 1162 switch (sensorsid) { 1163 case CW_ORIENTATION: 1164 case CW_ORIENTATION_W: 1165 mPendingMask.markBit(sensorsid); 1166 if ((sensorsid == CW_ORIENTATION) || (sensorsid == CW_ORIENTATION_W)) { 1167 mPendingEvents[sensorsid].orientation.status = bias[0]; 1168 } 1169 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_10; 1170 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_10; 1171 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_10; 1172 break; 1173 case CW_ACCELERATION: 1174 case CW_MAGNETIC: 1175 case CW_GYRO: 1176 case CW_LINEARACCELERATION: 1177 case CW_GRAVITY: 1178 case CW_ACCELERATION_W: 1179 case CW_MAGNETIC_W: 1180 case CW_GYRO_W: 1181 case CW_LINEARACCELERATION_W: 1182 case CW_GRAVITY_W: 1183 mPendingMask.markBit(sensorsid); 1184 if ((sensorsid == CW_MAGNETIC) || (sensorsid == CW_MAGNETIC_W)) { 1185 mPendingEvents[sensorsid].magnetic.status = bias[0]; 1186 ALOGV("CwMcuSensor::processEvent: magnetic accuracy = %d\n", 1187 mPendingEvents[sensorsid].magnetic.status); 1188 } 1189 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_100; 1190 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_100; 1191 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_100; 1192 break; 1193 case CW_PRESSURE: 1194 case CW_PRESSURE_W: 1195 mPendingMask.markBit(sensorsid); 1196 // .pressure is data[0] and the unit is hectopascal (hPa) 1197 mPendingEvents[sensorsid].pressure = ((float)*(int32_t *)(&data[0])) * CONVERT_100; 1198 // data[1] is not used, and data[2] is the temperature 1199 mPendingEvents[sensorsid].data[2] = ((float)data[2]) * CONVERT_100; 1200 break; 1201 case CW_ROTATIONVECTOR: 1202 case CW_GAME_ROTATION_VECTOR: 1203 case CW_GEOMAGNETIC_ROTATION_VECTOR: 1204 case CW_ROTATIONVECTOR_W: 1205 case CW_GAME_ROTATION_VECTOR_W: 1206 case CW_GEOMAGNETIC_ROTATION_VECTOR_W: 1207 mPendingMask.markBit(sensorsid); 1208 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_10000; 1209 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_10000; 1210 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_10000; 1211 break; 1212 case CW_MAGNETIC_UNCALIBRATED: 1213 case CW_GYROSCOPE_UNCALIBRATED: 1214 case CW_MAGNETIC_UNCALIBRATED_W: 1215 case CW_GYROSCOPE_UNCALIBRATED_W: 1216 mPendingMask.markBit(sensorsid); 1217 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_100; 1218 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_100; 1219 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_100; 1220 mPendingEvents[sensorsid].data[3] = (float)bias[0] * CONVERT_100; 1221 mPendingEvents[sensorsid].data[4] = (float)bias[1] * CONVERT_100; 1222 mPendingEvents[sensorsid].data[5] = (float)bias[2] * CONVERT_100; 1223 break; 1224 case CW_SIGNIFICANT_MOTION: 1225 mPendingMask.markBit(sensorsid); 1226 mPendingEvents[sensorsid].data[0] = 1.0; 1227 ALOGV("SIGNIFICANT timestamp = %" PRIu64 "\n", mPendingEvents[sensorsid].timestamp); 1228 break; 1229 case CW_LIGHT: 1230 mPendingMask.markBit(sensorsid); 1231 mPendingEvents[sensorsid].light = indexToValue(data[0]); 1232 break; 1233 case CW_STEP_DETECTOR: 1234 case CW_STEP_DETECTOR_W: 1235 mPendingMask.markBit(sensorsid); 1236 mPendingEvents[sensorsid].data[0] = data[0]; 1237 ALOGV("STEP_DETECTOR, timestamp = %" PRIu64 "\n", mPendingEvents[sensorsid].timestamp); 1238 break; 1239 case CW_STEP_COUNTER: 1240 case CW_STEP_COUNTER_W: 1241 mPendingMask.markBit(sensorsid); 1242 // We use 4 bytes in SensorHUB 1243 mPendingEvents[sensorsid].u64.step_counter = *(uint32_t *)&data[0]; 1244 mPendingEvents[sensorsid].u64.step_counter += 0x100000000LL * (*(uint32_t *)&bias[0]); 1245 ALOGV("processEvent: step counter = %" PRId64 "\n", 1246 mPendingEvents[sensorsid].u64.step_counter); 1247 break; 1248 case CW_META_DATA: 1249 mPendingEventsFlush.meta_data.what = META_DATA_FLUSH_COMPLETE; 1250 mPendingEventsFlush.meta_data.sensor = find_handle(data[0]); 1251 ALOGV("CW_META_DATA: meta_data.sensor = %d, data[0] = %d\n", 1252 mPendingEventsFlush.meta_data.sensor, data[0]); 1253 break; 1254 default: 1255 ALOGW("%s: Unknown sensorsid = %d\n", __func__, sensorsid); 1256 break; 1257 } 1258 1259 return sensorsid; 1260 } 1261 1262 1263 void CwMcuSensor::cw_save_calibrator_file(int type, const char * path, int* str) { 1264 FILE *fp_file; 1265 int i; 1266 int rc; 1267 1268 ALOGV("CwMcuSensor::cw_save_calibrator_file: path = %s\n", path); 1269 1270 fp_file = fopen(path, "w+"); 1271 if (!fp_file) { 1272 ALOGE("CwMcuSensor::cw_save_calibrator_file: open file '%s' failed: %s\n", 1273 path, strerror(errno)); 1274 return; 1275 } 1276 1277 if ((type == CW_GYRO) || (type == CW_ACCELERATION)) { 1278 fprintf(fp_file, "%d %d %d\n", str[0], str[1], str[2]); 1279 } else if(type == CW_MAGNETIC) { 1280 for (i = 0; i < COMPASS_CALIBRATION_DATA_SIZE; i++) { 1281 ALOGV("CwMcuSensor::cw_save_calibrator_file: str[%d] = %d\n", i, str[i]); 1282 rc = fprintf(fp_file, "%d%c", str[i], (i == (COMPASS_CALIBRATION_DATA_SIZE-1)) ? '\n' : ' '); 1283 if (rc < 0) { 1284 ALOGE("CwMcuSensor::cw_save_calibrator_file: fprintf fails, rc = %d\n", rc); 1285 } 1286 } 1287 } 1288 1289 fclose(fp_file); 1290 return; 1291 } 1292 1293 int CwMcuSensor::cw_read_calibrator_file(int type, const char * path, int* str) { 1294 FILE *fp; 1295 int readBytes; 1296 int data[COMPASS_CALIBRATION_DATA_SIZE] = {0}; 1297 unsigned int i; 1298 int my_errno; 1299 1300 ALOGV("CwMcuSensor::cw_read_calibrator_file: path = %s\n", path); 1301 1302 fp = fopen(path, "r"); 1303 if (!fp) { 1304 ALOGE("CwMcuSensor::cw_read_calibrator_file: open file '%s' failed: %s\n", 1305 path, strerror(errno)); 1306 // errno is reset to 0 before return 1307 return -1; 1308 } 1309 1310 if (type == CW_GYRO || type == CW_ACCELERATION) { 1311 readBytes = fscanf(fp, "%d %d %d\n", &str[0], &str[1], &str[2]); 1312 my_errno = errno; 1313 if (readBytes != 3) { 1314 ALOGE("CwMcuSensor::cw_read_calibrator_file: fscanf3, readBytes = %d, strerror = %s\n", readBytes, strerror(my_errno)); 1315 } 1316 1317 } else if (type == CW_MAGNETIC) { 1318 ALOGV("CwMcuSensor::cw_read_calibrator_file: COMPASS_CALIBRATION_DATA_SIZE = %d\n", COMPASS_CALIBRATION_DATA_SIZE); 1319 // COMPASS_CALIBRATION_DATA_SIZE is 26 1320 for (i = 0; i < COMPASS_CALIBRATION_DATA_SIZE; i++) { 1321 readBytes = fscanf(fp, "%d ", &str[i]); 1322 my_errno = errno; 1323 ALOGV("CwMcuSensor::cw_read_calibrator_file: str[%d] = %d\n", i, str[i]); 1324 if (readBytes < 1) { 1325 ALOGE("CwMcuSensor::cw_read_calibrator_file: fscanf26, readBytes = %d, strerror = %s\n", readBytes, strerror(my_errno)); 1326 fclose(fp); 1327 return readBytes; 1328 } 1329 } 1330 } 1331 fclose(fp); 1332 return 0; 1333 } 1334
