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: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $ 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup ML_STORED_DATA 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file ml_stored_data.c 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief functions for reading and writing stored data sets. 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Typically, these functions process stored calibration data. 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml_stored_data.h" 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mltypes.h" 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpDefault.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlstates.h" 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "checksum.h" 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsupervisor.h" 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-storeload" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Typedefs 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgralltypedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short); 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Globals 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallextern struct inv_obj_t inv_obj; 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Debugging Definitions 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall being read or stored in human-readable format. 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall When set to 0, the compiler will optimize and remove the printouts 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall from the code. 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define LOADCAL_DEBUG 0 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define STORECAL_DEBUG 0 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define LOADCAL_LOG(...) \ 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(LOADCAL_DEBUG) \ 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("LOADCAL: " __VA_ARGS__) 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define STORECAL_LOG(...) \ 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(STORECAL_DEBUG) \ 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("STORECAL: " __VA_ARGS__) 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * advanced algorithms library. To remove cross-dependency, for now, 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * we reimplement the same function here. 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param temp 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the temperature (1 count == 1 degree C). 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint FindTempBin(float temp) 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN); 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (bin < 0) 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin = 0; 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (bin > BINS - 1) 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin = BINS - 1; 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return bin; 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns the length of the <b>MPL internal calibration data</b>. 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Should be called before allocating the memory required to store 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * this data to a file. 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This function returns the total size required to store the cal 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * data including the header (4 bytes) and the checksum (2 bytes). 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre Must be in INV_STATE_DMP_OPENED state. 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_open() or inv_dmp_stop() must have been called. 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * been called. 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return the length of the internal calibrated data format. 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned int inv_get_cal_length(void) 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int length; 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length = INV_CAL_HDR_LEN + // header 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_CAL_ACCEL_LEN + // accel cal 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_CAL_COMPASS_LEN + // compass cal 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_CAL_CHK_LEN; // checksum 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return length; 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 0 set of calibration data. 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature, 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - gyro biases for X, Y, Z axes. 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data would normally be produced by the MPU Self 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Test and its size is 18 bytes (header and checksum included). 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 0 is currently <b>NOT</b> used and 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is substituted by type 5 : inv_load_cal_V5(). 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @note This calibration data format is obsoleted and no longer supported 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by the rest of the MPL 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len) 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const short expLen = 18; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long newGyroData[3] = { 0, 0, 0 }; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float newTemp; 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bin; 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V0\n"); 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newTemp = (float)inv_decode_temperature( 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newTemp = %f\n", newTemp); 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[0] > 32767L) 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[0] -= 65536L; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[1] > 32767L) 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[1] -= 65536L; 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[2] > 32767L) 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[2] -= 65536L; 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin = FindTempBin(newTemp); 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("bin = %d", bin); 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f", 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((float)newGyroData[0]) / 65536.f; 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((float)newGyroData[0]) / 65536.f; 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((float)newGyroData[0]) / 65536.f; 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.temp_ptrs[bin] == 0) 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[bin] = TRUE; 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_valid_data[bin]); 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V0\n"); 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 1 set of calibration data. 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature, 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - gyro biases for X, Y, Z axes, 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - accel biases for X, Y, Z axes. 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data would normally be produced by the MPU Self 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Test and its size is 24 bytes (header and checksum included). 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 1 is currently <b>NOT</b> used and 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is substituted by type 5 : inv_load_cal_V5(). 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @note In order to successfully work, the gyro bias must be stored 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * expressed in 250 dps full scale (131.072 sensitivity). Other full 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * scale range will produce unpredictable results in the gyro biases. 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const short expLen = 24; 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long newGyroData[3] = {0, 0, 0}; 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accelBias[3] = {0, 0, 0}; 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float gyroBias[3] = {0, 0, 0}; 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float newTemp = 0; 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bin; 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V1\n"); 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newTemp = (float)inv_decode_temperature( 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newTemp = %f\n", newTemp); 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[0] > 32767L) 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[0] -= 65536L; 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[1] > 32767L) 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[1] -= 65536L; 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]); 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (newGyroData[2] > 32767L) 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newGyroData[2] -= 65536L; 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin = FindTempBin(newTemp); 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("bin = %d\n", bin); 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroBias[0] = ((float)newGyroData[0]) / 131.072f; 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroBias[1] = ((float)newGyroData[1]) / 131.072f; 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroBias[2] = ((float)newGyroData[2]) / 131.072f; 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]); 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]); 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]); 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f", 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.temp_ptrs[bin] == 0) 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[bin] = TRUE; 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_valid_data[bin]); 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load accel biases and apply immediately */ 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]); 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accelBias[0] > 32767) 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[0] -= 65536L; 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[0] = (long)((long long)accelBias[0] * 65536L * 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens / 1073741824L); 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]); 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]); 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accelBias[1] > 32767) 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[1] -= 65536L; 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[1] = (long)((long long)accelBias[1] * 65536L * 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens / 1073741824L); 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]); 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]); 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accelBias[2] > 32767) 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[2] -= 65536L; 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[2] = (long)((long long)accelBias[2] * 65536L * 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens / 1073741824L); 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_array(INV_ACCEL_BIAS, accelBias); 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V1\n"); 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 2 set of calibration data. 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : temperature data points, 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : gyro biases data points for X, Y, 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z axes. 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - accel biases for X, Y, Z axes. 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data is produced internally by the MPL and its 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * size is 2222 bytes (header and checksum included). 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 2 is currently <b>NOT</b> used and 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is substituted by type 4 : inv_load_cal_V4(). 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const short expLen = 2222; 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accel_bias[3]; 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr = INV_CAL_HDR_LEN; 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j; 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp; 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V2\n"); 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n", 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall expLen, len); 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] = 0; 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] = 0; 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, inv_obj.temp_valid_data[i]); 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f\n", 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.temp_data[i][j]); 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.x_gyro_temp_data[i][j]); 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.y_gyro_temp_data[i][j]); 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.z_gyro_temp_data[i][j]); 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the accel biases */ 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accel_bias[i] = (int32_t) t; 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_array(INV_ACCEL_BIAS, accel_bias); 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V2\n"); 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 3 set of calibration data. 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : temperature data points, 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : gyro biases data points for X, Y, 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z axes. 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - accel biases for X, Y, Z axes. 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - compass biases for X, Y, Z axes and bias tracking algorithm 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * mock-up. 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data is produced internally by the MPL and its 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * size is 2429 bytes (header and checksum included). 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 3 is currently <b>NOT</b> used and 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is substituted by type 4 : inv_load_cal_V4(). 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall union doubleToLongLong { 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double db; 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long long ll; 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } dToLL; 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const short expLen = 2429; 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long bias[3]; 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j; 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr = INV_CAL_HDR_LEN; 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp; 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V3\n"); 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n", 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall expLen, len); 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] = 0; 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] = 0; 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, inv_obj.temp_valid_data[i]); 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f\n", 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.temp_data[i][j]); 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.x_gyro_temp_data[i][j]); 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.y_gyro_temp_data[i][j]); 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.z_gyro_temp_data[i][j]); 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the accel biases */ 645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[i] = (int32_t) t; 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_array(INV_ACCEL_BIAS, bias)) { 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_array(INV_ACCEL_BIAS, bias); 657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the compass biases */ 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_compass_bias = (int)calData[ptr++]; 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_init_compass_bias = (int)calData[ptr++]; 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_state = (int)calData[ptr++]; 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[i] = (int32_t) t; 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, 672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[i]); 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i] = (int32_t) t; 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i]); 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias[i] = (int32_t) t; 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 18; i++) { 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_peaks[i] = (int32_t) t; 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 0; 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_v[i] = dToLL.db; 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 9; i++) { 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 0; 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[i] = dToLL.db; 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[i]); 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V3\n"); 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 4 set of calibration data. 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : temperature data points, 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature compensation : gyro biases data points for X, Y, 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and Z axes. 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - accel biases for X, Y, Z axes. 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - compass biases for X, Y, Z axes, compass scale, and bias 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * tracking algorithm mock-up. 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data is produced internally by the MPL and its 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * size is 2777 bytes (header and checksum included). 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 4 is currently used and 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()). 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall union doubleToLongLong { 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double db; 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long long ll; 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } dToLL; 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const unsigned int expLen = 2782; 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long bias[3]; 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr = INV_CAL_HDR_LEN; 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j; 784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp; 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V4\n"); 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n", 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall expLen, len); 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] = 0; 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] = 0; 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, inv_obj.temp_valid_data[i]); 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f\n", 825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.temp_data[i][j]); 826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.x_gyro_temp_data[i][j]); 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.y_gyro_temp_data[i][j]); 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0; 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 16777216LL * (long long)calData[ptr++]; 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 65536LL * (long long)calData[ptr++]; 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 256LL * (long long)calData[ptr++]; 865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (long long)calData[ptr++]; 866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 2147483648LL) { 867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= 4294967296LL; 868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.z_gyro_temp_data[i][j]); 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the accel biases */ 876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[i] = (int32_t) t; 883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_array(INV_ACCEL_BIAS, bias)) { 886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); 887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_array(INV_ACCEL_BIAS, bias); 888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the compass biases */ 891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_compass_calibration(); 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_compass_bias = (int)calData[ptr++]; 894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); 895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_init_compass_bias = (int)calData[ptr++]; 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_state = (int)calData[ptr++]; 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state); 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[i] = (int32_t) t; 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, 908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[i]); 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i] = (int32_t) t; 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i]); 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias[i] = (int32_t) t; 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 18; i++) { 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_peaks[i] = (int32_t) t; 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_v[i] = dToLL.db; 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 9; i++) { 952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[i] = dToLL.db; 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[i]); 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = 0; 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 16777216UL * ((uint32_t) calData[ptr++]); 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 65536UL * ((uint32_t) calData[ptr++]); 969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += 256u * ((uint32_t) calData[ptr++]); 970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall t += (uint32_t) calData[ptr++]; 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_scale[i] = (int32_t) t; 972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t); 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 6; i++) { 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_prev_xty[i] = dToLL.db; 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db); 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 36; i++) { 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.ll += (unsigned long long)calData[ptr++]; 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_prev_m[i] = dToLL.db; 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db); 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Load the compass offset flag and values */ 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++]; 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[0] = calData[ptr++]; 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[1] = calData[ptr++]; 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[2] = calData[ptr++]; 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_accuracy = calData[ptr++]; 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* push the compass offset values to the device */ 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_compass_offset(); 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_SUCCESS) { 1012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_compass_check_range() != INV_SUCCESS) { 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("range check fail"); 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_compass_calibration(); 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_compass_offset(); 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V4\n"); 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a type 5 set of calibration data. 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibrations data format stores values for (in order of 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * appearance) : 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - temperature, 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - gyro biases for X, Y, Z axes, 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - accel biases for X, Y, Z axes. 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This calibration data would normally be produced by the MPU Self 1039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Test and its size is 36 bytes (header and checksum included). 1040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calibration format type 5 is produced by the MPU Self Test and 1041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * substitutes the type 1 : inv_load_cal_V1(). 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param len 1053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length of the calibration 1054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) 1058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const short expLen = 36; 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accelBias[3] = { 0, 0, 0 }; 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float gyroBias[3] = { 0, 0, 0 }; 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr = INV_CAL_HDR_LEN; 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short temp; 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float newTemp; 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int bin; 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Entering inv_load_cal_V5\n"); 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (len != expLen) { 1073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n", 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall expLen, len); 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FILE_READ; 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load the temperature */ 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = (unsigned short)calData[ptr++] * (1L << 8); 1080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp += calData[ptr++]; 1081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall newTemp = (float)inv_decode_temperature(temp) / 65536.f; 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("newTemp = %f\n", newTemp); 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */ 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int32_t tmp = 0; 1087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 24); 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 16); 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 8); 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++]; 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroBias[i] = (float)tmp / 65536.0f; 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]); 1093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* find the temperature bin */ 1095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin = FindTempBin(newTemp); 1096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* populate the temp comp data structure */ 1098895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 1099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_data[%d][%d] = %f\n", 1100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], newTemp); 1101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; 1103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 1104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], gyroBias[0]); 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; 1106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 1107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], gyroBias[1]); 1108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; 1109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 1110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_ptrs[bin], gyroBias[2]); 1111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 1112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 1113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.temp_ptrs[bin] == 0) 1115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.temp_valid_data[bin] = TRUE; 1116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 1117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bin, inv_obj.temp_valid_data[bin]); 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* load accel biases (represented in 2 ^ 16 == 1 gee) 1120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall and apply immediately */ 1121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int32_t tmp = 0; 1123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 24); 1124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 16); 1125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++] * (1L << 8); 1126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += (int32_t) calData[ptr++]; 1127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accelBias[i] = (long)tmp; 1128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); 1129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { 1131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); 1132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_array(INV_ACCEL_BIAS, accelBias); 1133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = TRUE; 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("got_no_motion_bias = 1\n"); 1137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.cal_loaded_flag = TRUE; 1138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("cal_loaded_flag = 1\n"); 1139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOADCAL_LOG("Exiting inv_load_cal_V5\n"); 1141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Loads a set of calibration data. 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It parses a binary data set containing calibration data. 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be loaded from a file. 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be parsed. 1158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_cal(unsigned char *calData) 1162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int calType = 0; 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int len = 0; 1166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr; 1167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t chk = 0; 1168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t cmp_chk = 0; 1169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tMLLoadFunc loaders[] = { 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V0, 1172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V1, 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V2, 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V3, 1175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V4, 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_load_cal_V5 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* read the header (type and len) 1183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len is the total record length including header and checksum */ 1184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len = 0; 1185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len += 16777216L * ((int)calData[0]); 1186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len += 65536L * ((int)calData[1]); 1187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len += 256 * ((int)calData[2]); 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len += (int)calData[3]; 1189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calType = ((int)calData[4]) * 256 + ((int)calData[5]); 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (calType > 5) { 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Unsupported calibration file format %d. " 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "Valid types 0..5\n", calType); 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* check the checksum */ 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk = 0; 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr = len - INV_CAL_CHK_LEN; 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk += 16777216L * ((uint32_t) calData[ptr++]); 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk += 65536L * ((uint32_t) calData[ptr++]); 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk += 256 * ((uint32_t) calData[ptr++]); 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk += (uint32_t) calData[ptr++]; 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN, 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (chk != cmp_chk) { 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_CALIBRATION_CHECKSUM; 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* call the proper method to read in the data */ 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return loaders[calType] (calData, len); 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Stores a set of calibration data. 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It generates a binary data set containing calibration data. 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The binary data set is intended to be stored into a file. 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param calData 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A pointer to an array of bytes to be stored. 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The amount of bytes available in the array. 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_store_cal(unsigned char *calData, int length) 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ptr = 0; 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i = 0; 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int j = 0; 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp; 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t chk; 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long bias[3]; 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //unsigned char state; 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall union doubleToLongLong { 1242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double db; 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long long ll; 1244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } dToLL; 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("Entering inv_store_cal\n"); 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // length 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[0] = (unsigned char)((length >> 24) & 0xff); 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[1] = (unsigned char)((length >> 16) & 0xff); 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[2] = (unsigned char)((length >> 8) & 0xff); 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[3] = (unsigned char)(length & 0xff); 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("calLen = %d\n", length); 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // calibration data format type 1259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[4] = 0; 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[5] = 4; 1261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]); 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // data 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ptr = 6; 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (int)inv_obj.temp_ptrs[i]; 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp); 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (int)inv_obj.temp_valid_data[i]; 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp); 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f); 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 4294967296LL; 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("temp_data[%d][%d] = %f\n", 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.temp_data[i][j]); 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 1299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f); 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 4294967296LL; 1302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 1308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.x_gyro_temp_data[i][j]); 1309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 1313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f); 1314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 4294967296LL; 1316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 1322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.y_gyro_temp_data[i][j]); 1323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < BINS; i++) { 1326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < PTS_PER_BIN; j++) { 1327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f); 1328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += 4294967296LL; 1330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(tmp & 0xff); 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 1336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, j, inv_obj.z_gyro_temp_data[i][j]); 1337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_array(INV_ACCEL_BIAS, bias); 1341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* write the accel biases */ 1343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) bias[i]; 1345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 1350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* write the compass calibration state */ 1353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias); 1354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); 1355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias); 1356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); 1357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.compass_state == SF_UNCALIBRATED) { 1358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = SF_UNCALIBRATED; 1359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = SF_STARTUP_SETTLE; 1361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state); 1363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) inv_obj.compass_bias_error[i]; 1366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_bias_error[%d] = %ld\n", 1371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i, inv_obj.compass_bias_error[i]); 1372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) inv_obj.init_compass_bias[i]; 1375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("init_compass_bias[%d] = %ld\n", i, 1380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i]); 1381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) inv_obj.compass_bias[i]; 1384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 1389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 18; i++) { 1391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) inv_obj.compass_peaks[i]; 1392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 1397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.db = inv_obj.compass_bias_v[i]; 1400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_bias_v[%d] = %lf\n", i, 1409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_v[i]); 1410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 9; i++) { 1412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.db = inv_obj.compass_bias_ptr[i]; 1413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 1422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[i]); 1423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 1425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t t = (uint32_t) inv_obj.compass_scale[i]; 1426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(t & 0xff); 1430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]); 1431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 6; i++) { 1433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.db = inv_obj.compass_prev_xty[i]; 1434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i, 1443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_prev_xty[i]); 1444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 36; i++) { 1446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall dToLL.db = inv_obj.compass_prev_m[i]; 1447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("compass_prev_m[%d] = %lf\n", i, 1456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_prev_m[i]); 1457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* store the compass offsets and validity flag */ 1460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID]; 1461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0]; 1462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1]; 1463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2]; 1464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* store the compass accuracy */ 1466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy); 1467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* add a checksum */ 1469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall chk = inv_checksum(calData + INV_CAL_HDR_LEN, 1470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); 1471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((chk >> 24) & 0xff); 1472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((chk >> 16) & 0xff); 1473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)((chk >> 8) & 0xff); 1474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData[ptr++] = (unsigned char)(chk & 0xff); 1475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall STORECAL_LOG("Exiting inv_store_cal\n"); 1477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Load a calibration file. 1482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre Must be in INV_STATE_DMP_OPENED state. 1484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_open() or inv_dmp_stop() must have been called. 1485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 1486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * been called. 1487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 or error code. 1489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_calibration(void) 1491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *calData; 1493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int length; 1495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_get_cal_length(&length); 1500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_ERROR_FILE_OPEN) { 1501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Calibration data not loaded\n"); 1502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result || length <= 0) { 1505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not get file calibration length - " 1506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error %d - aborting\n", result); 1507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData = (unsigned char *)inv_malloc(length); 1510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!calData) { 1511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not allocate buffer of %d bytes - " 1512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "aborting\n", length); 1513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_MEMORY_EXAUSTED; 1514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read_cal(calData, length); 1516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not read the calibration data from file - " 1518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error %d - aborting\n", result); 1519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto free_mem_n_exit; 1520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_load_cal(calData); 1523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not load the calibration data - " 1525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error %d - aborting\n", result); 1526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto free_mem_n_exit; 1527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallfree_mem_n_exit: 1533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_free(calData); 1534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Store runtime calibration data to a file 1539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre Must be in INV_STATE_DMP_OPENED state. 1541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_open() or inv_dmp_stop() must have been called. 1542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 1543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * been called. 1544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 or error code. 1546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_store_calibration(void) 1548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *calData; 1550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int length; 1552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length = inv_get_cal_length(); 1557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calData = (unsigned char *)inv_malloc(length); 1558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!calData) { 1559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not allocate buffer of %d bytes - " 1560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "aborting\n", length); 1561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_MEMORY_EXAUSTED; 1562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_store_cal(calData, length); 1564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not store calibrated data on file - " 1566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error %d - aborting\n", result); 1567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto free_mem_n_exit; 1568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_write_cal(calData, length); 1571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Could not write calibration data - " "error %d\n", result); 1573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall goto free_mem_n_exit; 1574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallfree_mem_n_exit: 1580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_free(calData); 1581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 1586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1587