1/* 2 $License: 3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. 4 $ 5 */ 6 7/****************************************************************************** 8 * 9 * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ 10 * 11 *****************************************************************************/ 12 13/** 14 * @defgroup ML_STORED_DATA 15 * 16 * @{ 17 * @file ml_stored_data.c 18 * @brief functions for reading and writing stored data sets. 19 * Typically, these functions process stored calibration data. 20 */ 21 22#undef MPL_LOG_NDEBUG 23#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ 24#undef MPL_LOG_TAG 25 26#include <stdio.h> 27 28#include "log.h" 29#undef MPL_LOG_TAG 30#define MPL_LOG_TAG "MPL-storeload" 31 32#include "ml_stored_data.h" 33#include "storage_manager.h" 34#include "mlos.h" 35 36#define LOADCAL_DEBUG 0 37#define STORECAL_DEBUG 0 38 39#define DEFAULT_KEY 29681 40 41#define STORECAL_LOG MPL_LOGI 42#define LOADCAL_LOG MPL_LOGI 43 44inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) 45{ 46 FILE *fp; 47 inv_error_t result = INV_SUCCESS; 48 size_t fsize; 49 50 fp = fopen(MLCAL_FILE,"rb"); 51 if (fp == NULL) { 52 MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); 53 return INV_ERROR_FILE_OPEN; 54 } 55 56 // obtain file size 57 fseek (fp, 0 , SEEK_END); 58 fsize = ftell (fp); 59 rewind (fp); 60 61 *calData = (unsigned char *)inv_malloc(fsize); 62 if (*calData==NULL) { 63 MPL_LOGE("Could not allocate buffer of %d bytes - " 64 "aborting\n", fsize); 65 fclose(fp); 66 return INV_ERROR_MEMORY_EXAUSTED; 67 } 68 69 *bytesRead = fread(*calData, 1, fsize, fp); 70 if (*bytesRead != fsize) { 71 MPL_LOGE("bytes read (%d) don't match file size (%d)\n", 72 *bytesRead, fsize); 73 result = INV_ERROR_FILE_READ; 74 goto read_cal_end; 75 } 76 else { 77 MPL_LOGI("Bytes read = %d", *bytesRead); 78 } 79 80read_cal_end: 81 fclose(fp); 82 return result; 83} 84 85inv_error_t inv_write_cal(unsigned char *cal, size_t len) 86{ 87 FILE *fp; 88 int bytesWritten; 89 inv_error_t result = INV_SUCCESS; 90 91 if (len <= 0) { 92 MPL_LOGE("Nothing to write"); 93 return INV_ERROR_FILE_WRITE; 94 } 95 else { 96 MPL_LOGI("cal data size to write = %d", len); 97 } 98 fp = fopen(MLCAL_FILE,"wb"); 99 if (fp == NULL) { 100 MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); 101 return INV_ERROR_FILE_OPEN; 102 } 103 bytesWritten = fwrite(cal, 1, len, fp); 104 if (bytesWritten != len) { 105 MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", 106 bytesWritten, len); 107 result = INV_ERROR_FILE_WRITE; 108 } 109 else { 110 MPL_LOGI("Bytes written = %d", bytesWritten); 111 } 112 fclose(fp); 113 return result; 114} 115 116/** 117 * @brief Loads a type 0 set of calibration data. 118 * It parses a binary data set containing calibration data. 119 * The binary data set is intended to be loaded from a file. 120 * This calibrations data format stores values for (in order of 121 * appearance) : 122 * - temperature compensation : temperature data points, 123 * - temperature compensation : gyro biases data points for X, Y, 124 * and Z axes. 125 * - accel biases for X, Y, Z axes. 126 * This calibration data is produced internally by the MPL and its 127 * size is 2777 bytes (header and checksum included). 128 * Calibration format type 1 is currently used for ITG3500 129 * 130 * @pre inv_init_storage_manager() 131 * must have been called. 132 * 133 * @param calData 134 * A pointer to an array of bytes to be parsed. 135 * @param len 136 * the length of the calibration 137 * 138 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 139 */ 140inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) 141{ 142 inv_error_t result; 143 144 LOADCAL_LOG("Entering inv_load_cal_V0\n"); 145 146 /*if (len != expLen) { 147 MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", 148 expLen, len); 149 return INV_ERROR_FILE_READ; 150 }*/ 151 152 result = inv_load_mpl_states(calData, len); 153 return result; 154} 155 156/** 157 * @brief Loads a type 1 set of calibration data. 158 * It parses a binary data set containing calibration data. 159 * The binary data set is intended to be loaded from a file. 160 * This calibrations data format stores values for (in order of 161 * appearance) : 162 * - temperature, 163 * - gyro biases for X, Y, Z axes, 164 * - accel biases for X, Y, Z axes. 165 * This calibration data would normally be produced by the MPU Self 166 * Test and its size is 36 bytes (header and checksum included). 167 * Calibration format type 1 is produced by the MPU Self Test and 168 * substitutes the type 0 : inv_load_cal_V0(). 169 * 170 * @pre 171 * 172 * @param calData 173 * A pointer to an array of bytes to be parsed. 174 * @param len 175 * the length of the calibration 176 * 177 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 178 */ 179inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) 180{ 181 return INV_SUCCESS; 182} 183 184/** 185 * @brief Loads a set of calibration data. 186 * It parses a binary data set containing calibration data. 187 * The binary data set is intended to be loaded from a file. 188 * 189 * @pre 190 * 191 * 192 * @param calData 193 * A pointer to an array of bytes to be parsed. 194 * 195 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 196 */ 197inv_error_t inv_load_cal(unsigned char *calData) 198{ 199 int calType = 0; 200 int len = 0; 201 //int ptr; 202 //uint32_t chk = 0; 203 //uint32_t cmp_chk = 0; 204 205 /*load_func_t loaders[] = { 206 inv_load_cal_V0, 207 inv_load_cal_V1, 208 }; 209 */ 210 211 inv_load_cal_V0(calData, len); 212 213 /* read the header (type and len) 214 len is the total record length including header and checksum */ 215 len = 0; 216 len += 16777216L * ((int)calData[0]); 217 len += 65536L * ((int)calData[1]); 218 len += 256 * ((int)calData[2]); 219 len += (int)calData[3]; 220 221 calType = ((int)calData[4]) * 256 + ((int)calData[5]); 222 if (calType > 5) { 223 MPL_LOGE("Unsupported calibration file format %d. " 224 "Valid types 0..5\n", calType); 225 return INV_ERROR_INVALID_PARAMETER; 226 } 227 228 /* call the proper method to read in the data */ 229 //return loaders[calType] (calData, len); 230 return 0; 231} 232 233/** 234 * @brief Stores a set of calibration data. 235 * It generates a binary data set containing calibration data. 236 * The binary data set is intended to be stored into a file. 237 * 238 * @pre inv_dmp_open() 239 * 240 * @param calData 241 * A pointer to an array of bytes to be stored. 242 * @param length 243 * The amount of bytes available in the array. 244 * 245 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 246 */ 247inv_error_t inv_store_cal(unsigned char *calData, size_t length) 248{ 249 inv_error_t res = 0; 250 size_t size; 251 252 STORECAL_LOG("Entering inv_store_cal\n"); 253 254 inv_get_mpl_state_size(&size); 255 256 MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); 257 258 /* store data */ 259 res = inv_save_mpl_states(calData, size); 260 if(res != 0) 261 { 262 MPL_LOGE("inv_save_mpl_states() failed"); 263 } 264 265 STORECAL_LOG("Exiting inv_store_cal\n"); 266 return INV_SUCCESS; 267} 268 269/** 270 * @brief Load a calibration file. 271 * 272 * @pre Must be in INV_STATE_DMP_OPENED state. 273 * inv_dmp_open() or inv_dmp_stop() must have been called. 274 * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 275 * been called. 276 * 277 * @return 0 or error code. 278 */ 279inv_error_t inv_load_calibration(void) 280{ 281 unsigned char *calData= NULL; 282 inv_error_t result = 0; 283 size_t bytesRead = 0; 284 285 result = inv_read_cal(&calData, &bytesRead); 286 if(result != INV_SUCCESS) { 287 MPL_LOGE("Could not load cal file - " 288 "aborting\n"); 289 goto free_mem_n_exit; 290 } 291 292 result = inv_load_mpl_states(calData, bytesRead); 293 if (result != INV_SUCCESS) { 294 MPL_LOGE("Could not load the calibration data - " 295 "error %d - aborting\n", result); 296 goto free_mem_n_exit; 297 } 298 299free_mem_n_exit: 300 inv_free(calData); 301 return result; 302} 303 304/** 305 * @brief Store runtime calibration data to a file 306 * 307 * @pre Must be in INV_STATE_DMP_OPENED state. 308 * inv_dmp_open() or inv_dmp_stop() must have been called. 309 * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 310 * been called. 311 * 312 * @return 0 or error code. 313 */ 314inv_error_t inv_store_calibration(void) 315{ 316 unsigned char *calData; 317 inv_error_t result; 318 size_t length; 319 320 result = inv_get_mpl_state_size(&length); 321 calData = (unsigned char *)inv_malloc(length); 322 if (!calData) { 323 MPL_LOGE("Could not allocate buffer of %d bytes - " 324 "aborting\n", length); 325 return INV_ERROR_MEMORY_EXAUSTED; 326 } 327 else { 328 MPL_LOGI("inv_get_mpl state size = %d", length); 329 } 330 331 result = inv_save_mpl_states(calData, length); 332 if (result != INV_SUCCESS) { 333 MPL_LOGE("Could not save mpl states - " 334 "error %d - aborting\n", result); 335 goto free_mem_n_exit; 336 } 337 else { 338 MPL_LOGE("calData from inv_save_mpl_states, size=%d", 339 strlen((char *)calData)); 340 } 341 342 result = inv_write_cal(calData, length); 343 if (result != INV_SUCCESS) { 344 MPL_LOGE("Could not store calibrated data on file - " 345 "error %d - aborting\n", result); 346 goto free_mem_n_exit; 347 348 } 349 350free_mem_n_exit: 351 inv_free(calData); 352 return result; 353} 354 355/** 356 * @} 357 */ 358