Official MA 5.1.2 - RC11 Release

Signed-off-by: Rosa Chow <rchow@invensense.com>

1 files changed, 242 insertions, 90 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..2fe2cff 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
@@ -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 {
+        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)
-        return -errno;
-    count = fprintf(fp, "%ld", data);
-    fclose(fp);
-    return count;
+    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;
 }
 
-/**
- *  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 {
+        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;
+    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, &timestamp);
-                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, &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.
@@ -225,12 +368,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 +387,30 @@ 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;
 }