diff options
Diffstat (limited to 'libsensors/software/simple_apps/self_test/inv_self_test.c')
| -rw-r--r-- | libsensors/software/simple_apps/self_test/inv_self_test.c | 264 |
1 files changed, 0 insertions, 264 deletions
diff --git a/libsensors/software/simple_apps/self_test/inv_self_test.c b/libsensors/software/simple_apps/self_test/inv_self_test.c deleted file mode 100644 index 4f9996c..0000000 --- a/libsensors/software/simple_apps/self_test/inv_self_test.c +++ /dev/null @@ -1,264 +0,0 @@ -/** - * 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; -} - |