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 #include <stdio.h> 23 24 #include "log.h" 25 #undef MPL_LOG_TAG 26 #define MPL_LOG_TAG "MPL-storeload" 27 28 #include "ml_stored_data.h" 29 #include "storage_manager.h" 30 #include "mlos.h" 31 32 #define LOADCAL_DEBUG 0 33 #define STORECAL_DEBUG 0 34 35 #define DEFAULT_KEY 29681 36 37 #define STORECAL_LOG MPL_LOGI 38 #define LOADCAL_LOG MPL_LOGI 39 40 inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) 41 { 42 FILE *fp; 43 inv_error_t result = INV_SUCCESS; 44 size_t fsize; 45 46 fp = fopen(MLCAL_FILE,"rb"); 47 if (fp == NULL) { 48 MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); 49 return INV_ERROR_FILE_OPEN; 50 } 51 52 // obtain file size 53 fseek (fp, 0 , SEEK_END); 54 fsize = ftell (fp); 55 rewind (fp); 56 57 *calData = (unsigned char *)inv_malloc(fsize); 58 if (*calData==NULL) { 59 MPL_LOGE("Could not allocate buffer of %d bytes - " 60 "aborting\n", fsize); 61 fclose(fp); 62 return INV_ERROR_MEMORY_EXAUSTED; 63 } 64 65 *bytesRead = fread(*calData, 1, fsize, fp); 66 if (*bytesRead != fsize) { 67 MPL_LOGE("bytes read (%d) don't match file size (%d)\n", 68 *bytesRead, fsize); 69 result = INV_ERROR_FILE_READ; 70 goto read_cal_end; 71 } 72 else { 73 MPL_LOGI("Bytes read = %d", *bytesRead); 74 } 75 76 read_cal_end: 77 fclose(fp); 78 return result; 79 } 80 81 inv_error_t inv_write_cal(unsigned char *cal, size_t len) 82 { 83 FILE *fp; 84 int bytesWritten; 85 inv_error_t result = INV_SUCCESS; 86 87 if (len <= 0) { 88 MPL_LOGE("Nothing to write"); 89 return INV_ERROR_FILE_WRITE; 90 } 91 else { 92 MPL_LOGI("cal data size to write = %d", len); 93 } 94 fp = fopen(MLCAL_FILE,"wb"); 95 if (fp == NULL) { 96 MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); 97 return INV_ERROR_FILE_OPEN; 98 } 99 bytesWritten = fwrite(cal, 1, len, fp); 100 if (bytesWritten != len) { 101 MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", 102 bytesWritten, len); 103 result = INV_ERROR_FILE_WRITE; 104 } 105 else { 106 MPL_LOGI("Bytes written = %d", bytesWritten); 107 } 108 fclose(fp); 109 return result; 110 } 111 112 /** 113 * @brief Loads a type 0 set of calibration data. 114 * It parses a binary data set containing calibration data. 115 * The binary data set is intended to be loaded from a file. 116 * This calibrations data format stores values for (in order of 117 * appearance) : 118 * - temperature compensation : temperature data points, 119 * - temperature compensation : gyro biases data points for X, Y, 120 * and Z axes. 121 * - accel biases for X, Y, Z axes. 122 * This calibration data is produced internally by the MPL and its 123 * size is 2777 bytes (header and checksum included). 124 * Calibration format type 1 is currently used for ITG3500 125 * 126 * @pre inv_init_storage_manager() 127 * must have been called. 128 * 129 * @param calData 130 * A pointer to an array of bytes to be parsed. 131 * @param len 132 * the length of the calibration 133 * 134 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 135 */ 136 inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) 137 { 138 inv_error_t result; 139 140 LOADCAL_LOG("Entering inv_load_cal_V0\n"); 141 142 /*if (len != expLen) { 143 MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", 144 expLen, len); 145 return INV_ERROR_FILE_READ; 146 }*/ 147 148 result = inv_load_mpl_states(calData, len); 149 return result; 150 } 151 152 /** 153 * @brief Loads a type 1 set of calibration data. 154 * It parses a binary data set containing calibration data. 155 * The binary data set is intended to be loaded from a file. 156 * This calibrations data format stores values for (in order of 157 * appearance) : 158 * - temperature, 159 * - gyro biases for X, Y, Z axes, 160 * - accel biases for X, Y, Z axes. 161 * This calibration data would normally be produced by the MPU Self 162 * Test and its size is 36 bytes (header and checksum included). 163 * Calibration format type 1 is produced by the MPU Self Test and 164 * substitutes the type 0 : inv_load_cal_V0(). 165 * 166 * @pre 167 * 168 * @param calData 169 * A pointer to an array of bytes to be parsed. 170 * @param len 171 * the length of the calibration 172 * 173 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 174 */ 175 inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) 176 { 177 return INV_SUCCESS; 178 } 179 180 /** 181 * @brief Loads a set of calibration data. 182 * It parses a binary data set containing calibration data. 183 * The binary data set is intended to be loaded from a file. 184 * 185 * @pre 186 * 187 * 188 * @param calData 189 * A pointer to an array of bytes to be parsed. 190 * 191 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 192 */ 193 inv_error_t inv_load_cal(unsigned char *calData) 194 { 195 int calType = 0; 196 int len = 0; 197 //int ptr; 198 //uint32_t chk = 0; 199 //uint32_t cmp_chk = 0; 200 201 /*load_func_t loaders[] = { 202 inv_load_cal_V0, 203 inv_load_cal_V1, 204 }; 205 */ 206 207 inv_load_cal_V0(calData, len); 208 209 /* read the header (type and len) 210 len is the total record length including header and checksum */ 211 len = 0; 212 len += 16777216L * ((int)calData[0]); 213 len += 65536L * ((int)calData[1]); 214 len += 256 * ((int)calData[2]); 215 len += (int)calData[3]; 216 217 calType = ((int)calData[4]) * 256 + ((int)calData[5]); 218 if (calType > 5) { 219 MPL_LOGE("Unsupported calibration file format %d. " 220 "Valid types 0..5\n", calType); 221 return INV_ERROR_INVALID_PARAMETER; 222 } 223 224 /* call the proper method to read in the data */ 225 //return loaders[calType] (calData, len); 226 return 0; 227 } 228 229 /** 230 * @brief Stores a set of calibration data. 231 * It generates a binary data set containing calibration data. 232 * The binary data set is intended to be stored into a file. 233 * 234 * @pre inv_dmp_open() 235 * 236 * @param calData 237 * A pointer to an array of bytes to be stored. 238 * @param length 239 * The amount of bytes available in the array. 240 * 241 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 242 */ 243 inv_error_t inv_store_cal(unsigned char *calData, size_t length) 244 { 245 inv_error_t res = 0; 246 size_t size; 247 248 STORECAL_LOG("Entering inv_store_cal\n"); 249 250 inv_get_mpl_state_size(&size); 251 252 MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); 253 254 /* store data */ 255 res = inv_save_mpl_states(calData, size); 256 if(res != 0) 257 { 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 */ 275 inv_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 295 free_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 */ 310 inv_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 } 323 else { 324 MPL_LOGI("mpl state size = %d", length); 325 } 326 327 result = inv_save_mpl_states(calData, length); 328 if (result != INV_SUCCESS) { 329 MPL_LOGE("Could not save mpl states - " 330 "error %d - aborting\n", result); 331 goto free_mem_n_exit; 332 } 333 else { 334 MPL_LOGE("calData from inv_save_mpl_states, size=%d", 335 strlen((char *)calData)); 336 } 337 338 result = inv_write_cal(calData, length); 339 if (result != INV_SUCCESS) { 340 MPL_LOGE("Could not store calibrated data on file - " 341 "error %d - aborting\n", result); 342 goto free_mem_n_exit; 343 344 } 345 346 free_mem_n_exit: 347 inv_free(calData); 348 return result; 349 } 350 351 /** 352 * @} 353 */ 354