| /** |
| * Self Test application for Invensense's MPU6050/MPU6500/MPU9150. |
| */ |
| |
| #include <unistd.h> |
| #include <dirent.h> |
| #include <fcntl.h> |
| #include <stdio.h> |
| #include <errno.h> |
| #include <sys/stat.h> |
| #include <stdlib.h> |
| #include <features.h> |
| #include <dirent.h> |
| #include <string.h> |
| #include <poll.h> |
| #include <stddef.h> |
| #include <linux/input.h> |
| #include <time.h> |
| #include <linux/time.h> |
| |
| #include "invensense.h" |
| #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 53395 |
| |
| #define FALSE 0 |
| #define TRUE 1 |
| |
| #define GYRO_PASS_STATUS_BIT 0x01 |
| #define ACCEL_PASS_STATUS_BIT 0x02 |
| #define COMPASS_PASS_STATUS_BIT 0x04 |
| |
| typedef union { |
| long l; |
| int i; |
| } bias_dtype; |
| |
| 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 *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 */ |
| long compass_bias[3]; |
| /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ |
| long gyro_bias[3]; |
| /** Temperature when *gyro_bias was stored. */ |
| long gyro_temp; |
| /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ |
| long accel_bias[3]; |
| /** 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 */ |
| 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 */ |
| static inv_error_t inv_db_save_func(unsigned char *data) |
| { |
| memcpy(data, &save_data, sizeof(save_data)); |
| return INV_SUCCESS; |
| } |
| |
| /** read a sysfs entry that represents an integer */ |
| int read_sysfs_int(char *filename, int *var) |
| { |
| int res=0; |
| FILE *fp; |
| |
| 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!=NULL) { |
| fprintf(fp, "%d\n", data); |
| fclose(fp); |
| } else { |
| LOGE("HAL:ERR open file to write"); |
| res= -1; |
| } |
| return res; |
| } |
| |
| 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 = 0; |
| inv_error_t result; |
| bias_dtype gyro_bias[3]; |
| bias_dtype accel_bias[3]; |
| int axis = 0; |
| size_t packet_sz; |
| int axis_sign = 1; |
| unsigned char *buffer; |
| long timestamp; |
| int temperature = 0; |
| bool compass_present = TRUE; |
| |
| result = inv_init_sysfs_attributes(); |
| if (result) |
| return -1; |
| |
| 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)); |
| |
| // 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); |
| |
| // Power ON MPUxxxx chip |
| 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) { |
| printf("Self-Test:ERR-Couldn't invoke self-test\n"); |
| result = -1; |
| goto free_sysfs_storage; |
| } |
| |
| // 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 { |
| 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"); |
| } |
| |
| 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: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. |
| // We store the bias in raw units scaled by 2^16 |
| save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f); |
| 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_data.gyro_temp = temperature * (1L << 16); |
| save_data.accel_temp = save_data.gyro_temp; |
| |
| // When we read accel bias, the bias is in raw units scaled by 1000. |
| // and it contains the gravity vector. |
| |
| // Find the orientation of the device, by looking for gravity |
| if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) { |
| axis = 1; |
| } |
| if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) { |
| axis = 2; |
| } |
| if (accel_bias[axis].l < 0) { |
| axis_sign = -1; |
| } |
| |
| // Remove gravity, gravity in raw units should be 16384 for a 2g setting. |
| // We read data scaled by 1000, so |
| accel_bias[axis].l -= axis_sign * 4096L * 1000L; |
| |
| // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16 |
| save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f); |
| save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f); |
| 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); |
| #endif |
| |
| // Get size of packet to store. |
| inv_get_mpl_state_size(&packet_sz); |
| |
| // Create place to store data |
| buffer = (unsigned char *)malloc(packet_sz + 10); |
| if (buffer == NULL) { |
| printf("Self-Test:Can't allocate buffer\n"); |
| result = -1; |
| goto free_sysfs_storage; |
| } |
| |
| // Store the data |
| result = inv_save_mpl_states(buffer, packet_sz); |
| if (result) { |
| 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", |
| MLCAL_FILE); |
| result = -1; |
| } |
| } |
| free(buffer); |
| |
| free_sysfs_storage: |
| free(sysfs_names_ptr); |
| return result; |
| } |
| |