summaryrefslogtreecommitdiff
path: root/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
diff options
context:
space:
mode:
Diffstat (limited to '60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c')
-rw-r--r--60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c417
1 files changed, 417 insertions, 0 deletions
diff --git a/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..87ed703
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,417 @@
+/**
+ * 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 {
+ MPL_LOGE("inv_self_test: 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 {
+ MPL_LOGE("inv_self_test: 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 {
+ MPL_LOGE("inv_self_test: 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++) {
+ MPL_LOGE("inv_self_test: 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, &timestamp);
+ 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;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-26 13:51:06 +0000