Invensense sensor code, moved to the open-source side
Change-Id: I616c13fdbc2235e400660f6f7a97f879b30879fc
diff --git a/mlsdk/mlutils/mputest.c b/mlsdk/mlutils/mputest.c
new file mode 100644
index 0000000..ac0fa30
--- /dev/null
+++ b/mlsdk/mlutils/mputest.c
@@ -0,0 +1,1396 @@
+/*
+ $License:
+ Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $
+ *
+ *****************************************************************************/
+
+/*
+ * MPU Self Test functions
+ * Version 2.4
+ * May 13th, 2011
+ */
+
+/**
+ * @defgroup MPU_SELF_TEST
+ * @brief MPU Self Test functions
+ *
+ * These functions provide an in-site test of the MPU 3xxx chips. The main
+ * entry point is the inv_mpu_test function.
+ * This runs the tests (as described in the accompanying documentation) and
+ * writes a configuration file containing initial calibration data.
+ * inv_mpu_test returns INV_SUCCESS if the chip passes the tests.
+ * Otherwise, an error code is returned.
+ * The functions in this file rely on MLSL and MLOS: refer to the MPL
+ * documentation for more information regarding the system interface
+ * files.
+ *
+ * @{
+ * @file mputest.c
+ * @brief MPU Self Test routines for assessing gyro sensor status
+ * after surface mount has happened on the target host platform.
+ */
+
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+#ifdef LINUX
+#include <unistd.h>
+#endif
+
+#include "mpu.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "accel.h"
+#include "mlFIFO.h"
+#include "slave.h"
+#include "ml.h"
+#include "ml_stored_data.h"
+#include "checksum.h"
+
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mpust"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ Defines
+*/
+
+#define VERBOSE_OUT 0
+
+/*#define TRACK_IDS*/
+
+/*--- Test parameters defaults. See set_test_parameters for more details ---*/
+
+#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */
+
+#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */
+#define DEF_GYRO_FULLSCALE (2000)
+#else
+#define DEF_GYRO_FULLSCALE (250)
+#endif
+
+#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE)
+ /* gyro sensitivity LSB/dps */
+#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */
+#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */
+#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS)
+ /* 40 dps in LSBs */
+#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS)
+ /* 0.4 dps-rms in LSB-rms */
+#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */
+#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting
+ data for each axis,
+ multiple of 600ms */
+#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to
+ average from, if applic. */
+
+#define ML_INIT_CAL_LEN (36) /* length in bytes of
+ calibration data file */
+
+/*
+ Macros
+*/
+
+#define CHECK_TEST_ERROR(x) \
+ if (x) { \
+ MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \
+ return (-1); \
+ }
+
+#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f)
+
+#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \
+ (chr)[1]=(unsigned char)(shrt);
+
+#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \
+ (chr)[1]=(unsigned char)(ui>>16); \
+ (chr)[2]=(unsigned char)(ui>>8); \
+ (chr)[3]=(unsigned char)(ui);
+
+#define FLOAT_TO_SHORT(f) ( \
+ (fabs(f-(short)f)>=0.5) ? ( \
+ ((short)f)+(f<0?(-1):(+1))) : \
+ ((short)f) \
+ )
+
+#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1])
+#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0])
+
+#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5]
+
+#define CHECK_NACKS(d) ( \
+ d[0]==0xff && d[1]==0xff && \
+ d[2]==0xff && d[3]==0xff && \
+ d[4]==0xff && d[5]==0xff \
+ )
+
+/*
+ Prototypes
+*/
+
+static inv_error_t test_get_data(
+ void *mlsl_handle,
+ struct mldl_cfg *mputestCfgPtr,
+ short *vals);
+
+/*
+ Types
+*/
+typedef struct {
+ float gyro_sens;
+ int gyro_fs;
+ int packet_thresh;
+ float total_timing_tol;
+ int bias_thresh;
+ float rms_threshSq;
+ float sp_shift_thresh;
+ unsigned int test_time_per_axis;
+ unsigned short accel_samples;
+} tTestSetup;
+
+/*
+ Global variables
+*/
+static unsigned char dataout[20];
+static unsigned char dataStore[ML_INIT_CAL_LEN];
+
+static tTestSetup test_setup = {
+ DEF_GYRO_SENS,
+ DEF_GYRO_FULLSCALE,
+ DEF_PACKET_THRESH,
+ DEF_TOTAL_TIMING_TOL,
+ (int)DEF_BIAS_THRESH,
+ DEF_RMS_THRESH * DEF_RMS_THRESH,
+ DEF_SP_SHIFT_THRESH_CUST,
+ DEF_TEST_TIME_PER_AXIS,
+ DEF_N_ACCEL_SAMPLES
+};
+
+static float adjGyroSens;
+static char a_name[3][2] = {"X", "Y", "Z"};
+
+/*
+ NOTE : modify get_slave_descr parameter below to reflect
+ the DEFAULT accelerometer in use. The accelerometer in use
+ can be modified at run-time using the inv_test_setup_accel API.
+ NOTE : modify the expected z axis orientation (Z axis pointing
+ upward or downward)
+*/
+
+signed char g_z_sign = +1;
+struct mldl_cfg *mputestCfgPtr = NULL;
+
+#ifndef LINUX
+/**
+ * @internal
+ * @brief usec precision sleep function.
+ * @param number of micro seconds (us) to sleep for.
+ */
+static void usleep(unsigned long t)
+{
+ unsigned long start = inv_get_tick_count();
+ while (inv_get_tick_count()-start < t / 1000);
+}
+#endif
+
+/**
+ * @brief Modify the self test limits from their default values.
+ *
+ * @param slave_addr
+ * the slave address the MPU device is setup to respond at.
+ * The default is DEF_MPU_ADDR = 0x68.
+ * @param sensitivity
+ * the read sensitivity of the device in LSB/dps as it is trimmed.
+ * NOTE : if using the self test as part of the MPL, the
+ * sensitivity the different sensitivity trims are already
+ * taken care of.
+ * @param p_thresh
+ * number of packets expected to be received in a 600 ms period.
+ * Depends on the sampling frequency of choice (set by default to
+ * 125 Hz) and low pass filter cut-off frequency selection (set
+ * to 42 Hz).
+ * The default is DEF_PACKET_THRESH = 75 packets.
+ * @param total_time_tol
+ * time skew tolerance, taking into account imprecision in turning
+ * the FIFO on and off and the processor time imprecision (for
+ * 1 GHz processor).
+ * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets.
+ * @param bias_thresh
+ * bias level threshold, the maximun acceptable no motion bias
+ * for a production quality part.
+ * The default is DEF_BIAS_THRESH = 40 dps.
+ * @param rms_thresh
+ * the limit standard deviation (=~ RMS) set to assess whether
+ * the noise level on the part is acceptable.
+ * The default is DEF_RMS_THRESH = 0.2 dps-rms.
+ * @param sp_shift_thresh
+ * the limit shift applicable to the Sense Path self test
+ * calculation.
+ */
+void inv_set_test_parameters(unsigned int slave_addr, float sensitivity,
+ int p_thresh, float total_time_tol,
+ int bias_thresh, float rms_thresh,
+ float sp_shift_thresh,
+ unsigned short accel_samples)
+{
+ mputestCfgPtr->addr = slave_addr;
+ test_setup.gyro_sens = sensitivity;
+ test_setup.gyro_fs = (int)(32768.f / sensitivity);
+ test_setup.packet_thresh = p_thresh;
+ test_setup.total_timing_tol = total_time_tol;
+ test_setup.bias_thresh = bias_thresh;
+ test_setup.rms_threshSq = rms_thresh * rms_thresh;
+ test_setup.sp_shift_thresh = sp_shift_thresh;
+ test_setup.accel_samples = accel_samples;
+}
+
+#define X (0)
+#define Y (1)
+#define Z (2)
+
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+/**
+ * @brief Test the gyroscope sensor.
+ * Implements the core logic of the MPU Self Test.
+ * Produces the PASS/FAIL result. Loads the calculated gyro biases
+ * and temperature datum into the corresponding pointers.
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param gyro_biases
+ * output pointer to store the initial bias calculation provided
+ * by the MPU Self Test. Requires 3 elements for gyro X, Y,
+ * and Z.
+ * @param temp_avg
+ * output pointer to store the initial average temperature as
+ * provided by the MPU Self Test.
+ * @param perform_full_test
+ * If 1:
+ * calculates offset, drive frequency, and noise and compare it
+ * against set thresholds.
+ * Report also the final result using a bit-mask like error code
+ * as explained in return value description.
+ * When 0:
+ * skip the noise and drive frequency calculation and pass/fail
+ * assessment; simply calculates the gyro biases.
+ *
+ * @return 0 on success.
+ * On error, the return value is a bitmask representing:
+ * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively
+ * (decimal values will be 1, 2, 4 respectively).
+ * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively
+ * (decimal values will be 8, 16, 32 respectively).
+ * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively
+ * (decimal values will be 64, 128, 256 respectively).
+ * 9 If any of the RMS noise values is zero, it could be
+ * due to a non-functional gyro or FIFO/register failure.
+ * (decimal value will be 512).
+ * (decimal values will be 1024, 2048, 4096 respectively).
+ */
+int inv_test_gyro_3050(void *mlsl_handle,
+ short gyro_biases[3], short *temp_avg,
+ uint_fast8_t perform_full_test)
+{
+ int retVal = 0;
+ inv_error_t result;
+
+ int total_count = 0;
+ int total_count_axis[3] = {0, 0, 0};
+ int packet_count;
+ short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ unsigned char regs[7];
+
+ int temperature;
+ float Avg[3];
+ float RMS[3];
+ int i, j, tmp;
+ char tmpStr[200];
+
+ temperature = 0;
+
+ /* sample rate = 8ms */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_SMPLRT_DIV, 0x07);
+ CHECK_TEST_ERROR(result);
+
+ regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
+ switch (DEF_GYRO_FULLSCALE) {
+ case 2000:
+ regs[0] |= 0x18;
+ break;
+ case 1000:
+ regs[0] |= 0x10;
+ break;
+ case 500:
+ regs[0] |= 0x08;
+ break;
+ case 250:
+ default:
+ regs[0] |= 0x00;
+ break;
+ }
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_DLPF_FS_SYNC, regs[0]);
+ CHECK_TEST_ERROR(result);
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_INT_CFG, 0x00);
+ CHECK_TEST_ERROR(result);
+
+ /* 1st, timing test */
+ for (j = 0; j < 3; j++) {
+
+ MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
+
+ /* turn on all gyros, use gyro X for clocking
+ Set to Y and Z for 2nd and 3rd iteration */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, j + 1);
+ CHECK_TEST_ERROR(result);
+
+ /* wait for 2 ms after switching clock source */
+ usleep(2000);
+
+ /* we will enable XYZ gyro in FIFO and nothing else */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_EN2, 0x00);
+ CHECK_TEST_ERROR(result);
+ /* enable/reset FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
+ CHECK_TEST_ERROR(result);
+
+ tmp = (int)(test_setup.test_time_per_axis / 600);
+ while (tmp-- > 0) {
+ /* enable XYZ gyro in FIFO and nothing else */
+ result = inv_serial_single_write(mlsl_handle,
+ mputestCfgPtr->addr, MPUREG_FIFO_EN1,
+ BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+ CHECK_TEST_ERROR(result);
+
+ /* wait for 600 ms for data */
+ usleep(600000);
+
+ /* stop storing gyro in the FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_EN1, 0x00);
+ CHECK_TEST_ERROR(result);
+
+ /* Getting number of bytes in FIFO */
+ result = inv_serial_read(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_COUNTH, 2, dataout);
+ CHECK_TEST_ERROR(result);
+ /* number of 6 B packets in the FIFO */
+ packet_count = CHARS_TO_SHORT(dataout) / 6;
+ sprintf(tmpStr, "Packet Count: %d - ", packet_count);
+
+ if ( abs(packet_count - test_setup.packet_thresh)
+ <= /* Within +/- total_timing_tol % range */
+ test_setup.total_timing_tol * test_setup.packet_thresh) {
+ for (i = 0; i < packet_count; i++) {
+ /* getting FIFO data */
+ result = inv_serial_read_fifo(mlsl_handle,
+ mputestCfgPtr->addr, 6, dataout);
+ CHECK_TEST_ERROR(result);
+ x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
+ y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
+ z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n",
+ total_count + i, x[total_count + i],
+ y[total_count + i], z[total_count + i]);
+ }
+ }
+ total_count += packet_count;
+ total_count_axis[j] += packet_count;
+ sprintf(tmpStr, "%sOK", tmpStr);
+ } else {
+ if (perform_full_test)
+ retVal |= 1 << j;
+ sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
+ }
+ MPL_LOGI("%s\n", tmpStr);
+ }
+
+ /* remove gyros from FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_EN1, 0x00);
+ CHECK_TEST_ERROR(result);
+
+ /* Read Temperature */
+ result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_TEMP_OUT_H, 2, dataout);
+ CHECK_TEST_ERROR(result);
+ temperature += (short)CHARS_TO_SHORT(dataout);
+ }
+
+ MPL_LOGI("\n");
+ MPL_LOGI("Total %d samples\n", total_count);
+ MPL_LOGI("\n");
+
+ /* 2nd, check bias from X and Y PLL clock source */
+ tmp = total_count != 0 ? total_count : 1;
+ for (i = 0,
+ Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
+ i < total_count; i++) {
+ Avg[X] += 1.f * x[i] / tmp;
+ Avg[Y] += 1.f * y[i] / tmp;
+ Avg[Z] += 1.f * z[i] / tmp;
+ }
+ MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n",
+ Avg[X], Avg[Y], Avg[Z]);
+ if (VERBOSE_OUT) {
+ MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n",
+ Avg[X] / adjGyroSens,
+ Avg[Y] / adjGyroSens,
+ Avg[Z] / adjGyroSens);
+ }
+ if(perform_full_test) {
+ for (j = 0; j < 3; j++) {
+ if (fabs(Avg[j]) > test_setup.bias_thresh) {
+ MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
+ "(threshold = %d)\n",
+ a_name[j], Avg[j], test_setup.bias_thresh);
+ retVal |= 1 << (3+j);
+ }
+ }
+ }
+
+ /* 3rd, check RMS */
+ if (perform_full_test) {
+ for (i = 0,
+ RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
+ i < total_count; i++) {
+ RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]);
+ RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]);
+ RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]);
+ }
+ for (j = 0; j < 3; j++) {
+ if (RMS[j] > test_setup.rms_threshSq * total_count) {
+ MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold "
+ "(threshold = %.2f)\n",
+ a_name[j], sqrt(RMS[j] / total_count),
+ sqrt(test_setup.rms_threshSq));
+ retVal |= 1 << (6+j);
+ }
+ }
+
+ MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
+ sqrt(RMS[X] / total_count),
+ sqrt(RMS[Y] / total_count),
+ sqrt(RMS[Z] / total_count));
+ if (VERBOSE_OUT) {
+ MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n",
+ RMS[X] / total_count,
+ RMS[Y] / total_count,
+ RMS[Z] / total_count);
+ }
+
+ if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) {
+ /* If any of the RMS noise value returns zero,
+ then we might have dead gyro or FIFO/register failure,
+ the part is sleeping, or the part is not responsive */
+ retVal |= 1 << 9;
+ }
+ }
+
+ /* 4th, temperature average */
+ temperature /= 3;
+ if (VERBOSE_OUT)
+ MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n",
+ SHORT_TO_TEMP_C(temperature), "", "");
+
+ /* load into final storage */
+ *temp_avg = (short)temperature;
+ gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
+ gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
+ gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
+
+ return retVal;
+}
+
+#else /* CONFIG_MPU_SENSORS_MPU3050 */
+
+/**
+ * @brief Test the gyroscope sensor.
+ * Implements the core logic of the MPU Self Test but does not provide
+ * a PASS/FAIL output as in the MPU-3050 implementation.
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param gyro_biases
+ * output pointer to store the initial bias calculation provided
+ * by the MPU Self Test. Requires 3 elements for gyro X, Y,
+ * and Z.
+ * @param temp_avg
+ * output pointer to store the initial average temperature as
+ * provided by the MPU Self Test.
+ *
+ * @return 0 on success.
+ * A non-zero error code on error.
+ */
+int inv_test_gyro_6050(void *mlsl_handle,
+ short gyro_biases[3], short *temp_avg)
+{
+ inv_error_t result;
+
+ int total_count = 0;
+ int total_count_axis[3] = {0, 0, 0};
+ int packet_count;
+ short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+ unsigned char regs[7];
+
+ int temperature = 0;
+ float Avg[3];
+ int i, j, tmp;
+ char tmpStr[200];
+
+ /* sample rate = 8ms */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_SMPLRT_DIV, 0x07);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
+ switch (DEF_GYRO_FULLSCALE) {
+ case 2000:
+ regs[0] |= 0x18;
+ break;
+ case 1000:
+ regs[0] |= 0x10;
+ break;
+ case 500:
+ regs[0] |= 0x08;
+ break;
+ case 250:
+ default:
+ regs[0] |= 0x00;
+ break;
+ }
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_CONFIG, regs[0]);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_INT_ENABLE, 0x00);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* 1st, timing test */
+ for (j = 0; j < 3; j++) {
+ MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
+
+ /* turn on all gyros, use gyro X for clocking
+ Set to Y and Z for 2nd and 3rd iteration */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1));
+#else
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGMT_1, j + 1);
+#endif
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* wait for 2 ms after switching clock source */
+ usleep(2000);
+
+ /* enable/reset FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ tmp = (int)(test_setup.test_time_per_axis / 600);
+ while (tmp-- > 0) {
+ /* enable XYZ gyro in FIFO and nothing else */
+ result = inv_serial_single_write(mlsl_handle,
+ mputestCfgPtr->addr, MPUREG_FIFO_EN,
+ BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* wait for 600 ms for data */
+ usleep(600000);
+ /* stop storing gyro in the FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_EN, 0x00);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ /* Getting number of bytes in FIFO */
+ result = inv_serial_read(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_COUNTH, 2, dataout);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ /* number of 6 B packets in the FIFO */
+ packet_count = CHARS_TO_SHORT(dataout) / 6;
+ sprintf(tmpStr, "Packet Count: %d - ", packet_count);
+
+ if (abs(packet_count - test_setup.packet_thresh)
+ <= /* Within +/- total_timing_tol % range */
+ test_setup.total_timing_tol * test_setup.packet_thresh) {
+ for (i = 0; i < packet_count; i++) {
+ /* getting FIFO data */
+ result = inv_serial_read_fifo(mlsl_handle,
+ mputestCfgPtr->addr, 6, dataout);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
+ y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
+ z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n",
+ total_count + i, x[total_count + i],
+ y[total_count + i], z[total_count + i]);
+ }
+ }
+ total_count += packet_count;
+ total_count_axis[j] += packet_count;
+ sprintf(tmpStr, "%sOK", tmpStr);
+ } else {
+ sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
+ }
+ MPL_LOGI("%s\n", tmpStr);
+ }
+
+ /* remove gyros from FIFO */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_FIFO_EN, 0x00);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* Read Temperature */
+ result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_TEMP_OUT_H, 2, dataout);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ temperature += (short)CHARS_TO_SHORT(dataout);
+ }
+
+ MPL_LOGI("\n");
+ MPL_LOGI("Total %d samples\n", total_count);
+ MPL_LOGI("\n");
+
+ /* 2nd, check bias from X and Y PLL clock source */
+ tmp = total_count != 0 ? total_count : 1;
+ for (i = 0,
+ Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
+ i < total_count; i++) {
+ Avg[X] += 1.f * x[i] / tmp;
+ Avg[Y] += 1.f * y[i] / tmp;
+ Avg[Z] += 1.f * z[i] / tmp;
+ }
+ MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n",
+ Avg[X], Avg[Y], Avg[Z]);
+ if (VERBOSE_OUT) {
+ MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n",
+ Avg[X] / adjGyroSens,
+ Avg[Y] / adjGyroSens,
+ Avg[Z] / adjGyroSens);
+ }
+
+ temperature /= 3;
+ if (VERBOSE_OUT)
+ MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n",
+ SHORT_TO_TEMP_C(temperature), "", "");
+
+ /* load into final storage */
+ *temp_avg = (short)temperature;
+ gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
+ gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
+ gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
+
+ return INV_SUCCESS;
+}
+
+#endif /* CONFIG_MPU_SENSORS_MPU3050 */
+
+#ifdef TRACK_IDS
+/**
+ * @internal
+ * @brief Retrieve the unique MPU device identifier from the internal OTP
+ * bank 0 memory.
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @return 0 on success, a non-zero error code from the serial layer on error.
+ */
+static inv_error_t test_get_mpu_id(void *mlsl_handle)
+{
+ inv_error_t result;
+ unsigned char otp0[8];
+
+
+ result =
+ inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr,
+ (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 |
+ 0x00, 6, otp0);
+ if (result)
+ goto close;
+
+ MPL_LOGI("\n");
+ MPL_LOGI("DIE_ID : %06X\n",
+ ((int)otp0[1] << 8 | otp0[0]) & 0x1fff);
+ MPL_LOGI("WAFER_ID : %06X\n",
+ (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5);
+ MPL_LOGI("A_LOT_ID : %06X\n",
+ ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 |
+ otp0[2]) & 0x3ffff) >> 2);
+ MPL_LOGI("W_LOT_ID : %06X\n",
+ ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2);
+ MPL_LOGI("WP_ID : %06X\n",
+ ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7);
+ MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2);
+ MPL_LOGI("\n");
+
+close:
+ result =
+ inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00);
+ return result;
+}
+#endif /* TRACK_IDS */
+
+/**
+ * @brief If requested via inv_test_setup_accel(), test the accelerometer biases
+ * and calculate the necessary bias correction.
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param bias
+ * output pointer to store the initial bias calculation provided
+ * by the MPU Self Test. Requires 3 elements to store accel X, Y,
+ * and Z axis bias.
+ * @param perform_full_test
+ * If 1:
+ * calculates offsets and noise and compare it against set
+ * thresholds. The final exist status will reflect if any of the
+ * value is outside of the expected range.
+ * When 0;
+ * skip the noise calculation and pass/fail assessment; simply
+ * calculates the accel biases.
+ *
+ * @return 0 on success. A non-zero error code on error.
+ */
+int inv_test_accel(void *mlsl_handle,
+ short *bias, long gravity,
+ uint_fast8_t perform_full_test)
+{
+ int i;
+
+ short *p_vals;
+ float x = 0.f, y = 0.f, z = 0.f, zg = 0.f;
+ float RMS[3];
+ float accelRmsThresh = 1000000.f; /* enourmous so that the test always
+ passes - future deployment */
+ unsigned short res;
+ unsigned long orig_requested_sensors;
+ struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata;
+
+ p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
+
+ /* load the slave descr from the getter */
+ if (mputestPData->accel.get_slave_descr == NULL) {
+ MPL_LOGI("\n");
+ MPL_LOGI("No accelerometer configured\n");
+ return 0;
+ }
+ if (mputestCfgPtr->accel == NULL) {
+ MPL_LOGI("\n");
+ MPL_LOGI("No accelerometer configured\n");
+ return 0;
+ }
+
+ /* resume the accel */
+ orig_requested_sensors = mputestCfgPtr->requested_sensors;
+ mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO;
+ res = inv_mpu_resume(mputestCfgPtr,
+ mlsl_handle, NULL, NULL, NULL,
+ mputestCfgPtr->requested_sensors);
+ if(res != INV_SUCCESS)
+ goto accel_error;
+
+ /* wait at least a sample cycle for the
+ accel data to be retrieved by MPU */
+ inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000);
+
+ /* collect the samples */
+ for(i = 0; i < test_setup.accel_samples; i++) {
+ short *vals = &p_vals[3 * i];
+ if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) {
+ goto accel_error;
+ }
+ x += 1.f * vals[X] / test_setup.accel_samples;
+ y += 1.f * vals[Y] / test_setup.accel_samples;
+ z += 1.f * vals[Z] / test_setup.accel_samples;
+ }
+
+ mputestCfgPtr->requested_sensors = orig_requested_sensors;
+ res = inv_mpu_suspend(mputestCfgPtr,
+ mlsl_handle, NULL, NULL, NULL,
+ INV_ALL_SENSORS);
+ if (res != INV_SUCCESS)
+ goto accel_error;
+
+ MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z);
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n",
+ x / gravity, y / gravity, z / gravity);
+ }
+
+ bias[0] = FLOAT_TO_SHORT(x);
+ bias[1] = FLOAT_TO_SHORT(y);
+ zg = z - g_z_sign * gravity;
+ bias[2] = FLOAT_TO_SHORT(zg);
+
+ MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
+ bias[0], bias[1], bias[2]);
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Accel correct.: "
+ "%+13.3f %+13.3f %+13.3f (gee)\n",
+ 1.f * bias[0] / gravity,
+ 1.f * bias[1] / gravity,
+ 1.f * bias[2] / gravity);
+ }
+
+ if (perform_full_test) {
+ /* accel RMS - for now the threshold is only indicative */
+ for (i = 0,
+ RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
+ i < test_setup.accel_samples; i++) {
+ short *vals = &p_vals[3 * i];
+ RMS[X] += (vals[X] - x) * (vals[X] - x);
+ RMS[Y] += (vals[Y] - y) * (vals[Y] - y);
+ RMS[Z] += (vals[Z] - z) * (vals[Z] - z);
+ }
+ for (i = 0; i < 3; i++) {
+ if (RMS[i] > accelRmsThresh * accelRmsThresh
+ * test_setup.accel_samples) {
+ MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
+ "(threshold = %.2f)\n",
+ a_name[i], sqrt(RMS[i] / test_setup.accel_samples),
+ accelRmsThresh);
+ goto accel_error;
+ }
+ }
+ MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
+ sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES),
+ sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES),
+ sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES));
+ }
+
+ return 0; /* success */
+
+accel_error: /* error */
+ bias[0] = bias[1] = bias[2] = 0;
+ return 1;
+}
+
+/**
+ * @brief an user-space substitute of the power management function(s)
+ * in mldl_cfg.c for self test usage.
+ * Wake up and sleep the device, whether that is MPU3050, MPU6050A2,
+ * or MPU6050B1.
+ * @param mlsl_handle
+ * a file handle for the serial communication port used to
+ * communicate with the MPU device.
+ * @param power_level
+ * the power state to change the device into. Currently only 0 or
+ * 1 are supported, for sleep and full-power respectively.
+ * @return 0 on success; a non-zero error code on error.
+ */
+static inv_error_t inv_device_power_mgmt(void *mlsl_handle,
+ uint_fast8_t power_level)
+{
+ inv_error_t result;
+ static unsigned char pwr_mgm;
+ unsigned char b;
+
+ if (power_level != 0 && power_level != 1) {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+
+ if (power_level) {
+ result = inv_serial_read(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, 1, &pwr_mgm);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* reset */
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET);
+#ifndef CONFIG_MPU_SENSORS_MPU6050A2
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+#endif
+ inv_sleep(5);
+
+ /* re-read power mgmt reg -
+ it may have reset after H_RESET is applied */
+ result = inv_serial_read(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, 1, &b);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ /* wake up */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+ if ((b & BITS_PWRSEL) != BITS_PWRSEL) {
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, BITS_PWRSEL);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+#else
+ if (pwr_mgm & BIT_SLEEP) {
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, 0x00);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+#endif
+ inv_sleep(60);
+
+#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \
+ defined(CONFIG_MPU_SENSORS_MPU6050B1)
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+#endif
+ } else {
+ /* restore the power state the part was found in */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+ if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) {
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, pwr_mgm);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+#else
+ if (pwr_mgm & BIT_SLEEP) {
+ result = inv_serial_single_write(
+ mlsl_handle, mputestCfgPtr->addr,
+ MPUREG_PWR_MGM, pwr_mgm);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+#endif
+ }
+
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief The main entry point of the MPU Self Test, triggering the run of
+ * the single tests, for gyros and accelerometers.
+ * Prepares the MPU for the test, taking the device out of low power
+ * state if necessary, switching the MPU secondary I2C interface into
+ * bypass mode and restoring the original power state at the end of
+ * the test.
+ * This function is also responsible for encoding the output of each
+ * test in the correct format as it is stored on the file/medium of
+ * choice (according to inv_serial_write_cal() function).
+ * The format needs to stay perfectly consistent with the one expected
+ * by the corresponding loader in ml_stored_data.c; currectly the
+ * loaded in use is inv_load_cal_V1 (record type 1 - initial
+ * calibration).
+ *
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param provide_result
+ * If 1:
+ * perform and analyze the offset, drive frequency, and noise
+ * calculation and compare it against set threshouds. Report
+ * also the final result using a bit-mask like error code as
+ * described in the inv_test_gyro() function.
+ * When 0:
+ * skip the noise and drive frequency calculation and pass/fail
+ * assessment. It simply calculates the gyro and accel biases.
+ * NOTE: for MPU6050 devices, this parameter is currently
+ * ignored.
+ *
+ * @return 0 on success. A non-zero error code on error.
+ * Propagates the errors from the tests up to the caller.
+ */
+int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result)
+{
+ int result = 0;
+
+ short temp_avg;
+ short gyro_biases[3] = {0, 0, 0};
+ short accel_biases[3] = {0, 0, 0};
+
+ unsigned long testStart = inv_get_tick_count();
+ long accelSens[3] = {0};
+ int ptr;
+ int tmp;
+ long long lltmp;
+ uint32_t chk;
+
+ MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n",
+ DEF_TEST_TIME_PER_AXIS / 600);
+ MPL_LOGI("\n");
+
+ result = inv_device_power_mgmt(mlsl_handle, TRUE);
+
+#ifdef TRACK_IDS
+ result = test_get_mpu_id(mlsl_handle);
+ if (result != INV_SUCCESS) {
+ MPL_LOGI("Could not read the device's unique ID\n");
+ MPL_LOGI("\n");
+ return result;
+ }
+#endif
+
+ /* adjust the gyro sensitivity according to the gyro_sens_trim value */
+ adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f;
+ test_setup.gyro_fs = (int)(32768.f / adjGyroSens);
+
+ /* collect gyro and temperature data */
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+ result = inv_test_gyro_3050(mlsl_handle,
+ gyro_biases, &temp_avg, provide_result);
+#else
+ MPL_LOGW_IF(provide_result,
+ "Self Test for MPU-6050 devices is for bias correction only: "
+ "no test PASS/FAIL result will be provided\n");
+ result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg);
+#endif
+
+ MPL_LOGI("\n");
+ MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart);
+ if (result)
+ return result;
+
+ /* collect accel data. if this step is skipped,
+ ensure the array still contains zeros. */
+ if (mputestCfgPtr->accel != NULL) {
+ float fs;
+ RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs);
+ accelSens[0] = (long)(32768L / fs);
+ accelSens[1] = (long)(32768L / fs);
+ accelSens[2] = (long)(32768L / fs);
+#if defined CONFIG_MPU_SENSORS_MPU6050B1
+ if (MPL_PROD_KEY(mputestCfgPtr->product_id,
+ mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) {
+ accelSens[2] /= 2;
+ } else {
+ unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim;
+ accelSens[0] /= trim_corr;
+ accelSens[1] /= trim_corr;
+ accelSens[2] /= trim_corr;
+ }
+#endif
+ } else {
+ /* would be 0, but 1 to avoid divide-by-0 below */
+ accelSens[0] = accelSens[1] = accelSens[2] = 1;
+ }
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+ result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
+ provide_result);
+#else
+ result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
+ FALSE);
+#endif
+ if (result)
+ return result;
+
+ result = inv_device_power_mgmt(mlsl_handle, FALSE);
+ if (result)
+ return result;
+
+ ptr = 0;
+ dataStore[ptr++] = 0; /* total len of factory cal */
+ dataStore[ptr++] = 0;
+ dataStore[ptr++] = 0;
+ dataStore[ptr++] = ML_INIT_CAL_LEN;
+ dataStore[ptr++] = 0;
+ dataStore[ptr++] = 5; /* record type 5 - initial calibration */
+
+ tmp = temp_avg; /* temperature */
+ if (tmp < 0) tmp += 2 << 16;
+ USHORT_TO_CHARS(&dataStore[ptr], tmp);
+ ptr += 2;
+
+ /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
+ lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+ lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+ lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+
+ lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+ lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+ lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */
+ if (lltmp < 0) lltmp += 1LL << 32;
+ UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+ ptr += 4;
+
+ /* add a checksum for data */
+ chk = inv_checksum(
+ dataStore + INV_CAL_HDR_LEN,
+ ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
+ UINT_TO_CHARS(&dataStore[ptr], chk);
+ ptr += 4;
+
+ if (ptr != ML_INIT_CAL_LEN) {
+ MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
+ ML_INIT_CAL_LEN, ptr);
+ return -1;
+ }
+
+ return result;
+}
+
+/**
+ * @brief The main test API. Runs the MPU Self Test and, if successful,
+ * stores the encoded initial calibration data on the final storage
+ * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE
+ * define in your mlsl implementation).
+ *
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param provide_result
+ * If 1:
+ * perform and analyze the offset, drive frequency, and noise
+ * calculation and compare it against set threshouds. Report
+ * also the final result using a bit-mask like error code as
+ * described in the inv_test_gyro() function.
+ * When 0:
+ * skip the noise and drive frequency calculation and pass/fail
+ * assessment. It simply calculates the gyro and accel biases.
+ *
+ * @return 0 on success or a non-zero error code from the callees on error.
+ */
+inv_error_t inv_factory_calibrate(void *mlsl_handle,
+ uint_fast8_t provide_result)
+{
+ int result;
+
+ result = inv_mpu_test(mlsl_handle, provide_result);
+ if (provide_result) {
+ MPL_LOGI("\n");
+ if (result == 0) {
+ MPL_LOGI("Test : PASSED\n");
+ } else {
+ MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result);
+ return result; /* abort writing the calibration if the
+ test is not successful */
+ }
+ MPL_LOGI("\n");
+ } else {
+ MPL_LOGI("\n");
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+
+ result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN);
+ if (result) {
+ MPL_LOGI("Error : cannot write calibration on file - error %d\n",
+ result);
+ return result;
+ }
+
+ return INV_SUCCESS;
+}
+
+
+
+/* -----------------------------------------------------------------------
+ accel interface functions
+ -----------------------------------------------------------------------*/
+
+/**
+ * @internal
+ * @brief Reads data for X, Y, and Z axis from the accelerometer device.
+ * Used only if an accelerometer has been setup using the
+ * inv_test_setup_accel() API.
+ * Takes care of the accelerometer endianess according to how the
+ * device has been described in the corresponding accelerometer driver
+ * file.
+ * @param mlsl_handle
+ * serial interface handle to allow serial communication with the
+ * device, both gyro and accelerometer.
+ * @param slave
+ * a pointer to the descriptor of the slave accelerometer device
+ * in use. Contains the necessary information to operate, read,
+ * and communicate with the accelerometer device of choice.
+ * See the declaration of struct ext_slave_descr in mpu.h.
+ * @param pdata
+ * a pointer to the platform info of the slave accelerometer
+ * device in use. Describes how the device is oriented and
+ * mounted on host platform's PCB.
+ * @param vals
+ * output pointer to return the accelerometer's X, Y, and Z axis
+ * sensor data collected.
+ * @return 0 on success or a non-zero error code on error.
+ */
+static inv_error_t test_get_data(
+ void *mlsl_handle,
+ struct mldl_cfg *mputestCfgPtr,
+ short *vals)
+{
+ inv_error_t result;
+ unsigned char data[20];
+ struct ext_slave_descr *slave = mputestCfgPtr->accel;
+#ifndef CONFIG_MPU_SENSORS_MPU3050
+ struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel;
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+ result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23,
+ 6, data);
+#else
+ result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg,
+ slave->read_len, data);
+#endif
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n",
+ ACCEL_UNPACK(data));
+ }
+
+ if (CHECK_NACKS(data)) {
+ MPL_LOGI("Error fetching data from the accelerometer : "
+ "all bytes read 0xff\n");
+ return INV_ERROR_SERIAL_READ;
+ }
+
+ if (slave->endian == EXT_SLAVE_BIG_ENDIAN) {
+ vals[0] = CHARS_TO_SHORT(&data[0]);
+ vals[1] = CHARS_TO_SHORT(&data[2]);
+ vals[2] = CHARS_TO_SHORT(&data[4]);
+ } else {
+ vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]);
+ vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]);
+ vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]);
+ }
+
+ if (VERBOSE_OUT) {
+ MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n",
+ vals[0], vals[1], vals[2]);
+ }
+ return INV_SUCCESS;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+