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