diff options
author | JP Abgrall <jpa@google.com> | 2012-10-03 20:16:57 -0700 |
---|---|---|
committer | JP Abgrall <jpa@google.com> | 2012-10-03 20:16:57 -0700 |
commit | 33ce91b37062fa63af192f5643de93f3beebe854 (patch) | |
tree | ba6a03c7954a32ce23b00c6d46cd3d524f4d7007 /libsensors_iio/software/simple_apps/self_test/inv_self_test.c | |
parent | 64ca18f95225d0a86f7ccfd1d21c23971b9f77ae (diff) | |
download | android_hardware_invensense-33ce91b37062fa63af192f5643de93f3beebe854.tar.gz android_hardware_invensense-33ce91b37062fa63af192f5643de93f3beebe854.tar.bz2 android_hardware_invensense-33ce91b37062fa63af192f5643de93f3beebe854.zip |
MotionApps 5.1.1 release, with MA 5.1.0 for further merge review.cm-10.1.3-RC2cm-10.1.3-RC1cm-10.1.3cm-10.1.2cm-10.1.1cm-10.1.0-RC5cm-10.1.0-RC4cm-10.1.0-RC3cm-10.1.0-RC2cm-10.1.0-RC1cm-10.1.0cm-10.1-M3cm-10.1-M2cm-10.1-M1mr1.1-stagingcm-10.1
1. Removed all #ifdef in HAL's member APIs.
2. Added necessary comments as reference.
3. Made changes for coding style, optimization and so on per prior comments.
4. Now raw/calibrated gyroscope sensors could co-exist
Default sensor would be calibrated gyroscope sensor
for getDefaultSensor() call in Android.
* Correctly handle onPower()/masterEnable().
* Use the support functions for reading/writing sysfs.
1 line instead of 9 all over the place.
* Fix return code for {read,write}_sysfs_int():
was > 0 in case of failure instead of < 0.
Bug: 7211625
Change-Id: Ib49dab8ca0f48f45a2838de72f4f8ac011d0e68f
Diffstat (limited to 'libsensors_iio/software/simple_apps/self_test/inv_self_test.c')
-rw-r--r-- | libsensors_iio/software/simple_apps/self_test/inv_self_test.c | 338 |
1 files changed, 245 insertions, 93 deletions
diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c index 4f9996c..f3eadc4 100644 --- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c +++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c @@ -1,5 +1,5 @@ /** - * Self Test application for Invensense's MPU6050/MPU9150. + * Self Test application for Invensense's MPU6050/MPU6500/MPU9150. */ #include <unistd.h> @@ -22,13 +22,19 @@ #include "ml_math_func.h" #include "storage_manager.h" #include "ml_stored_data.h" +#include "ml_sysfs_helper.h" #ifndef ABS #define ABS(x)(((x) >= 0) ? (x) : -(x)) #endif +//#define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */ + +#define MAX_SYSFS_NAME_LEN (100) +#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) + /** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53394 +#define INV_DB_SAVE_KEY 53395 #define FALSE 0 #define TRUE 1 @@ -38,26 +44,29 @@ #define COMPASS_PASS_STATUS_BIT 0x04 typedef union { - long l; + long l; int i; } bias_dtype; -struct inv_sysfs_names_s { +char *sysfs_names_ptr; + +struct sysfs_attrbs { char *enable; char *power_state; + char *dmp_on; + char *dmp_int_on; char *self_test; char *temperature; - char *accl_bias; -}; - -const struct inv_sysfs_names_s mpu= { - /* MPU6050 & MPU9150 */ - .enable = "/sys/class/invensense/mpu/enable", - .power_state = "/sys/class/invensense/mpu/power_state", - .self_test = "/sys/class/invensense/mpu/self_test", - .temperature = "/sys/class/invensense/mpu/temperature", - .accl_bias = "/sys/class/invensense/mpu/accl_bias" -}; + char *gyro_enable; + char *gyro_x_bias; + char *gyro_y_bias; + char *gyro_z_bias; + char *accel_enable; + char *accel_x_bias; + char *accel_y_bias; + char *accel_z_bias; + char *compass_enable; +} mpu; struct inv_db_save_t { /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ @@ -71,46 +80,123 @@ struct inv_db_save_t { /** Temperature when accel bias was stored. */ long accel_temp; long gyro_temp_slope[3]; + /** Sensor Accuracy */ + int gyro_accuracy; + int accel_accuracy; + int compass_accuracy; }; static struct inv_db_save_t save_data; -/** This function receives the data that was stored in non-volatile memory between power off */ +/** This function receives the data that was stored in non-volatile memory + between power off */ static inv_error_t inv_db_load_func(const unsigned char *data) { memcpy(&save_data, data, sizeof(save_data)); return INV_SUCCESS; } -/** This function returns the data to be stored in non-volatile memory between power off */ +/** This function returns the data to be stored in non-volatile memory between + power off */ static inv_error_t inv_db_save_func(unsigned char *data) { memcpy(data, &save_data, sizeof(save_data)); return INV_SUCCESS; } -int inv_sysfs_write(char *filename, long data) +/** read a sysfs entry that represents an integer */ +int read_sysfs_int(char *filename, int *var) { + int res=0; FILE *fp; - int count; - if (!filename) - return -1; + fp = fopen(filename, "r"); + if (fp != NULL) { + fscanf(fp, "%d\n", var); + fclose(fp); + } else { + LOGE("HAL:ERR open file to read"); + res= -1; + } + return res; +} + +/** write a sysfs entry that represents an integer */ +int write_sysfs_int(char *filename, int data) +{ + int res=0; + FILE *fp; + fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; + if (fp!=NULL) { + fprintf(fp, "%d\n", data); + fclose(fp); + } else { + LOGE("HAL:ERR open file to write"); + res= -1; + } + return res; } -/** - * Main Self test - */ +int inv_init_sysfs_attributes(void) +{ + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char *sptr; + char **dptr; + + sysfs_names_ptr = + (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = sysfs_names_ptr; + if (sptr != NULL) { + dptr = (char**)&mpu; + do { + *dptr++ = sptr; + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < MAX_SYSFS_ATTRB); + } else { + LOGE("HAL:couldn't alloc mem for sysfs paths"); + return -1; + } + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + inv_get_sysfs_path(sysfs_path); + + sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable"); + sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); + sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); + sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); + sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); + + sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); + sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias"); + sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias"); + sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias"); + + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); + sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias"); + sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias"); + sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias"); + + sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable"); + +#if 0 + // test print sysfs paths + dptr = (char**)&mpu; + for (i = 0; i < MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} + +/******************************************************************************* + * M a i n S e l f T e s t + ******************************************************************************/ int main(int argc, char **argv) { FILE *fptr; - int self_test_status; + int self_test_status = 0; inv_error_t result; bias_dtype gyro_bias[3]; bias_dtype accel_bias[3]; @@ -119,76 +205,133 @@ int main(int argc, char **argv) int axis_sign = 1; unsigned char *buffer; long timestamp; - int temperature=0; + int temperature = 0; + bool compass_present = TRUE; + + result = inv_init_sysfs_attributes(); + if (result) + return -1; - // Initialize storage manager inv_init_storage_manager(); // Clear out data. memset(&save_data, 0, sizeof(save_data)); - memset(gyro_bias,0, sizeof(gyro_bias)); - memset(accel_bias,0, sizeof(accel_bias)); + memset(gyro_bias, 0, sizeof(gyro_bias)); + memset(accel_bias, 0, sizeof(accel_bias)); // Register packet to be saved. - result = inv_register_load_store(inv_db_load_func, inv_db_save_func, - sizeof(save_data), - INV_DB_SAVE_KEY); + result = inv_register_load_store( + inv_db_load_func, inv_db_save_func, + sizeof(save_data), INV_DB_SAVE_KEY); // Power ON MPUxxxx chip - if (inv_sysfs_write(mpu.power_state, 1) <0) { - printf("ERR- Failed to set power state=1\n"); + if (write_sysfs_int(mpu.power_state, 1) < 0) { + printf("Self-Test:ERR-Failed to set power state=1\n"); } else { // Note: Driver turns on power automatically when self-test invoked } + // Disable Master enable + if (write_sysfs_int(mpu.enable, 0) < 0) { + printf("Self-Test:ERR-Failed to disable master enable\n"); + } + + // Disable DMP + if (write_sysfs_int(mpu.dmp_on, 0) < 0) { + printf("Self-Test:ERR-Failed to disable DMP\n"); + } + + // Enable Accel + if (write_sysfs_int(mpu.accel_enable, 1) < 0) { + printf("Self-Test:ERR-Failed to enable accel\n"); + } + + // Enable Gyro + if (write_sysfs_int(mpu.gyro_enable, 1) < 0) { + printf("Self-Test:ERR-Failed to enable gyro\n"); + } + + // Enable Compass + if (write_sysfs_int(mpu.compass_enable, 1) < 0) { +#ifdef DEBUG_PRINT + printf("Self-Test:ERR-Failed to enable compass\n"); +#endif + compass_present= FALSE; + } + fptr = fopen(mpu.self_test, "r"); - if (fptr != NULL) { - // Invoke self-test and read gyro bias - fscanf(fptr, "%d,%d,%d,%d", - &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status); - - printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", - (self_test_status & GYRO_PASS_STATUS_BIT), - (self_test_status & ACCEL_PASS_STATUS_BIT) >>1, - (self_test_status & COMPASS_PASS_STATUS_BIT) >>2); - printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); - fclose(fptr); - - if (!(self_test_status & GYRO_PASS_STATUS_BIT)) { - // Reset gyro bias data if gyro self-test failed - memset(gyro_bias,0, sizeof(gyro_bias)); - printf("Self-Test:Failed Gyro self-test\n"); - } + if (!fptr) { + printf("Self-Test:ERR-Couldn't invoke self-test\n"); + result = -1; + goto free_sysfs_storage; + } - if (self_test_status & ACCEL_PASS_STATUS_BIT) { - // Read Accel Bias - fptr= fopen(mpu.accl_bias, "r"); - if (fptr != NULL) { - fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i); - printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); - fclose(fptr); - } else { - printf("Self-Test:ERR-Couldn't read accel bias\n"); - } + // Invoke self-test + fscanf(fptr, "%d", &self_test_status); + if (compass_present == TRUE) { + printf("Self-Test:Self test result- " + "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1, + (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2); + } else { + printf("Self-Test:Self test result- " + "Gyro passed= %x, Accel passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1); + } + fclose(fptr); + + if (self_test_status & GYRO_PASS_STATUS_BIT) { + // Read Gyro Bias + if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 || + read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 || + read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) { + memset(gyro_bias, 0, sizeof(gyro_bias)); + printf("Self-Test:Failed to read Gyro bias\n"); } else { - memset(accel_bias,0, sizeof(accel_bias)); - printf("Self-Test:Failed Accel self-test\n"); + save_data.gyro_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", + gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); +#endif } + } else { + printf("Self-Test:Failed Gyro self-test\n"); + } - // Read temperature - if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT)) - { - fptr= fopen(mpu.temperature, "r"); - if (fptr != NULL) { - fscanf(fptr,"%d %ld", &temperature, ×tamp); - fclose(fptr); - } else { - printf("Self-Test:ERR-Couldn't read temperature\n"); - } - } - + if (self_test_status & ACCEL_PASS_STATUS_BIT) { + // Read Accel Bias + if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 || + read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 || + read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) { + memset(accel_bias,0, sizeof(accel_bias)); + printf("Self-Test:Failed to read Accel bias\n"); + } else { + save_data.accel_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n", + accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); +#endif + } } else { - printf("Self-Test:ERR-Couldn't invoke self-test\n"); + printf("Self-Test:Failed Accel self-test\n"); + } + + if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) { + printf("Self-Test:Failed Gyro and Accel self-test, " + "nothing left to do\n"); + result = -1; + goto free_sysfs_storage; + } + + // Read temperature + fptr= fopen(mpu.temperature, "r"); + if (fptr != NULL) { + fscanf(fptr,"%d %ld", &temperature, ×tamp); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read temperature\n"); } // When we read gyro bias, the bias is in raw units scaled by 1000. @@ -197,7 +340,8 @@ int main(int argc, char **argv) save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f); save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f); - // Save temperature @ time stored. Temperature is in degrees Celsius scaled by 2^16 + // Save temperature @ time stored. + // Temperature is in degrees Celsius scaled by 2^16 save_data.gyro_temp = temperature * (1L << 16); save_data.accel_temp = save_data.gyro_temp; @@ -225,12 +369,16 @@ int main(int argc, char **argv) save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f); #if 1 - printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0], - save_data.accel_bias[1], save_data.accel_bias[2]); - printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0], - save_data.gyro_bias[1], save_data.gyro_bias[2]); - printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp); - printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp); + printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", + save_data.accel_bias[0], save_data.accel_bias[1], + save_data.accel_bias[2]); + printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", + save_data.gyro_bias[0], save_data.gyro_bias[1], + save_data.gyro_bias[2]); + printf("Self-Test:Gyro temperature @ time stored %ld\n", + save_data.gyro_temp); + printf("Self-Test:Accel temperature @ time stored %ld\n", + save_data.accel_temp); #endif // Get size of packet to store. @@ -240,25 +388,29 @@ int main(int argc, char **argv) buffer = (unsigned char *)malloc(packet_sz + 10); if (buffer == NULL) { printf("Self-Test:Can't allocate buffer\n"); - return -1; + result = -1; + goto free_sysfs_storage; } + // Store the data result = inv_save_mpl_states(buffer, packet_sz); if (result) { - result= -1; + result = -1; } else { fptr= fopen(MLCAL_FILE, "wb+"); if (fptr != NULL) { fwrite(buffer, 1, packet_sz, fptr); fclose(fptr); } else { - printf("Self-Test:ERR- Can't open calibration file to write - %s\n", + printf("Self-Test:ERR- Can't open calibration file to write - %s\n", MLCAL_FILE); - result= -1; + result = -1; } } - free(buffer); + +free_sysfs_storage: + free(sysfs_names_ptr); return result; } |