| /** |
| * Self Test application for Invensense's MPU6050/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" |
| |
| #ifndef ABS |
| #define ABS(x)(((x) >= 0) ? (x) : -(x)) |
| #endif |
| |
| /** Change this key if the data being stored by this file changes */ |
| #define INV_DB_SAVE_KEY 53394 |
| |
| #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; |
| |
| struct inv_sysfs_names_s { |
| char *enable; |
| char *power_state; |
| 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" |
| }; |
| |
| 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]; |
| }; |
| |
| 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; |
| } |
| |
| int inv_sysfs_write(char *filename, long data) |
| { |
| FILE *fp; |
| int count; |
| |
| if (!filename) |
| return -1; |
| fp = fopen(filename, "w"); |
| if (!fp) |
| return -errno; |
| count = fprintf(fp, "%ld", data); |
| fclose(fp); |
| return count; |
| } |
| |
| /** |
| * Main Self test |
| */ |
| int main(int argc, char **argv) |
| { |
| FILE *fptr; |
| int self_test_status; |
| 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; |
| |
| // 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)); |
| |
| // 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 (inv_sysfs_write(mpu.power_state, 1) <0) { |
| printf("ERR- Failed to set power state=1\n"); |
| } else { |
| // Note: Driver turns on power automatically when self-test invoked |
| } |
| |
| 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 (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"); |
| } |
| } else { |
| memset(accel_bias,0, sizeof(accel_bias)); |
| printf("Self-Test:Failed Accel 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"); |
| } |
| } |
| |
| } else { |
| printf("Self-Test:ERR-Couldn't invoke self-test\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"); |
| return -1; |
| } |
| |
| 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); |
| return result; |
| } |
| |