1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/****************************************************************************** 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * MPU Self Test functions 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Version 2.4 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * May 13th, 2011 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup MPU_SELF_TEST 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief MPU Self Test functions 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * These functions provide an in-site test of the MPU 3xxx chips. The main 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * entry point is the inv_mpu_test function. 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This runs the tests (as described in the accompanying documentation) and 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * writes a configuration file containing initial calibration data. 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Otherwise, an error code is returned. 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The functions in this file rely on MLSL and MLOS: refer to the MPL 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * documentation for more information regarding the system interface 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * files. 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file mputest.c 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief MPU Self Test routines for assessing gyro sensor status 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * after surface mount has happened on the target host platform. 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <stdio.h> 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <time.h> 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <math.h> 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <stdlib.h> 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef LINUX 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <unistd.h> 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "accel.h" 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "slave.h" 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml_stored_data.h" 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "checksum.h" 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-mpust" 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef __cplusplus 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallextern "C" { 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Defines 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define VERBOSE_OUT 0 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/*#define TRACK_IDS*/ 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/*--- Test parameters defaults. See set_test_parameters for more details ---*/ 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_GYRO_FULLSCALE (2000) 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_GYRO_FULLSCALE (250) 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* gyro sensitivity LSB/dps */ 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 40 dps in LSBs */ 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 0.4 dps-rms in LSB-rms */ 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data for each axis, 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall multiple of 600ms */ 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall average from, if applic. */ 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_INIT_CAL_LEN (36) /* length in bytes of 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calibration data file */ 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Macros 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CHECK_TEST_ERROR(x) \ 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (x) { \ 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (-1); \ 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (chr)[1]=(unsigned char)(shrt); 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (chr)[1]=(unsigned char)(ui>>16); \ 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (chr)[2]=(unsigned char)(ui>>8); \ 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (chr)[3]=(unsigned char)(ui); 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FLOAT_TO_SHORT(f) ( \ 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (fabs(f-(short)f)>=0.5) ? ( \ 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((short)f)+(f<0?(-1):(+1))) : \ 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((short)f) \ 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ) 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CHECK_NACKS(d) ( \ 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d[0]==0xff && d[1]==0xff && \ 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d[2]==0xff && d[3]==0xff && \ 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d[4]==0xff && d[5]==0xff \ 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ) 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Prototypes 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t test_get_data( 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void *mlsl_handle, 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mputestCfgPtr, 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *vals); 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Types 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgralltypedef struct { 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float gyro_sens; 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int gyro_fs; 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int packet_thresh; 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float total_timing_tol; 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bias_thresh; 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float rms_threshSq; 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float sp_shift_thresh; 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int test_time_per_axis; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short accel_samples; 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} tTestSetup; 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Global variables 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned char dataout[20]; 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned char dataStore[ML_INIT_CAL_LEN]; 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic tTestSetup test_setup = { 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_GYRO_SENS, 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_GYRO_FULLSCALE, 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_PACKET_THRESH, 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_TOTAL_TIMING_TOL, 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int)DEF_BIAS_THRESH, 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_RMS_THRESH * DEF_RMS_THRESH, 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_SP_SHIFT_THRESH_CUST, 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_TEST_TIME_PER_AXIS, 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_N_ACCEL_SAMPLES 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic float adjGyroSens; 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic char a_name[3][2] = {"X", "Y", "Z"}; 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall NOTE : modify get_slave_descr parameter below to reflect 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall the DEFAULT accelerometer in use. The accelerometer in use 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall can be modified at run-time using the inv_test_setup_accel API. 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall NOTE : modify the expected z axis orientation (Z axis pointing 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall upward or downward) 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallsigned char g_z_sign = +1; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct mldl_cfg *mputestCfgPtr = NULL; 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifndef LINUX 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief usec precision sleep function. 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param number of micro seconds (us) to sleep for. 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic void usleep(unsigned long t) 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long start = inv_get_tick_count(); 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (inv_get_tick_count()-start < t / 1000); 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Modify the self test limits from their default values. 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param slave_addr 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the slave address the MPU device is setup to respond at. 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The default is DEF_MPU_ADDR = 0x68. 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sensitivity 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the read sensitivity of the device in LSB/dps as it is trimmed. 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * NOTE : if using the self test as part of the MPL, the 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sensitivity the different sensitivity trims are already 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * taken care of. 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param p_thresh 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * number of packets expected to be received in a 600 ms period. 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Depends on the sampling frequency of choice (set by default to 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 125 Hz) and low pass filter cut-off frequency selection (set 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * to 42 Hz). 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The default is DEF_PACKET_THRESH = 75 packets. 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param total_time_tol 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * time skew tolerance, taking into account imprecision in turning 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the FIFO on and off and the processor time imprecision (for 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1 GHz processor). 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param bias_thresh 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bias level threshold, the maximun acceptable no motion bias 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a production quality part. 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The default is DEF_BIAS_THRESH = 40 dps. 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param rms_thresh 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the limit standard deviation (=~ RMS) set to assess whether 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the noise level on the part is acceptable. 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The default is DEF_RMS_THRESH = 0.2 dps-rms. 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sp_shift_thresh 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the limit shift applicable to the Sense Path self test 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculation. 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_set_test_parameters(unsigned int slave_addr, float sensitivity, 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int p_thresh, float total_time_tol, 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bias_thresh, float rms_thresh, 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float sp_shift_thresh, 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short accel_samples) 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->addr = slave_addr; 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.gyro_sens = sensitivity; 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.gyro_fs = (int)(32768.f / sensitivity); 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.packet_thresh = p_thresh; 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.total_timing_tol = total_time_tol; 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.bias_thresh = bias_thresh; 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.rms_threshSq = rms_thresh * rms_thresh; 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.sp_shift_thresh = sp_shift_thresh; 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.accel_samples = accel_samples; 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define X (0) 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define Y (1) 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define Z (2) 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU3050 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Test the gyroscope sensor. 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Implements the core logic of the MPU Self Test. 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Produces the PASS/FAIL result. Loads the calculated gyro biases 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and temperature datum into the corresponding pointers. 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param gyro_biases 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to store the initial bias calculation provided 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by the MPU Self Test. Requires 3 elements for gyro X, Y, 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z. 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param temp_avg 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to store the initial average temperature as 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * provided by the MPU Self Test. 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param perform_full_test 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * If 1: 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculates offset, drive frequency, and noise and compare it 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * against set thresholds. 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Report also the final result using a bit-mask like error code 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * as explained in return value description. 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * When 0: 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * skip the noise and drive frequency calculation and pass/fail 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * assessment; simply calculates the gyro biases. 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success. 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * On error, the return value is a bitmask representing: 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (decimal values will be 1, 2, 4 respectively). 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (decimal values will be 8, 16, 32 respectively). 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (decimal values will be 64, 128, 256 respectively). 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 9 If any of the RMS noise values is zero, it could be 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * due to a non-functional gyro or FIFO/register failure. 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (decimal value will be 512). 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (decimal values will be 1024, 2048, 4096 respectively). 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_test_gyro_3050(void *mlsl_handle, 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short gyro_biases[3], short *temp_avg, 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t perform_full_test) 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int retVal = 0; 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int total_count = 0; 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int total_count_axis[3] = {0, 0, 0}; 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int packet_count; 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[7]; 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int temperature; 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float Avg[3]; 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float RMS[3]; 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j, tmp; 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall char tmpStr[200]; 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temperature = 0; 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* sample rate = 8ms */ 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_SMPLRT_DIV, 0x07); 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (DEF_GYRO_FULLSCALE) { 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 2000: 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x18; 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 1000: 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x10; 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 500: 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x08; 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 250: 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x00; 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_DLPF_FS_SYNC, regs[0]); 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_CFG, 0x00); 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 1st, timing test */ 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 3; j++) { 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* turn on all gyros, use gyro X for clocking 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Set to Y and Z for 2nd and 3rd iteration */ 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, j + 1); 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wait for 2 ms after switching clock source */ 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall usleep(2000); 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* we will enable XYZ gyro in FIFO and nothing else */ 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_EN2, 0x00); 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* enable/reset FIFO */ 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (int)(test_setup.test_time_per_axis / 600); 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (tmp-- > 0) { 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* enable XYZ gyro in FIFO and nothing else */ 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(mlsl_handle, 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->addr, MPUREG_FIFO_EN1, 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wait for 600 ms for data */ 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall usleep(600000); 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* stop storing gyro in the FIFO */ 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_EN1, 0x00); 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Getting number of bytes in FIFO */ 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read( 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_COUNTH, 2, dataout); 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* number of 6 B packets in the FIFO */ 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall packet_count = CHARS_TO_SHORT(dataout) / 6; 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "Packet Count: %d - ", packet_count); 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ( abs(packet_count - test_setup.packet_thresh) 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall <= /* Within +/- total_timing_tol % range */ 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.total_timing_tol * test_setup.packet_thresh) { 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < packet_count; i++) { 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* getting FIFO data */ 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read_fifo(mlsl_handle, 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->addr, 6, dataout); 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count + i, x[total_count + i], 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall y[total_count + i], z[total_count + i]); 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count += packet_count; 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count_axis[j] += packet_count; 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "%sOK", tmpStr); 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (perform_full_test) 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall retVal |= 1 << j; 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("%s\n", tmpStr); 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* remove gyros from FIFO */ 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_EN1, 0x00); 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Read Temperature */ 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_TEMP_OUT_H, 2, dataout); 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CHECK_TEST_ERROR(result); 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temperature += (short)CHARS_TO_SHORT(dataout); 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Total %d samples\n", total_count); 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 2nd, check bias from X and Y PLL clock source */ 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = total_count != 0 ? total_count : 1; 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i < total_count; i++) { 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] += 1.f * x[i] / tmp; 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Y] += 1.f * y[i] / tmp; 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Z] += 1.f * z[i] / tmp; 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X], Avg[Y], Avg[Z]); 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] / adjGyroSens, 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Y] / adjGyroSens, 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Z] / adjGyroSens); 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(perform_full_test) { 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 3; j++) { 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fabs(Avg[j]) > test_setup.bias_thresh) { 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "(threshold = %d)\n", 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a_name[j], Avg[j], test_setup.bias_thresh); 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall retVal |= 1 << (3+j); 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 3rd, check RMS */ 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (perform_full_test) { 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i < total_count; i++) { 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 3; j++) { 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (RMS[j] > test_setup.rms_threshSq * total_count) { 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "(threshold = %.2f)\n", 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a_name[j], sqrt(RMS[j] / total_count), 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(test_setup.rms_threshSq)); 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall retVal |= 1 << (6+j); 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[X] / total_count), 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[Y] / total_count), 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[Z] / total_count)); 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[X] / total_count, 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Y] / total_count, 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Z] / total_count); 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* If any of the RMS noise value returns zero, 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall then we might have dead gyro or FIFO/register failure, 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall the part is sleeping, or the part is not responsive */ 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall retVal |= 1 << 9; 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 4th, temperature average */ 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temperature /= 3; 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SHORT_TO_TEMP_C(temperature), "", ""); 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load into final storage */ 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *temp_avg = (short)temperature; 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return retVal; 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else /* CONFIG_MPU_SENSORS_MPU3050 */ 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Test the gyroscope sensor. 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Implements the core logic of the MPU Self Test but does not provide 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a PASS/FAIL output as in the MPU-3050 implementation. 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param gyro_biases 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to store the initial bias calculation provided 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by the MPU Self Test. Requires 3 elements for gyro X, Y, 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z. 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param temp_avg 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to store the initial average temperature as 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * provided by the MPU Self Test. 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success. 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A non-zero error code on error. 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_test_gyro_6050(void *mlsl_handle, 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short gyro_biases[3], short *temp_avg) 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int total_count = 0; 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int total_count_axis[3] = {0, 0, 0}; 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int packet_count; 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[7]; 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int temperature = 0; 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float Avg[3]; 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j, tmp; 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall char tmpStr[200]; 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* sample rate = 8ms */ 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_SMPLRT_DIV, 0x07); 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (DEF_GYRO_FULLSCALE) { 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 2000: 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x18; 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 1000: 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x10; 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 500: 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x08; 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 250: 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 0x00; 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_CONFIG, regs[0]); 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_ENABLE, 0x00); 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 1st, timing test */ 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 3; j++) { 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* turn on all gyros, use gyro X for clocking 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Set to Y and Z for 2nd and 3rd iteration */ 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU6050A2 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1)); 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGMT_1, j + 1); 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wait for 2 ms after switching clock source */ 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall usleep(2000); 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* enable/reset FIFO */ 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (int)(test_setup.test_time_per_axis / 600); 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (tmp-- > 0) { 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* enable XYZ gyro in FIFO and nothing else */ 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(mlsl_handle, 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->addr, MPUREG_FIFO_EN, 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wait for 600 ms for data */ 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall usleep(600000); 672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* stop storing gyro in the FIFO */ 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_EN, 0x00); 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Getting number of bytes in FIFO */ 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read( 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_COUNTH, 2, dataout); 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* number of 6 B packets in the FIFO */ 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall packet_count = CHARS_TO_SHORT(dataout) / 6; 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "Packet Count: %d - ", packet_count); 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (abs(packet_count - test_setup.packet_thresh) 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall <= /* Within +/- total_timing_tol % range */ 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.total_timing_tol * test_setup.packet_thresh) { 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < packet_count; i++) { 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* getting FIFO data */ 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read_fifo(mlsl_handle, 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->addr, 6, dataout); 699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count + i, x[total_count + i], 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall y[total_count + i], z[total_count + i]); 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count += packet_count; 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall total_count_axis[j] += packet_count; 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "%sOK", tmpStr); 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("%s\n", tmpStr); 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* remove gyros from FIFO */ 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_EN, 0x00); 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Read Temperature */ 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_TEMP_OUT_H, 2, dataout); 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temperature += (short)CHARS_TO_SHORT(dataout); 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Total %d samples\n", total_count); 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 2nd, check bias from X and Y PLL clock source */ 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = total_count != 0 ? total_count : 1; 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i < total_count; i++) { 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] += 1.f * x[i] / tmp; 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Y] += 1.f * y[i] / tmp; 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Z] += 1.f * z[i] / tmp; 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X], Avg[Y], Avg[Z]); 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[X] / adjGyroSens, 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Y] / adjGyroSens, 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Avg[Z] / adjGyroSens); 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temperature /= 3; 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SHORT_TO_TEMP_C(temperature), "", ""); 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load into final storage */ 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *temp_avg = (short)temperature; 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif /* CONFIG_MPU_SENSORS_MPU3050 */ 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef TRACK_IDS 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Retrieve the unique MPU device identifier from the internal OTP 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bank 0 memory. 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success, a non-zero error code from the serial layer on error. 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t test_get_mpu_id(void *mlsl_handle) 789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char otp0[8]; 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, 796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 0x00, 6, otp0); 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) 799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto close; 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("DIE_ID : %06X\n", 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("WAFER_ID : %06X\n", 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); 806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("A_LOT_ID : %06X\n", 807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | 808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall otp0[2]) & 0x3ffff) >> 2); 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("W_LOT_ID : %06X\n", 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("WP_ID : %06X\n", 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); 813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallclose: 817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); 819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif /* TRACK_IDS */ 822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief If requested via inv_test_setup_accel(), test the accelerometer biases 825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and calculate the necessary bias correction. 826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param bias 830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to store the initial bias calculation provided 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by the MPU Self Test. Requires 3 elements to store accel X, Y, 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z axis bias. 833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param perform_full_test 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * If 1: 835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculates offsets and noise and compare it against set 836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * thresholds. The final exist status will reflect if any of the 837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * value is outside of the expected range. 838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * When 0; 839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * skip the noise calculation and pass/fail assessment; simply 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculates the accel biases. 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success. A non-zero error code on error. 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_test_accel(void *mlsl_handle, 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *bias, long gravity, 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t perform_full_test) 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *p_vals; 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; 852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float RMS[3]; 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float accelRmsThresh = 1000000.f; /* enourmous so that the test always 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall passes - future deployment */ 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short res; 856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long orig_requested_sensors; 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load the slave descr from the getter */ 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mputestPData->accel.get_slave_descr == NULL) { 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("No accelerometer configured\n"); 865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mputestCfgPtr->accel == NULL) { 868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("No accelerometer configured\n"); 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* resume the accel */ 874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall orig_requested_sensors = mputestCfgPtr->requested_sensors; 875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; 876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_mpu_resume(mputestCfgPtr, 877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, NULL, NULL, NULL, 878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->requested_sensors); 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(res != INV_SUCCESS) 880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto accel_error; 881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wait at least a sample cycle for the 883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accel data to be retrieved by MPU */ 884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); 885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* collect the samples */ 887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for(i = 0; i < test_setup.accel_samples; i++) { 888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *vals = &p_vals[3 * i]; 889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto accel_error; 891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x += 1.f * vals[X] / test_setup.accel_samples; 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall y += 1.f * vals[Y] / test_setup.accel_samples; 894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall z += 1.f * vals[Z] / test_setup.accel_samples; 895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->requested_sensors = orig_requested_sensors; 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_mpu_suspend(mputestCfgPtr, 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, NULL, NULL, NULL, 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_ALL_SENSORS); 901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res != INV_SUCCESS) 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto accel_error; 903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); 905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x / gravity, y / gravity, z / gravity); 908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[0] = FLOAT_TO_SHORT(x); 911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[1] = FLOAT_TO_SHORT(y); 912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall zg = z - g_z_sign * gravity; 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[2] = FLOAT_TO_SHORT(zg); 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[0], bias[1], bias[2]); 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel correct.: " 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "%+13.3f %+13.3f %+13.3f (gee)\n", 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1.f * bias[0] / gravity, 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1.f * bias[1] / gravity, 922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1.f * bias[2] / gravity); 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (perform_full_test) { 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* accel RMS - for now the threshold is only indicative */ 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, 928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i < test_setup.accel_samples; i++) { 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *vals = &p_vals[3 * i]; 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[X] += (vals[X] - x) * (vals[X] - x); 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Y] += (vals[Y] - y) * (vals[Y] - y); 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RMS[Z] += (vals[Z] - z) * (vals[Z] - z); 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (RMS[i] > accelRmsThresh * accelRmsThresh 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * test_setup.accel_samples) { 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "(threshold = %.2f)\n", 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a_name[i], sqrt(RMS[i] / test_setup.accel_samples), 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelRmsThresh); 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto accel_error; 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; /* success */ 952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallaccel_error: /* error */ 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[0] = bias[1] = bias[2] = 0; 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 1; 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief an user-space substitute of the power management function(s) 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in mldl_cfg.c for self test usage. 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or MPU6050B1. 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a file handle for the serial communication port used to 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * communicate with the MPU device. 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param power_level 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the power state to change the device into. Currently only 0 or 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1 are supported, for sleep and full-power respectively. 969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success; a non-zero error code on error. 970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t inv_device_power_mgmt(void *mlsl_handle, 972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t power_level) 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static unsigned char pwr_mgm; 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char b; 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (power_level != 0 && power_level != 1) { 979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (power_level) { 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read( 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, 1, &pwr_mgm); 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* reset */ 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifndef CONFIG_MPU_SENSORS_MPU6050A2 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_sleep(5); 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* re-read power mgmt reg - 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall it may have reset after H_RESET is applied */ 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read( 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, 1, &b); 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* wake up */ 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU6050A2 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((b & BITS_PWRSEL) != BITS_PWRSEL) { 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, BITS_PWRSEL); 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (pwr_mgm & BIT_SLEEP) { 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, 0x00); 1029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_sleep(60); 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \ 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined(CONFIG_MPU_SENSORS_MPU6050B1) 1039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 1040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* restore the power state the part was found in */ 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU6050A2 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) { 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, pwr_mgm); 1054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (pwr_mgm & BIT_SLEEP) { 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write( 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlsl_handle, mputestCfgPtr->addr, 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_PWR_MGM, pwr_mgm); 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The main entry point of the MPU Self Test, triggering the run of 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the single tests, for gyros and accelerometers. 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Prepares the MPU for the test, taking the device out of low power 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * state if necessary, switching the MPU secondary I2C interface into 1080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bypass mode and restoring the original power state at the end of 1081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the test. 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This function is also responsible for encoding the output of each 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * test in the correct format as it is stored on the file/medium of 1084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * choice (according to inv_serial_write_cal() function). 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The format needs to stay perfectly consistent with the one expected 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by the corresponding loader in ml_stored_data.c; currectly the 1087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * loaded in use is inv_load_cal_V1 (record type 1 - initial 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calibration). 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 1093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param provide_result 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * If 1: 1095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * perform and analyze the offset, drive frequency, and noise 1096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculation and compare it against set threshouds. Report 1097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * also the final result using a bit-mask like error code as 1098895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * described in the inv_test_gyro() function. 1099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * When 0: 1100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * skip the noise and drive frequency calculation and pass/fail 1101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * assessment. It simply calculates the gyro and accel biases. 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * NOTE: for MPU6050 devices, this parameter is currently 1103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * ignored. 1104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success. A non-zero error code on error. 1106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Propagates the errors from the tests up to the caller. 1107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) 1109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result = 0; 1111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short temp_avg; 1113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short gyro_biases[3] = {0, 0, 0}; 1114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short accel_biases[3] = {0, 0, 0}; 1115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long testStart = inv_get_tick_count(); 1117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accelSens[3] = {0}; 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr; 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int tmp; 1120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long lltmp; 1121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t chk; 1122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", 1124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DEF_TEST_TIME_PER_AXIS / 600); 1125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_device_power_mgmt(mlsl_handle, TRUE); 1128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef TRACK_IDS 1130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = test_get_mpu_id(mlsl_handle); 1131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 1132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Could not read the device's unique ID\n"); 1133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* adjust the gyro sensitivity according to the gyro_sens_trim value */ 1139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; 1140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test_setup.gyro_fs = (int)(32768.f / adjGyroSens); 1141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* collect gyro and temperature data */ 1143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU3050 1144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_test_gyro_3050(mlsl_handle, 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyro_biases, &temp_avg, provide_result); 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGW_IF(provide_result, 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "Self Test for MPU-6050 devices is for bias correction only: " 1149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "no test PASS/FAIL result will be provided\n"); 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg); 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); 1155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* collect accel data. if this step is skipped, 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ensure the array still contains zeros. */ 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mputestCfgPtr->accel != NULL) { 1161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float fs; 1162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); 1163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[0] = (long)(32768L / fs); 1164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[1] = (long)(32768L / fs); 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[2] = (long)(32768L / fs); 1166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050B1 1167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (MPL_PROD_KEY(mputestCfgPtr->product_id, 1168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) { 1169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[2] /= 2; 1170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim; 1172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[0] /= trim_corr; 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[1] /= trim_corr; 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[2] /= trim_corr; 1175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* would be 0, but 1 to avoid divide-by-0 below */ 1179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelSens[0] = accelSens[1] = accelSens[2] = 1; 1180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU3050 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], 1183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall provide_result); 1184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], 1186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FALSE); 1187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) 1189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_device_power_mgmt(mlsl_handle, FALSE); 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr = 0; 1196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = 0; /* total len of factory cal */ 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = 0; 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = 0; 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = ML_INIT_CAL_LEN; 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = 0; 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore[ptr++] = 5; /* record type 5 - initial calibration */ 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = temp_avg; /* temperature */ 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) tmp += 2 << 16; 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall USHORT_TO_CHARS(&dataStore[ptr], tmp); 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 2; 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lltmp < 0) lltmp += 1LL << 32; 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* add a checksum for data */ 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk = inv_checksum( 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dataStore + INV_CAL_HDR_LEN, 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall UINT_TO_CHARS(&dataStore[ptr], chk); 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr += 4; 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ptr != ML_INIT_CAL_LEN) { 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", 1244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ML_INIT_CAL_LEN, ptr); 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -1; 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The main test API. Runs the MPU Self Test and, if successful, 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * stores the encoded initial calibration data on the final storage 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * define in your mlsl implementation). 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 1259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param provide_result 1261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * If 1: 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * perform and analyze the offset, drive frequency, and noise 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * calculation and compare it against set threshouds. Report 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * also the final result using a bit-mask like error code as 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * described in the inv_test_gyro() function. 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * When 0: 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * skip the noise and drive frequency calculation and pass/fail 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * assessment. It simply calculates the gyro and accel biases. 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or a non-zero error code from the callees on error. 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_factory_calibrate(void *mlsl_handle, 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t provide_result) 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result; 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_test(mlsl_handle, provide_result); 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (provide_result) { 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == 0) { 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Test : PASSED\n"); 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; /* abort writing the calibration if the 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall test is not successful */ 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("\n"); 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); 1297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Error : cannot write calibration on file - error %d\n", 1299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result); 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ----------------------------------------------------------------------- 1309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accel interface functions 1310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall -----------------------------------------------------------------------*/ 1311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 1314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Reads data for X, Y, and Z axis from the accelerometer device. 1315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Used only if an accelerometer has been setup using the 1316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_test_setup_accel() API. 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Takes care of the accelerometer endianess according to how the 1318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device has been described in the corresponding accelerometer driver 1319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * file. 1320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlsl_handle 1321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * serial interface handle to allow serial communication with the 1322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device, both gyro and accelerometer. 1323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param slave 1324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a pointer to the descriptor of the slave accelerometer device 1325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in use. Contains the necessary information to operate, read, 1326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and communicate with the accelerometer device of choice. 1327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * See the declaration of struct ext_slave_descr in mpu.h. 1328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param pdata 1329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a pointer to the platform info of the slave accelerometer 1330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * device in use. Describes how the device is oriented and 1331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * mounted on host platform's PCB. 1332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param vals 1333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output pointer to return the accelerometer's X, Y, and Z axis 1334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sensor data collected. 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or a non-zero error code on error. 1336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t test_get_data( 1338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void *mlsl_handle, 1339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mputestCfgPtr, 1340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short *vals) 1341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[20]; 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_descr *slave = mputestCfgPtr->accel; 1345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifndef CONFIG_MPU_SENSORS_MPU3050 1346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel; 1347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU3050 1350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, 1351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 6, data); 1352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, 1354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall slave->read_len, data); 1355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 1362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", 1363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ACCEL_UNPACK(data)); 1364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (CHECK_NACKS(data)) { 1367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Error fetching data from the accelerometer : " 1368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "all bytes read 0xff\n"); 1369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SERIAL_READ; 1370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { 1373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[0] = CHARS_TO_SHORT(&data[0]); 1374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[1] = CHARS_TO_SHORT(&data[2]); 1375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[2] = CHARS_TO_SHORT(&data[4]); 1376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); 1378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); 1379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); 1380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (VERBOSE_OUT) { 1383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", 1384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall vals[0], vals[1], vals[2]); 1385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef __cplusplus 1390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 1395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1397