diff options
author | Nick Vaccaro <nvaccaro@google.com> | 2014-04-17 19:35:45 +0000 |
---|---|---|
committer | Android Git Automerger <android-git-automerger@android.com> | 2014-04-17 19:35:45 +0000 |
commit | aca08e1f76184fa6305d32a31a1f0d619ac1f6af (patch) | |
tree | d36c196e23b9f8b4ad99d8af3dffae2516d6c37d | |
parent | 7e66e753d95ddc1f90db1c666b559f287a3d75c5 (diff) | |
parent | bccee9300ff0362d6548da6f9961b1362c6824c0 (diff) | |
download | invensense-aca08e1f76184fa6305d32a31a1f0d619ac1f6af.tar.gz |
am bccee930: am dc17e951: Invensense: 6515: update HAL to 5.2.0 RC22
* commit 'bccee9300ff0362d6548da6f9961b1362c6824c0':
Invensense: 6515: update HAL to 5.2.0 RC22
-rw-r--r--[-rwxr-xr-x] | 6515/libsensors_iio/MPLSensor.cpp | 83 | ||||
-rwxr-xr-x[-rw-r--r--] | 6515/libsensors_iio/libmllite.so | bin | 122040 -> 120644 bytes | |||
-rwxr-xr-x[-rw-r--r--] | 6515/libsensors_iio/libmplmpu.so | bin | 212813 -> 207805 bytes | |||
-rwxr-xr-x | 6515/libsensors_iio/software/core/mllite/build/android/libmllite.so | bin | 122040 -> 120644 bytes | |||
-rw-r--r-- | 6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c | 30 | ||||
-rw-r--r-- | 6515/libsensors_iio/software/core/mllite/mpl.c | 2 | ||||
-rwxr-xr-x | 6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so | bin | 212813 -> 207805 bytes | |||
-rw-r--r-- | 6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c | 121 |
8 files changed, 147 insertions, 89 deletions
diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp index 620ea5f..afb6119 100755..100644 --- a/6515/libsensors_iio/MPLSensor.cpp +++ b/6515/libsensors_iio/MPLSensor.cpp @@ -589,9 +589,12 @@ void MPLSensor::enable_iio_sysfs(void) if (tempFp == NULL) { LOGE("HAL:could not open timestamp enable"); } else { - if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { + if(fprintf(tempFp, "%d", 1) < 0) { LOGE("HAL:could not enable timestamp"); } + if(fclose(tempFp) < 0) { + LOGE("HAL:could not close timestamp"); + } } LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", @@ -600,9 +603,12 @@ void MPLSensor::enable_iio_sysfs(void) if (tempFp == NULL) { LOGE("HAL:could not open buffer length"); } else { - if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) { + if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) { LOGE("HAL:could not write buffer length"); } + if (fclose(tempFp) < 0) { + LOGE("HAL:could not close buffer length"); + } } LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", @@ -611,9 +617,12 @@ void MPLSensor::enable_iio_sysfs(void) if (tempFp == NULL) { LOGE("HAL:could not open chip enable"); } else { - if (fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { + if (fprintf(tempFp, "%d", 1) < 0) { LOGE("HAL:could not write chip enable"); } + if (fclose(tempFp) < 0) { + LOGE("HAL:could not close chip enable"); + } } inv_get_iio_device_node(iio_device_node); @@ -809,11 +818,14 @@ void MPLSensor::loadDMP(void) if(fptr == NULL) { LOGE("HAL:could not open dmp_firmware"); } else { - if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) { + if (inv_load_dmp(fptr) < 0) { LOGE("HAL:load DMP failed"); } else { LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); } + if (fclose(fptr) < 0) { + LOGE("HAL:could not close dmp firmware"); + } } } else { LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded"); @@ -837,7 +849,7 @@ void MPLSensor::inv_get_sensors_orientation(void) int om[9]; if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], - &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { + &om[6], &om[7], &om[8]) < 0) { LOGE("HAL:Could not read gyro mounting matrix"); } else { LOGV_IF(EXTRA_VERBOSE, @@ -855,6 +867,9 @@ void MPLSensor::inv_get_sensors_orientation(void) mGyroOrientation[7] = om[7]; mGyroOrientation[8] = om[8]; } + if (fclose(fptr) < 0) { + LOGE("HAL:Could not close gyro mounting matrix"); + } } // get accel orientation @@ -865,7 +880,7 @@ void MPLSensor::inv_get_sensors_orientation(void) int om[9]; if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], - &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { + &om[6], &om[7], &om[8]) < 0) { LOGE("HAL:could not read accel mounting matrix"); } else { LOGV_IF(EXTRA_VERBOSE, @@ -883,6 +898,9 @@ void MPLSensor::inv_get_sensors_orientation(void) mAccelOrientation[7] = om[7]; mAccelOrientation[8] = om[8]; } + if (fclose(fptr) < 0) { + LOGE("HAL:could not close accel mounting matrix"); + } } } @@ -5734,24 +5752,6 @@ int MPLSensor::flush(int handle) LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle); -#if 0 - switch (what) { - case Gyro: - case RawGyro: - case Accelerometer: - case MagneticField: - case RawMagneticField: - case Pressure: - case GameRotationVector: - case StepDetector: - LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle); - break; - default: - LOGE("sensor (handle %d) is not supported in batch or flush mode", handle); - return -EINVAL; - } -#endif - if (((what != StepDetector) && (!(mEnabled & (1 << what)))) || ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) { LOGE_IF(ENG_VERBOSE, "HAL: flush - sensor %s not enabled", sname.string()); @@ -5769,20 +5769,19 @@ int MPLSensor::flush(int handle) status = read_sysfs_int(mpu.flush_batch, &res); if (status < 0) - return status; + LOGE("HAL: flush - error invoking flush_batch"); /* driver returns 0 if FIFO is empty */ /* ensure we return status zero */ if (res == 0) { LOGI("HAL: flush - no data in FIFO"); - status = 0; } mFlushSensorEnabledVector.push_back(handle); LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabledVector=%d res=%d", handle, res); mFlushBatchSet = 0; - return status; + return 0; } int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask) @@ -5945,10 +5944,16 @@ int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count, if (fp == NULL) { LOGE("HAL:cannot open pedometer_steps"); } else { - if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) { + if (fscanf(fp, "%lld\n", &stepCount) < 0) { LOGW("HAL:cannot read pedometer_steps"); + if (fclose(fp) < 0) { + LOGW("HAL:cannot close pedometer_steps"); + } return 0; } + if (fclose(fp) < 0) { + LOGW("HAL:cannot close pedometer_steps"); + } } /* return event onChange only */ @@ -5963,10 +5968,17 @@ int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count, if (fp == NULL) { LOGE("HAL:cannot open pedometer_counter"); } else{ - if (fscanf(fp, "%lld\n", &stepCountTs) < 0 || fclose(fp) < 0) { + if (fscanf(fp, "%lld\n", &stepCountTs) < 0) { LOGE("HAL:cannot read pedometer_counter"); + if (fclose(fp) < 0) { + LOGE("HAL:cannot close pedometer_counter"); + } return 0; } + if (fclose(fp) < 0) { + LOGE("HAL:cannot close pedometer_counter"); + return 0; + } } mScEvents.timestamp = stepCountTs; @@ -6021,9 +6033,12 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) LOGE("HAL:cannot open event_smd"); return 0; } else { - if (fscanf(fp, "%d\n", &significantMotion) < 0 || fclose(fp) < 0) { + if (fscanf(fp, "%d\n", &significantMotion) < 0) { LOGE("HAL:cannot read event_smd"); } + if (fclose(fp) < 0) { + LOGE("HAL:cannot close event_smd"); + } } if(mDmpSignificantMotionEnabled && count > 0) { @@ -6041,6 +6056,9 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) /* reset smd state */ mDmpSignificantMotionEnabled = 0; mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION; + + /* auto disable this sensor */ + enableDmpSignificantMotion(0); } } @@ -6123,11 +6141,14 @@ void MPLSensor::setInitial6QuatValue() if(fptr == NULL) { LOGE("HAL:could not open six_axis_q_value"); } else { - if (fwrite(quat, 1, length, fptr) != length || fclose(fptr) < 0) { + if (fwrite(quat, 1, length, fptr) != length) { LOGE("HAL:write six axis q value failed"); } else { mInitial6QuatValueAvailable = 0; } + if (fclose(fptr) < 0) { + LOGE("HAL:could not close six_axis_q_value"); + } } return; diff --git a/6515/libsensors_iio/libmllite.so b/6515/libsensors_iio/libmllite.so Binary files differindex 4a7e0b8..252aa37 100644..100755 --- a/6515/libsensors_iio/libmllite.so +++ b/6515/libsensors_iio/libmllite.so diff --git a/6515/libsensors_iio/libmplmpu.so b/6515/libsensors_iio/libmplmpu.so Binary files differindex cfd0958..a50bdfb 100644..100755 --- a/6515/libsensors_iio/libmplmpu.so +++ b/6515/libsensors_iio/libmplmpu.so diff --git a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so Binary files differindex 4a7e0b8..252aa37 100755 --- a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so +++ b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c index 7715f47..fc7dc50 100644 --- a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -30,7 +30,15 @@ #define LOADDMP_LOG MPL_LOGI #define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) #define DMP_CODE_SIZE 2950 +#define FIFO_SIZE 1024 +#define RESERVED_SIZE 32 +#define MEMORY_SIZE 1024*4 +#define DMP_SIZE_AVAILABLE (MEMORY_SIZE - FIFO_SIZE - RESERVED_SIZE - DMP_CODE_SIZE) +#if DMP_SIZE_AVAILABLE < 0 +#error "DMP image size is larger than available memory" +#endif static const unsigned char dmpMemory[DMP_CODE_SIZE] = { /* bank # 0 */ @@ -45,9 +53,9 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x36, 0x66, 0x66, 0x66, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x1f, 0xdb, 0xe2, 0xb9, 0x3b, 0x7b, 0x11, 0x7b, 0x32, 0x57, 0x83, 0x7b, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x1f, 0xdb, 0xe2, 0xb9, 0x3b, 0x7b, 0x11, 0x7b, 0x32, 0x57, 0x83, 0x7b, /* bank # 1 */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0xeb, 0x85, 0x3f, 0xae, 0x14, 0x7b, 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, @@ -167,12 +175,12 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0xd8, 0xf1, 0xbb, 0xb3, 0xb7, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xd8, 0xf1, 0xb3, 0xb7, 0xbb, 0x87, 0xaa, 0xd0, 0xc1, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, - 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xf1, 0xb0, 0xb4, 0xb8, 0x87, 0x98, 0xad, 0x20, 0xad, + 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xf1, 0xb0, 0xb4, 0xb8, 0x87, 0x98, 0xaf, 0x20, 0xaf, /* bank # 8 */ - 0x83, 0x9d, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0x8e, 0xb4, 0x94, 0x01, 0x29, 0x51, 0x79, - 0x97, 0xae, 0xf1, 0x28, 0x4c, 0x6c, 0x8d, 0x0c, 0x9e, 0x18, 0x8e, 0x78, 0x87, 0x98, 0xad, 0x20, - 0xad, 0x83, 0x9d, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0xb2, 0x81, 0xb4, 0x94, 0x01, 0x9d, - 0x29, 0x51, 0x79, 0x97, 0xba, 0xa1, 0xf1, 0x28, 0x4c, 0x6c, 0xb0, 0x8d, 0x0c, 0xb6, 0x91, 0x18, + 0x83, 0x9f, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0x8e, 0xb4, 0x94, 0x01, 0x29, 0x51, 0x79, + 0x97, 0xae, 0xf1, 0x28, 0x4c, 0x6c, 0x8f, 0x0c, 0x9e, 0x18, 0x8e, 0x78, 0x87, 0x98, 0xaf, 0x20, + 0xaf, 0x83, 0x9f, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0xb2, 0x81, 0xb4, 0x94, 0x01, 0x9f, + 0x29, 0x51, 0x79, 0x97, 0xba, 0xa1, 0xf1, 0x28, 0x4c, 0x6c, 0xb0, 0x8f, 0x0c, 0xb6, 0x91, 0x18, 0xb2, 0x81, 0x78, 0xf1, 0xb0, 0x87, 0xb4, 0x98, 0xba, 0xab, 0x20, 0xab, 0xb2, 0x86, 0xb6, 0x9b, 0x02, 0xb7, 0x93, 0xf0, 0x11, 0x11, 0x00, 0x84, 0xb6, 0x97, 0x01, 0x29, 0x51, 0x79, 0xb4, 0x97, 0xa4, 0xf1, 0x28, 0x4c, 0x6c, 0x8b, 0x0c, 0xb6, 0x94, 0x18, 0xb2, 0x84, 0x78, 0xbb, 0xa3, 0xb3, @@ -180,15 +188,15 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0xd9, 0xa3, 0xd0, 0xf8, 0xf5, 0x8e, 0x9e, 0xb9, 0xa3, 0x78, 0xf1, 0xb1, 0x8a, 0xb5, 0x93, 0x45, 0xb2, 0x8c, 0x2e, 0xb1, 0x8a, 0x54, 0x83, 0xaa, 0xd0, 0xc6, 0xf5, 0xb2, 0x81, 0xb6, 0x91, 0xa3, 0x78, 0xf1, 0xb1, 0x8a, 0xb5, 0x93, 0x65, 0xb2, 0x8c, 0x2e, 0xb1, 0x8a, 0x74, 0x83, 0xaa, 0xd0, - 0xc7, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xad, 0xc6, 0xf9, 0xa3, 0xb4, 0x9d, 0x41, 0xd9, 0xf1, 0xb1, + 0xc7, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xaf, 0xc6, 0xf9, 0xa3, 0xb4, 0x9f, 0x41, 0xd9, 0xf1, 0xb1, 0x8a, 0xba, 0xa6, 0xc5, 0xc7, 0xf1, 0xb2, 0xb6, 0xb9, 0x86, 0x96, 0xa3, 0x50, 0xb1, 0x83, 0xb5, 0x93, 0x04, 0x2c, 0xf3, 0xb8, 0xa3, 0xd0, 0xf8, 0xf9, 0xd1, 0xdb, 0xf1, 0xb6, 0x9c, 0xb9, 0xa3, - 0xd0, 0x56, 0xd8, 0xf2, 0xb0, 0x83, 0xb4, 0x9d, 0xb8, 0xa3, 0x41, 0xdb, 0xf3, 0x97, 0x49, 0xf1, - 0xb1, 0x83, 0xb6, 0x9c, 0xb9, 0xa3, 0xd0, 0x5e, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xa3, 0xb4, 0x9d, + 0xd0, 0x56, 0xd8, 0xf2, 0xb0, 0x83, 0xb4, 0x9f, 0xb8, 0xa3, 0x41, 0xdb, 0xf3, 0x97, 0x49, 0xf1, + 0xb1, 0x83, 0xb6, 0x9c, 0xb9, 0xa3, 0xd0, 0x5e, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xa3, 0xb4, 0x9f, /* bank # 9 */ 0x41, 0xd9, 0xf3, 0xa3, 0xd0, 0xde, 0xf8, 0xf1, 0xb1, 0x83, 0xb6, 0x96, 0xb9, 0xa3, 0xdb, 0x69, 0xf3, 0xb8, 0xa3, 0xd0, 0xde, 0xf1, 0xb3, 0x86, 0xba, 0xa0, 0xc0, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, - 0xa3, 0xb4, 0x9d, 0x41, 0xd9, 0xd0, 0xde, 0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00, + 0xa3, 0xb4, 0x9f, 0x41, 0xd9, 0xd0, 0xde, 0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xf1, 0xba, 0xa2, 0xd0, 0xfa, 0xf2, 0xab, 0xc2, 0xf8, 0xf9, 0xd1, 0xf1, 0xb9, 0xa4, 0xd9, 0xde, 0xda, 0xf8, 0xd8, 0xf2, 0xbb, 0xaf, 0x80, 0x92, 0x50, 0x8f, 0x0d, 0xdb, 0xf1, 0xb1, 0x84, 0xb5, 0x94, 0x21, 0xd9, 0xf5, 0xb3, 0x85, 0xb7, 0x95, 0xb9, 0xa3, 0x78, 0xf1, 0xb1, 0x80, diff --git a/6515/libsensors_iio/software/core/mllite/mpl.c b/6515/libsensors_iio/software/core/mllite/mpl.c index 085e717..79631e8 100644 --- a/6515/libsensors_iio/software/core/mllite/mpl.c +++ b/6515/libsensors_iio/software/core/mllite/mpl.c @@ -46,7 +46,7 @@ inv_error_t inv_init_mpl(void) return INV_SUCCESS; } -const char ml_ver[] = "InvenSense MA 5.2.0 K RC20"; +const char ml_ver[] = "InvenSense MA 5.2.0 K RC22"; /** * @brief used to get the MPL version. diff --git a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differindex cfd0958..a50bdfb 100755 --- a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so +++ b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c index 32bb7ad..0186041 100644 --- a/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c @@ -62,9 +62,15 @@ static void handle_smd() { static void pedo_print() { - printf("steps=%lld, time=%lld\n", + struct timespec aa; + unsigned long long t; + + clock_gettime(CLOCK_REALTIME, &aa); + t = (unsigned long long)aa.tv_sec * 1000000000 + aa.tv_nsec; + printf("steps=%lld, time=%lld, system=%lld\n", read_sysfs_poslonglong("pedometer_steps", dev_dir_name), - read_sysfs_poslonglong("pedometer_time", dev_dir_name)); + read_sysfs_poslonglong("pedometer_time", dev_dir_name), + t); } struct dmp_struct event_file[] = { @@ -155,11 +161,11 @@ static void inv_set_rate() ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5); ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5); if (has_compass) - ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 50); + ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 10); if (has_pressure) - ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 15); + ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 30); + ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 5); + ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 5); ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5); } @@ -186,7 +192,7 @@ static int setup_offset_and_bias() printf("write accel y offset failed.\n"); ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0); if (ret < 0) - printf("write accel z offset failed.\n"); + printf("write accel z offset failed.\n"); ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0); if (ret < 0) @@ -226,17 +232,17 @@ static void setup_dmp(char *dev_path){ return; ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); if (ret < 0) - return; + return; ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); if (ret < 0) - return; + return; ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); if (ret < 0) return; sprintf(dmp_path, "%s/dmp_firmware", dev_path); if ((fd = fopen(dmp_path, "wb")) < 0 ) { perror("dmp fail"); - } + } inv_load_dmp(fd); fclose(fd); printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); @@ -247,16 +253,12 @@ static void setup_dmp(char *dev_path){ if (ret < 0) return; /* selelct which event to enable and interrupt on/off here */ - //enable_glu(sysfs_path, 0); - ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0); - if (ret < 0) - return; ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1); ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1); ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1); - write_sysfs_int64("pedometer_steps", sysfs_path, 0x3ffffffff); - write_sysfs_int64("pedometer_time", sysfs_path, 0xffffffff); + write_sysfs_int64("pedometer_steps", sysfs_path, 0); + write_sysfs_int64("pedometer_time", sysfs_path, 0); if (ret < 0) return; @@ -316,9 +318,9 @@ static void *get_dmp_event(void *param) { fscanf(fp, "%d\n", &data); event_file[i].action(&event_file[i], data); } - } + } } - + return 0; } @@ -419,14 +421,14 @@ static int enable_enable(int on){ int ret; if (0 == on) { - pthread_mutex_lock(&data_switch_lock); + //pthread_mutex_lock(&data_switch_lock); } ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on); if (ret < 0) printf("write enable failed\n"); if (on) { - pthread_mutex_unlock(&data_switch_lock); + //pthread_mutex_unlock(&data_switch_lock); } return 0; @@ -469,8 +471,8 @@ static void dmp_event_control(on){ inv_set_rate(); //ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 5000); -// ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0); + ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 10000); + ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0); //ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10); if (ret < 0) return; @@ -507,8 +509,8 @@ static int run_enable_sequence() if (!g && !a) a = true; - //g = true; - //a = true; + g = true; + a = true; /*disable the master enable */ enable_enable(0); if(g) { @@ -521,8 +523,8 @@ static int run_enable_sequence() enable_quaternion(1); enable_gyro_output(1); } - // enable_quaternion(0); - // enable_gyro_output(0); + enable_quaternion(1); + enable_gyro_output(0); } else { enable_gyro(0); @@ -531,7 +533,7 @@ static int run_enable_sequence() } if(a) { enable_accel(1); - enable_accel_output(1); + enable_accel_output(0); } else { enable_accel(0); enable_accel_output(0); @@ -542,7 +544,7 @@ static int run_enable_sequence() else enable_compass(0); enable_compass(counter%2); - //enable_compass(0); + enable_compass(0); } if (has_pressure) { if(rand()%2) @@ -550,12 +552,12 @@ static int run_enable_sequence() else enable_pressure(0); enable_pressure(counter%3); - //enable_pressure(0); + enable_pressure(0); } enable_step_detector(1); enable_step_indicator(1); - //enable_step_detector(0); - //enable_step_indicator(0); + enable_step_detector(0); + enable_step_indicator(0); write_dmp_event(0); @@ -567,7 +569,6 @@ static int run_enable_sequence() first_flag = 1; /*enable the master enable */ enable_enable(1); - //write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack"); if (enable_random_delay) random_delay(); else { @@ -600,6 +601,15 @@ static int run_disable_sequence() { sleep(disable_delay); } + if (has_pressure) { + if(rand()%2) + enable_pressure(1); + else + enable_pressure(0); + enable_pressure(counter%3); + enable_pressure(1); + } + return 0; } static int run_dmp_off() { @@ -613,8 +623,7 @@ static int run_dmp_off() { g = true; a = true; -// a = false; -// g = false; + a = false; /*disable the master enable */ enable_enable(0); if(g) { @@ -644,7 +653,7 @@ static int run_dmp_off() { else enable_compass(0); enable_compass(counter%2); - enable_compass(1); + enable_compass(0); } if (has_pressure) { if(rand()%2) @@ -654,22 +663,24 @@ static int run_dmp_off() { enable_pressure(counter%3); enable_pressure(1); } + printf("111111111111111\n"); - write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,100); + write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,15); + write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); first_flag = 1; /*enable the master enable */ - enable_enable(1); - sleep(2); + enable_enable(1); + //sleep(2); return 0; -} +} static void *control_switch(void *param) { while(1) { run_enable_sequence(); - run_dmp_off(); + //run_dmp_off(); printf("sleeping\n"); - sleep(1000); + //sleep(1000); run_disable_sequence(); } return 0; @@ -686,17 +697,21 @@ void get_sensor_data(char *d, short *sensor) static void *read_data(void *param) { char *buffer_access; - char data[1048], *dptr, tmp[24]; + char data[2048], *dptr, tmp[24]; short sensor[3]; int q[3], i, ind, left_over_size, buf_size; int ret, fp,read_size; unsigned short hdr; bool done_flag; + struct timespec ts_1; + unsigned long long t0, t1; + int g_count, sq_count; #define PRESSURE_HDR 0x8000 #define ACCEL_HDR 0x4000 #define GYRO_HDR 0x2000 #define COMPASS_HDR 0x1000 +#define COMPASS_HDR_2 0x1800 #define LPQUAT_HDR 0x0800 #define SIXQUAT_HDR 0x0400 #define PEDQUAT_HDR 0x0200 @@ -720,8 +735,18 @@ static void *read_data(void *param) goto error_open_buffer_access; } ind = 0; - + + clock_gettime(CLOCK_REALTIME, &ts_1); + t0 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; while(1) { + + clock_gettime(CLOCK_REALTIME, &ts_1); + t1 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; + //printf("diff=%lld, a_count=%d, sq_count=%d\n", (t1-t0), g_count, sq_count); + g_count = 0; + sq_count = 0; + t0 = t1; + struct pollfd pfd = { .fd = fp, .events = POLLIN, @@ -731,7 +756,7 @@ static void *read_data(void *param) if (left_over_size > 0) memcpy(data, tmp, left_over_size); dptr = data + left_over_size; - read_size = read(fp, dptr, 1024); + read_size = read(fp, dptr, 2000); printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size); if (read_size <= 0) { printf("Wrong size=%d\n", read_size); @@ -740,7 +765,7 @@ static void *read_data(void *param) } ind = read_size + left_over_size; dptr = data; - printf("ind=%d\n", ind); + //printf("ind=%d\n", ind); buf_size = ind - (dptr - data); done_flag = false; @@ -769,6 +794,7 @@ static void *read_data(void *param) if (buf_size >= 16) { get_sensor_data(dptr, sensor); dptr += 8; + g_count++; printf("G:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); } else done_flag = true; @@ -807,6 +833,7 @@ static void *read_data(void *param) q[1] = *(int *)(dptr); q[2] = *(int *)(dptr + 4); dptr += 8; + sq_count++; printf("SIXQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); } else done_flag = true; @@ -825,6 +852,8 @@ static void *read_data(void *param) printf("emptry marker !!!!!!!!!!!\n"); } else if (hdr == END_MARKER) { printf("end marker !!!!!\n"); + } else if (hdr == COMPASS_HDR_2) { + printf("bad compass data\n"); } else { dptr +=8; printf("%lld\n", *(long long *)dptr); @@ -992,7 +1021,7 @@ int main(int argc, char **argv) goto error_free_buf_dir_name; } /* Setup ring buffer parameters */ - /* length must be even number because iio_store_to_sw_ring is expecting + /* length must be even number because iio_store_to_sw_ring is expecting half pointer to be equal to the read pointer, which is impossible when buflen is odd number. This is actually a bug in the code */ ret = write_sysfs_int("length", buf_dir_name, buf_len*2); |