1 /* 2 $License: 3 Copyright 2011 InvenSense, Inc. 4 5 Licensed under the Apache License, Version 2.0 (the "License"); 6 you may not use this file except in compliance with the License. 7 You may obtain a copy of the License at 8 9 http://www.apache.org/licenses/LICENSE-2.0 10 11 Unless required by applicable law or agreed to in writing, software 12 distributed under the License is distributed on an "AS IS" BASIS, 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 See the License for the specific language governing permissions and 15 limitations under the License. 16 $ 17 */ 18 19 /****************************************************************************** 20 * 21 * $Id: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $ 22 * 23 *****************************************************************************/ 24 25 /** 26 * @defgroup ML_STORED_DATA 27 * 28 * @{ 29 * @file ml_stored_data.c 30 * @brief functions for reading and writing stored data sets. 31 * Typically, these functions process stored calibration data. 32 */ 33 34 #include "ml_stored_data.h" 35 #include "ml.h" 36 #include "mlFIFO.h" 37 #include "mltypes.h" 38 #include "mlinclude.h" 39 #include "compass.h" 40 #include "dmpKey.h" 41 #include "dmpDefault.h" 42 #include "mlstates.h" 43 #include "checksum.h" 44 #include "mlsupervisor.h" 45 46 #include "mlsl.h" 47 #include "mlos.h" 48 49 #include "log.h" 50 #undef MPL_LOG_TAG 51 #define MPL_LOG_TAG "MPL-storeload" 52 53 /* 54 Typedefs 55 */ 56 typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short); 57 58 /* 59 Globals 60 */ 61 extern struct inv_obj_t inv_obj; 62 63 /* 64 Debugging Definitions 65 set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields 66 being read or stored in human-readable format. 67 When set to 0, the compiler will optimize and remove the printouts 68 from the code. 69 */ 70 #define LOADCAL_DEBUG 0 71 #define STORECAL_DEBUG 0 72 73 #define LOADCAL_LOG(...) \ 74 if(LOADCAL_DEBUG) \ 75 MPL_LOGI("LOADCAL: " __VA_ARGS__) 76 #define STORECAL_LOG(...) \ 77 if(STORECAL_DEBUG) \ 78 MPL_LOGI("STORECAL: " __VA_ARGS__) 79 80 /** 81 * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl 82 * advanced algorithms library. To remove cross-dependency, for now, 83 * we reimplement the same function here. 84 * @param temp 85 * the temperature (1 count == 1 degree C). 86 */ 87 int FindTempBin(float temp) 88 { 89 int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN); 90 if (bin < 0) 91 bin = 0; 92 if (bin > BINS - 1) 93 bin = BINS - 1; 94 return bin; 95 } 96 97 /** 98 * @brief Returns the length of the <b>MPL internal calibration data</b>. 99 * Should be called before allocating the memory required to store 100 * this data to a file. 101 * This function returns the total size required to store the cal 102 * data including the header (4 bytes) and the checksum (2 bytes). 103 * 104 * @pre Must be in INV_STATE_DMP_OPENED state. 105 * inv_dmp_open() or inv_dmp_stop() must have been called. 106 * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 107 * been called. 108 * 109 * @return the length of the internal calibrated data format. 110 */ 111 unsigned int inv_get_cal_length(void) 112 { 113 INVENSENSE_FUNC_START; 114 unsigned int length; 115 length = INV_CAL_HDR_LEN + // header 116 BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal 117 INV_CAL_ACCEL_LEN + // accel cal 118 INV_CAL_COMPASS_LEN + // compass cal 119 INV_CAL_CHK_LEN; // checksum 120 return length; 121 } 122 123 /** 124 * @brief Loads a type 0 set of calibration data. 125 * It parses a binary data set containing calibration data. 126 * The binary data set is intended to be loaded from a file. 127 * This calibrations data format stores values for (in order of 128 * appearance) : 129 * - temperature, 130 * - gyro biases for X, Y, Z axes. 131 * This calibration data would normally be produced by the MPU Self 132 * Test and its size is 18 bytes (header and checksum included). 133 * Calibration format type 0 is currently <b>NOT</b> used and 134 * is substituted by type 5 : inv_load_cal_V5(). 135 * 136 * @note This calibration data format is obsoleted and no longer supported 137 * by the rest of the MPL 138 * 139 * @pre inv_dmp_open() 140 * @ifnot MPL_MF 141 * or inv_open_low_power_pedometer() 142 * or inv_eis_open_dmp() 143 * @endif 144 * must have been called. 145 * 146 * @param calData 147 * A pointer to an array of bytes to be parsed. 148 * @param len 149 * the length of the calibration 150 * 151 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 152 */ 153 inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len) 154 { 155 INVENSENSE_FUNC_START; 156 const short expLen = 18; 157 long newGyroData[3] = { 0, 0, 0 }; 158 float newTemp; 159 int bin; 160 161 LOADCAL_LOG("Entering inv_load_cal_V0\n"); 162 163 if (len != expLen) { 164 MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); 165 return INV_ERROR_FILE_READ; 166 } 167 168 newTemp = (float)inv_decode_temperature( 169 (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; 170 LOADCAL_LOG("newTemp = %f\n", newTemp); 171 172 newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); 173 if (newGyroData[0] > 32767L) 174 newGyroData[0] -= 65536L; 175 LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); 176 newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); 177 if (newGyroData[1] > 32767L) 178 newGyroData[1] -= 65536L; 179 LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 180 newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); 181 if (newGyroData[2] > 32767L) 182 newGyroData[2] -= 65536L; 183 LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 184 185 bin = FindTempBin(newTemp); 186 LOADCAL_LOG("bin = %d", bin); 187 188 inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 189 LOADCAL_LOG("temp_data[%d][%d] = %f", 190 bin, inv_obj.temp_ptrs[bin], 191 inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); 192 inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 193 ((float)newGyroData[0]) / 65536.f; 194 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 195 bin, inv_obj.temp_ptrs[bin], 196 inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 197 inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 198 ((float)newGyroData[0]) / 65536.f; 199 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 200 bin, inv_obj.temp_ptrs[bin], 201 inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 202 inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = 203 ((float)newGyroData[0]) / 65536.f; 204 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 205 bin, inv_obj.temp_ptrs[bin], 206 inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 207 208 inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 209 LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 210 211 if (inv_obj.temp_ptrs[bin] == 0) 212 inv_obj.temp_valid_data[bin] = TRUE; 213 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 214 bin, inv_obj.temp_valid_data[bin]); 215 216 inv_obj.got_no_motion_bias = TRUE; 217 LOADCAL_LOG("got_no_motion_bias = 1\n"); 218 inv_obj.cal_loaded_flag = TRUE; 219 LOADCAL_LOG("cal_loaded_flag = 1\n"); 220 221 LOADCAL_LOG("Exiting inv_load_cal_V0\n"); 222 return INV_SUCCESS; 223 } 224 225 /** 226 * @brief Loads a type 1 set of calibration data. 227 * It parses a binary data set containing calibration data. 228 * The binary data set is intended to be loaded from a file. 229 * This calibrations data format stores values for (in order of 230 * appearance) : 231 * - temperature, 232 * - gyro biases for X, Y, Z axes, 233 * - accel biases for X, Y, Z axes. 234 * This calibration data would normally be produced by the MPU Self 235 * Test and its size is 24 bytes (header and checksum included). 236 * Calibration format type 1 is currently <b>NOT</b> used and 237 * is substituted by type 5 : inv_load_cal_V5(). 238 * 239 * @note In order to successfully work, the gyro bias must be stored 240 * expressed in 250 dps full scale (131.072 sensitivity). Other full 241 * scale range will produce unpredictable results in the gyro biases. 242 * 243 * @pre inv_dmp_open() 244 * @ifnot MPL_MF 245 * or inv_open_low_power_pedometer() 246 * or inv_eis_open_dmp() 247 * @endif 248 * must have been called. 249 * 250 * @param calData 251 * A pointer to an array of bytes to be parsed. 252 * @param len 253 * the length of the calibration 254 * 255 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 256 */ 257 inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) 258 { 259 INVENSENSE_FUNC_START; 260 const short expLen = 24; 261 long newGyroData[3] = {0, 0, 0}; 262 long accelBias[3] = {0, 0, 0}; 263 float gyroBias[3] = {0, 0, 0}; 264 float newTemp = 0; 265 int bin; 266 267 LOADCAL_LOG("Entering inv_load_cal_V1\n"); 268 269 if (len != expLen) { 270 MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); 271 return INV_ERROR_FILE_READ; 272 } 273 274 newTemp = (float)inv_decode_temperature( 275 (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; 276 LOADCAL_LOG("newTemp = %f\n", newTemp); 277 278 newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); 279 if (newGyroData[0] > 32767L) 280 newGyroData[0] -= 65536L; 281 LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); 282 newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); 283 if (newGyroData[1] > 32767L) 284 newGyroData[1] -= 65536L; 285 LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]); 286 newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); 287 if (newGyroData[2] > 32767L) 288 newGyroData[2] -= 65536L; 289 LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); 290 291 bin = FindTempBin(newTemp); 292 LOADCAL_LOG("bin = %d\n", bin); 293 294 gyroBias[0] = ((float)newGyroData[0]) / 131.072f; 295 gyroBias[1] = ((float)newGyroData[1]) / 131.072f; 296 gyroBias[2] = ((float)newGyroData[2]) / 131.072f; 297 LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]); 298 LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]); 299 LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]); 300 301 inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 302 LOADCAL_LOG("temp_data[%d][%d] = %f", 303 bin, inv_obj.temp_ptrs[bin], 304 inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); 305 inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; 306 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 307 bin, inv_obj.temp_ptrs[bin], 308 inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 309 inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; 310 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 311 bin, inv_obj.temp_ptrs[bin], 312 inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 313 inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; 314 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 315 bin, inv_obj.temp_ptrs[bin], 316 inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); 317 318 inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 319 LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 320 321 if (inv_obj.temp_ptrs[bin] == 0) 322 inv_obj.temp_valid_data[bin] = TRUE; 323 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 324 bin, inv_obj.temp_valid_data[bin]); 325 326 /* load accel biases and apply immediately */ 327 accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]); 328 if (accelBias[0] > 32767) 329 accelBias[0] -= 65536L; 330 accelBias[0] = (long)((long long)accelBias[0] * 65536L * 331 inv_obj.accel_sens / 1073741824L); 332 LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]); 333 accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]); 334 if (accelBias[1] > 32767) 335 accelBias[1] -= 65536L; 336 accelBias[1] = (long)((long long)accelBias[1] * 65536L * 337 inv_obj.accel_sens / 1073741824L); 338 LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]); 339 accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]); 340 if (accelBias[2] > 32767) 341 accelBias[2] -= 65536L; 342 accelBias[2] = (long)((long long)accelBias[2] * 65536L * 343 inv_obj.accel_sens / 1073741824L); 344 LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); 345 if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { 346 LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); 347 return inv_set_array(INV_ACCEL_BIAS, accelBias); 348 } 349 350 inv_obj.got_no_motion_bias = TRUE; 351 LOADCAL_LOG("got_no_motion_bias = 1\n"); 352 inv_obj.cal_loaded_flag = TRUE; 353 LOADCAL_LOG("cal_loaded_flag = 1\n"); 354 355 LOADCAL_LOG("Exiting inv_load_cal_V1\n"); 356 return INV_SUCCESS; 357 } 358 359 /** 360 * @brief Loads a type 2 set of calibration data. 361 * It parses a binary data set containing calibration data. 362 * The binary data set is intended to be loaded from a file. 363 * This calibrations data format stores values for (in order of 364 * appearance) : 365 * - temperature compensation : temperature data points, 366 * - temperature compensation : gyro biases data points for X, Y, 367 * and Z axes. 368 * - accel biases for X, Y, Z axes. 369 * This calibration data is produced internally by the MPL and its 370 * size is 2222 bytes (header and checksum included). 371 * Calibration format type 2 is currently <b>NOT</b> used and 372 * is substituted by type 4 : inv_load_cal_V4(). 373 * 374 * @pre inv_dmp_open() 375 * @ifnot MPL_MF 376 * or inv_open_low_power_pedometer() 377 * or inv_eis_open_dmp() 378 * @endif 379 * must have been called. 380 * 381 * @param calData 382 * A pointer to an array of bytes to be parsed. 383 * @param len 384 * the length of the calibration 385 * 386 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 387 */ 388 inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) 389 { 390 INVENSENSE_FUNC_START; 391 const short expLen = 2222; 392 long accel_bias[3]; 393 int ptr = INV_CAL_HDR_LEN; 394 395 int i, j; 396 long long tmp; 397 398 LOADCAL_LOG("Entering inv_load_cal_V2\n"); 399 400 if (len != expLen) { 401 MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n", 402 expLen, len); 403 return INV_ERROR_FILE_READ; 404 } 405 406 for (i = 0; i < BINS; i++) { 407 inv_obj.temp_ptrs[i] = 0; 408 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 409 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 410 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 411 inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 412 LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 413 } 414 for (i = 0; i < BINS; i++) { 415 inv_obj.temp_valid_data[i] = 0; 416 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 417 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 418 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 419 inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 420 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 421 i, inv_obj.temp_valid_data[i]); 422 } 423 424 for (i = 0; i < BINS; i++) { 425 for (j = 0; j < PTS_PER_BIN; j++) { 426 tmp = 0; 427 tmp += 16777216LL * (long long)calData[ptr++]; 428 tmp += 65536LL * (long long)calData[ptr++]; 429 tmp += 256LL * (long long)calData[ptr++]; 430 tmp += (long long)calData[ptr++]; 431 if (tmp > 2147483648LL) { 432 tmp -= 4294967296LL; 433 } 434 inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 435 LOADCAL_LOG("temp_data[%d][%d] = %f\n", 436 i, j, inv_obj.temp_data[i][j]); 437 } 438 439 } 440 for (i = 0; i < BINS; i++) { 441 for (j = 0; j < PTS_PER_BIN; j++) { 442 tmp = 0; 443 tmp += 16777216LL * (long long)calData[ptr++]; 444 tmp += 65536LL * (long long)calData[ptr++]; 445 tmp += 256LL * (long long)calData[ptr++]; 446 tmp += (long long)calData[ptr++]; 447 if (tmp > 2147483648LL) { 448 tmp -= 4294967296LL; 449 } 450 inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 451 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 452 i, j, inv_obj.x_gyro_temp_data[i][j]); 453 } 454 } 455 for (i = 0; i < BINS; i++) { 456 for (j = 0; j < PTS_PER_BIN; j++) { 457 tmp = 0; 458 tmp += 16777216LL * (long long)calData[ptr++]; 459 tmp += 65536LL * (long long)calData[ptr++]; 460 tmp += 256LL * (long long)calData[ptr++]; 461 tmp += (long long)calData[ptr++]; 462 if (tmp > 2147483648LL) { 463 tmp -= 4294967296LL; 464 } 465 inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 466 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 467 i, j, inv_obj.y_gyro_temp_data[i][j]); 468 } 469 } 470 for (i = 0; i < BINS; i++) { 471 for (j = 0; j < PTS_PER_BIN; j++) { 472 tmp = 0; 473 tmp += 16777216LL * (long long)calData[ptr++]; 474 tmp += 65536LL * (long long)calData[ptr++]; 475 tmp += 256LL * (long long)calData[ptr++]; 476 tmp += (long long)calData[ptr++]; 477 if (tmp > 2147483648LL) { 478 tmp -= 4294967296LL; 479 } 480 inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 481 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 482 i, j, inv_obj.z_gyro_temp_data[i][j]); 483 } 484 } 485 486 /* read the accel biases */ 487 for (i = 0; i < 3; i++) { 488 uint32_t t = 0; 489 t += 16777216UL * ((uint32_t) calData[ptr++]); 490 t += 65536UL * ((uint32_t) calData[ptr++]); 491 t += 256u * ((uint32_t) calData[ptr++]); 492 t += (uint32_t) calData[ptr++]; 493 accel_bias[i] = (int32_t) t; 494 LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); 495 } 496 497 if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { 498 LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); 499 return inv_set_array(INV_ACCEL_BIAS, accel_bias); 500 } 501 502 inv_obj.got_no_motion_bias = TRUE; 503 LOADCAL_LOG("got_no_motion_bias = 1\n"); 504 inv_obj.cal_loaded_flag = TRUE; 505 LOADCAL_LOG("cal_loaded_flag = 1\n"); 506 507 LOADCAL_LOG("Exiting inv_load_cal_V2\n"); 508 return INV_SUCCESS; 509 } 510 511 /** 512 * @brief Loads a type 3 set of calibration data. 513 * It parses a binary data set containing calibration data. 514 * The binary data set is intended to be loaded from a file. 515 * This calibrations data format stores values for (in order of 516 * appearance) : 517 * - temperature compensation : temperature data points, 518 * - temperature compensation : gyro biases data points for X, Y, 519 * and Z axes. 520 * - accel biases for X, Y, Z axes. 521 * - compass biases for X, Y, Z axes and bias tracking algorithm 522 * mock-up. 523 * This calibration data is produced internally by the MPL and its 524 * size is 2429 bytes (header and checksum included). 525 * Calibration format type 3 is currently <b>NOT</b> used and 526 * is substituted by type 4 : inv_load_cal_V4(). 527 528 * @pre inv_dmp_open() 529 * @ifnot MPL_MF 530 * or inv_open_low_power_pedometer() 531 * or inv_eis_open_dmp() 532 * @endif 533 * must have been called. 534 * 535 * @param calData 536 * A pointer to an array of bytes to be parsed. 537 * @param len 538 * the length of the calibration 539 * 540 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 541 */ 542 inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) 543 { 544 INVENSENSE_FUNC_START; 545 union doubleToLongLong { 546 double db; 547 unsigned long long ll; 548 } dToLL; 549 550 const short expLen = 2429; 551 long bias[3]; 552 int i, j; 553 int ptr = INV_CAL_HDR_LEN; 554 long long tmp; 555 556 LOADCAL_LOG("Entering inv_load_cal_V3\n"); 557 558 if (len != expLen) { 559 MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n", 560 expLen, len); 561 return INV_ERROR_FILE_READ; 562 } 563 564 for (i = 0; i < BINS; i++) { 565 inv_obj.temp_ptrs[i] = 0; 566 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 567 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 568 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 569 inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 570 LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 571 } 572 for (i = 0; i < BINS; i++) { 573 inv_obj.temp_valid_data[i] = 0; 574 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 575 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 576 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 577 inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 578 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 579 i, inv_obj.temp_valid_data[i]); 580 } 581 582 for (i = 0; i < BINS; i++) { 583 for (j = 0; j < PTS_PER_BIN; j++) { 584 tmp = 0; 585 tmp += 16777216LL * (long long)calData[ptr++]; 586 tmp += 65536LL * (long long)calData[ptr++]; 587 tmp += 256LL * (long long)calData[ptr++]; 588 tmp += (long long)calData[ptr++]; 589 if (tmp > 2147483648LL) { 590 tmp -= 4294967296LL; 591 } 592 inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 593 LOADCAL_LOG("temp_data[%d][%d] = %f\n", 594 i, j, inv_obj.temp_data[i][j]); 595 } 596 } 597 598 for (i = 0; i < BINS; i++) { 599 for (j = 0; j < PTS_PER_BIN; j++) { 600 tmp = 0; 601 tmp += 16777216LL * (long long)calData[ptr++]; 602 tmp += 65536LL * (long long)calData[ptr++]; 603 tmp += 256LL * (long long)calData[ptr++]; 604 tmp += (long long)calData[ptr++]; 605 if (tmp > 2147483648LL) { 606 tmp -= 4294967296LL; 607 } 608 inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 609 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 610 i, j, inv_obj.x_gyro_temp_data[i][j]); 611 } 612 } 613 for (i = 0; i < BINS; i++) { 614 for (j = 0; j < PTS_PER_BIN; j++) { 615 tmp = 0; 616 tmp += 16777216LL * (long long)calData[ptr++]; 617 tmp += 65536LL * (long long)calData[ptr++]; 618 tmp += 256LL * (long long)calData[ptr++]; 619 tmp += (long long)calData[ptr++]; 620 if (tmp > 2147483648LL) { 621 tmp -= 4294967296LL; 622 } 623 inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 624 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 625 i, j, inv_obj.y_gyro_temp_data[i][j]); 626 } 627 } 628 for (i = 0; i < BINS; i++) { 629 for (j = 0; j < PTS_PER_BIN; j++) { 630 tmp = 0; 631 tmp += 16777216LL * (long long)calData[ptr++]; 632 tmp += 65536LL * (long long)calData[ptr++]; 633 tmp += 256LL * (long long)calData[ptr++]; 634 tmp += (long long)calData[ptr++]; 635 if (tmp > 2147483648LL) { 636 tmp -= 4294967296LL; 637 } 638 inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 639 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 640 i, j, inv_obj.z_gyro_temp_data[i][j]); 641 } 642 } 643 644 /* read the accel biases */ 645 for (i = 0; i < 3; i++) { 646 uint32_t t = 0; 647 t += 16777216UL * ((uint32_t) calData[ptr++]); 648 t += 65536UL * ((uint32_t) calData[ptr++]); 649 t += 256u * ((uint32_t) calData[ptr++]); 650 t += (uint32_t) calData[ptr++]; 651 bias[i] = (int32_t) t; 652 LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 653 } 654 if (inv_set_array(INV_ACCEL_BIAS, bias)) { 655 LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); 656 return inv_set_array(INV_ACCEL_BIAS, bias); 657 } 658 659 /* read the compass biases */ 660 inv_obj.got_compass_bias = (int)calData[ptr++]; 661 inv_obj.got_init_compass_bias = (int)calData[ptr++]; 662 inv_obj.compass_state = (int)calData[ptr++]; 663 664 for (i = 0; i < 3; i++) { 665 uint32_t t = 0; 666 t += 16777216UL * ((uint32_t) calData[ptr++]); 667 t += 65536UL * ((uint32_t) calData[ptr++]); 668 t += 256u * ((uint32_t) calData[ptr++]); 669 t += (uint32_t) calData[ptr++]; 670 inv_obj.compass_bias_error[i] = (int32_t) t; 671 LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, 672 inv_obj.compass_bias_error[i]); 673 } 674 for (i = 0; i < 3; i++) { 675 uint32_t t = 0; 676 t += 16777216UL * ((uint32_t) calData[ptr++]); 677 t += 65536UL * ((uint32_t) calData[ptr++]); 678 t += 256u * ((uint32_t) calData[ptr++]); 679 t += (uint32_t) calData[ptr++]; 680 inv_obj.init_compass_bias[i] = (int32_t) t; 681 LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, 682 inv_obj.init_compass_bias[i]); 683 } 684 for (i = 0; i < 3; i++) { 685 uint32_t t = 0; 686 t += 16777216UL * ((uint32_t) calData[ptr++]); 687 t += 65536UL * ((uint32_t) calData[ptr++]); 688 t += 256u * ((uint32_t) calData[ptr++]); 689 t += (uint32_t) calData[ptr++]; 690 inv_obj.compass_bias[i] = (int32_t) t; 691 LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 692 } 693 for (i = 0; i < 18; i++) { 694 uint32_t t = 0; 695 t += 16777216UL * ((uint32_t) calData[ptr++]); 696 t += 65536UL * ((uint32_t) calData[ptr++]); 697 t += 256u * ((uint32_t) calData[ptr++]); 698 t += (uint32_t) calData[ptr++]; 699 inv_obj.compass_peaks[i] = (int32_t) t; 700 LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 701 } 702 for (i = 0; i < 3; i++) { 703 dToLL.ll = 0; 704 dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 705 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 706 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 707 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 708 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 709 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 710 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 711 dToLL.ll += (unsigned long long)calData[ptr++]; 712 713 inv_obj.compass_bias_v[i] = dToLL.db; 714 LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); 715 } 716 for (i = 0; i < 9; i++) { 717 dToLL.ll = 0; 718 dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 719 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 720 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 721 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 722 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 723 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 724 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 725 dToLL.ll += (unsigned long long)calData[ptr++]; 726 727 inv_obj.compass_bias_ptr[i] = dToLL.db; 728 LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 729 inv_obj.compass_bias_ptr[i]); 730 } 731 732 inv_obj.got_no_motion_bias = TRUE; 733 LOADCAL_LOG("got_no_motion_bias = 1\n"); 734 inv_obj.cal_loaded_flag = TRUE; 735 LOADCAL_LOG("cal_loaded_flag = 1\n"); 736 737 LOADCAL_LOG("Exiting inv_load_cal_V3\n"); 738 return INV_SUCCESS; 739 } 740 741 /** 742 * @brief Loads a type 4 set of calibration data. 743 * It parses a binary data set containing calibration data. 744 * The binary data set is intended to be loaded from a file. 745 * This calibrations data format stores values for (in order of 746 * appearance) : 747 * - temperature compensation : temperature data points, 748 * - temperature compensation : gyro biases data points for X, Y, 749 * and Z axes. 750 * - accel biases for X, Y, Z axes. 751 * - compass biases for X, Y, Z axes, compass scale, and bias 752 * tracking algorithm mock-up. 753 * This calibration data is produced internally by the MPL and its 754 * size is 2777 bytes (header and checksum included). 755 * Calibration format type 4 is currently used and 756 * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()). 757 * 758 * @pre inv_dmp_open() 759 * @ifnot MPL_MF 760 * or inv_open_low_power_pedometer() 761 * or inv_eis_open_dmp() 762 * @endif 763 * must have been called. 764 * 765 * @param calData 766 * A pointer to an array of bytes to be parsed. 767 * @param len 768 * the length of the calibration 769 * 770 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 771 */ 772 inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) 773 { 774 INVENSENSE_FUNC_START; 775 union doubleToLongLong { 776 double db; 777 unsigned long long ll; 778 } dToLL; 779 780 const unsigned int expLen = 2782; 781 long bias[3]; 782 int ptr = INV_CAL_HDR_LEN; 783 int i, j; 784 long long tmp; 785 inv_error_t result; 786 787 LOADCAL_LOG("Entering inv_load_cal_V4\n"); 788 789 if (len != expLen) { 790 MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n", 791 expLen, len); 792 return INV_ERROR_FILE_READ; 793 } 794 795 for (i = 0; i < BINS; i++) { 796 inv_obj.temp_ptrs[i] = 0; 797 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); 798 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); 799 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); 800 inv_obj.temp_ptrs[i] += (int)calData[ptr++]; 801 LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); 802 } 803 for (i = 0; i < BINS; i++) { 804 inv_obj.temp_valid_data[i] = 0; 805 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); 806 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); 807 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); 808 inv_obj.temp_valid_data[i] += (int)calData[ptr++]; 809 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 810 i, inv_obj.temp_valid_data[i]); 811 } 812 813 for (i = 0; i < BINS; i++) { 814 for (j = 0; j < PTS_PER_BIN; j++) { 815 tmp = 0; 816 tmp += 16777216LL * (long long)calData[ptr++]; 817 tmp += 65536LL * (long long)calData[ptr++]; 818 tmp += 256LL * (long long)calData[ptr++]; 819 tmp += (long long)calData[ptr++]; 820 if (tmp > 2147483648LL) { 821 tmp -= 4294967296LL; 822 } 823 inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; 824 LOADCAL_LOG("temp_data[%d][%d] = %f\n", 825 i, j, inv_obj.temp_data[i][j]); 826 } 827 } 828 829 for (i = 0; i < BINS; i++) { 830 for (j = 0; j < PTS_PER_BIN; j++) { 831 tmp = 0; 832 tmp += 16777216LL * (long long)calData[ptr++]; 833 tmp += 65536LL * (long long)calData[ptr++]; 834 tmp += 256LL * (long long)calData[ptr++]; 835 tmp += (long long)calData[ptr++]; 836 if (tmp > 2147483648LL) { 837 tmp -= 4294967296LL; 838 } 839 inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 840 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 841 i, j, inv_obj.x_gyro_temp_data[i][j]); 842 } 843 } 844 for (i = 0; i < BINS; i++) { 845 for (j = 0; j < PTS_PER_BIN; j++) { 846 tmp = 0; 847 tmp += 16777216LL * (long long)calData[ptr++]; 848 tmp += 65536LL * (long long)calData[ptr++]; 849 tmp += 256LL * (long long)calData[ptr++]; 850 tmp += (long long)calData[ptr++]; 851 if (tmp > 2147483648LL) { 852 tmp -= 4294967296LL; 853 } 854 inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 855 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 856 i, j, inv_obj.y_gyro_temp_data[i][j]); 857 } 858 } 859 for (i = 0; i < BINS; i++) { 860 for (j = 0; j < PTS_PER_BIN; j++) { 861 tmp = 0; 862 tmp += 16777216LL * (long long)calData[ptr++]; 863 tmp += 65536LL * (long long)calData[ptr++]; 864 tmp += 256LL * (long long)calData[ptr++]; 865 tmp += (long long)calData[ptr++]; 866 if (tmp > 2147483648LL) { 867 tmp -= 4294967296LL; 868 } 869 inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; 870 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 871 i, j, inv_obj.z_gyro_temp_data[i][j]); 872 } 873 } 874 875 /* read the accel biases */ 876 for (i = 0; i < 3; i++) { 877 uint32_t t = 0; 878 t += 16777216UL * ((uint32_t) calData[ptr++]); 879 t += 65536UL * ((uint32_t) calData[ptr++]); 880 t += 256u * ((uint32_t) calData[ptr++]); 881 t += (uint32_t) calData[ptr++]; 882 bias[i] = (int32_t) t; 883 LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 884 } 885 if (inv_set_array(INV_ACCEL_BIAS, bias)) { 886 LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); 887 return inv_set_array(INV_ACCEL_BIAS, bias); 888 } 889 890 /* read the compass biases */ 891 inv_reset_compass_calibration(); 892 893 inv_obj.got_compass_bias = (int)calData[ptr++]; 894 LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); 895 inv_obj.got_init_compass_bias = (int)calData[ptr++]; 896 LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); 897 inv_obj.compass_state = (int)calData[ptr++]; 898 LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state); 899 900 for (i = 0; i < 3; i++) { 901 uint32_t t = 0; 902 t += 16777216UL * ((uint32_t) calData[ptr++]); 903 t += 65536UL * ((uint32_t) calData[ptr++]); 904 t += 256u * ((uint32_t) calData[ptr++]); 905 t += (uint32_t) calData[ptr++]; 906 inv_obj.compass_bias_error[i] = (int32_t) t; 907 LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, 908 inv_obj.compass_bias_error[i]); 909 } 910 for (i = 0; i < 3; i++) { 911 uint32_t t = 0; 912 t += 16777216UL * ((uint32_t) calData[ptr++]); 913 t += 65536UL * ((uint32_t) calData[ptr++]); 914 t += 256u * ((uint32_t) calData[ptr++]); 915 t += (uint32_t) calData[ptr++]; 916 inv_obj.init_compass_bias[i] = (int32_t) t; 917 LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, 918 inv_obj.init_compass_bias[i]); 919 } 920 for (i = 0; i < 3; i++) { 921 uint32_t t = 0; 922 t += 16777216UL * ((uint32_t) calData[ptr++]); 923 t += 65536UL * ((uint32_t) calData[ptr++]); 924 t += 256u * ((uint32_t) calData[ptr++]); 925 t += (uint32_t) calData[ptr++]; 926 inv_obj.compass_bias[i] = (int32_t) t; 927 LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 928 } 929 for (i = 0; i < 18; i++) { 930 uint32_t t = 0; 931 t += 16777216UL * ((uint32_t) calData[ptr++]); 932 t += 65536UL * ((uint32_t) calData[ptr++]); 933 t += 256u * ((uint32_t) calData[ptr++]); 934 t += (uint32_t) calData[ptr++]; 935 inv_obj.compass_peaks[i] = (int32_t) t; 936 LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 937 } 938 for (i = 0; i < 3; i++) { 939 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 940 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 941 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 942 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 943 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 944 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 945 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 946 dToLL.ll += (unsigned long long)calData[ptr++]; 947 948 inv_obj.compass_bias_v[i] = dToLL.db; 949 LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); 950 } 951 for (i = 0; i < 9; i++) { 952 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 953 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 954 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 955 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 956 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 957 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 958 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 959 dToLL.ll += (unsigned long long)calData[ptr++]; 960 961 inv_obj.compass_bias_ptr[i] = dToLL.db; 962 LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 963 inv_obj.compass_bias_ptr[i]); 964 } 965 for (i = 0; i < 3; i++) { 966 uint32_t t = 0; 967 t += 16777216UL * ((uint32_t) calData[ptr++]); 968 t += 65536UL * ((uint32_t) calData[ptr++]); 969 t += 256u * ((uint32_t) calData[ptr++]); 970 t += (uint32_t) calData[ptr++]; 971 inv_obj.compass_scale[i] = (int32_t) t; 972 LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t); 973 } 974 for (i = 0; i < 6; i++) { 975 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 976 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 977 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 978 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 979 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 980 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 981 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 982 dToLL.ll += (unsigned long long)calData[ptr++]; 983 984 inv_obj.compass_prev_xty[i] = dToLL.db; 985 LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db); 986 } 987 for (i = 0; i < 36; i++) { 988 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); 989 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); 990 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); 991 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); 992 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); 993 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); 994 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); 995 dToLL.ll += (unsigned long long)calData[ptr++]; 996 997 inv_obj.compass_prev_m[i] = dToLL.db; 998 LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db); 999 } 1000 1001 /* Load the compass offset flag and values */ 1002 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++]; 1003 inv_obj.compass_offsets[0] = calData[ptr++]; 1004 inv_obj.compass_offsets[1] = calData[ptr++]; 1005 inv_obj.compass_offsets[2] = calData[ptr++]; 1006 1007 inv_obj.compass_accuracy = calData[ptr++]; 1008 /* push the compass offset values to the device */ 1009 result = inv_set_compass_offset(); 1010 1011 if (result == INV_SUCCESS) { 1012 if (inv_compass_check_range() != INV_SUCCESS) { 1013 MPL_LOGI("range check fail"); 1014 inv_reset_compass_calibration(); 1015 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 1016 inv_set_compass_offset(); 1017 } 1018 } 1019 1020 inv_obj.got_no_motion_bias = TRUE; 1021 LOADCAL_LOG("got_no_motion_bias = 1\n"); 1022 inv_obj.cal_loaded_flag = TRUE; 1023 LOADCAL_LOG("cal_loaded_flag = 1\n"); 1024 1025 LOADCAL_LOG("Exiting inv_load_cal_V4\n"); 1026 return INV_SUCCESS; 1027 } 1028 1029 /** 1030 * @brief Loads a type 5 set of calibration data. 1031 * It parses a binary data set containing calibration data. 1032 * The binary data set is intended to be loaded from a file. 1033 * This calibrations data format stores values for (in order of 1034 * appearance) : 1035 * - temperature, 1036 * - gyro biases for X, Y, Z axes, 1037 * - accel biases for X, Y, Z axes. 1038 * This calibration data would normally be produced by the MPU Self 1039 * Test and its size is 36 bytes (header and checksum included). 1040 * Calibration format type 5 is produced by the MPU Self Test and 1041 * substitutes the type 1 : inv_load_cal_V1(). 1042 * 1043 * @pre inv_dmp_open() 1044 * @ifnot MPL_MF 1045 * or inv_open_low_power_pedometer() 1046 * or inv_eis_open_dmp() 1047 * @endif 1048 * must have been called. 1049 * 1050 * @param calData 1051 * A pointer to an array of bytes to be parsed. 1052 * @param len 1053 * the length of the calibration 1054 * 1055 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1056 */ 1057 inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) 1058 { 1059 INVENSENSE_FUNC_START; 1060 const short expLen = 36; 1061 long accelBias[3] = { 0, 0, 0 }; 1062 float gyroBias[3] = { 0, 0, 0 }; 1063 1064 int ptr = INV_CAL_HDR_LEN; 1065 unsigned short temp; 1066 float newTemp; 1067 int bin; 1068 int i; 1069 1070 LOADCAL_LOG("Entering inv_load_cal_V5\n"); 1071 1072 if (len != expLen) { 1073 MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n", 1074 expLen, len); 1075 return INV_ERROR_FILE_READ; 1076 } 1077 1078 /* load the temperature */ 1079 temp = (unsigned short)calData[ptr++] * (1L << 8); 1080 temp += calData[ptr++]; 1081 newTemp = (float)inv_decode_temperature(temp) / 65536.f; 1082 LOADCAL_LOG("newTemp = %f\n", newTemp); 1083 1084 /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */ 1085 for (i = 0; i < 3; i++) { 1086 int32_t tmp = 0; 1087 tmp += (int32_t) calData[ptr++] * (1L << 24); 1088 tmp += (int32_t) calData[ptr++] * (1L << 16); 1089 tmp += (int32_t) calData[ptr++] * (1L << 8); 1090 tmp += (int32_t) calData[ptr++]; 1091 gyroBias[i] = (float)tmp / 65536.0f; 1092 LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]); 1093 } 1094 /* find the temperature bin */ 1095 bin = FindTempBin(newTemp); 1096 1097 /* populate the temp comp data structure */ 1098 inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; 1099 LOADCAL_LOG("temp_data[%d][%d] = %f\n", 1100 bin, inv_obj.temp_ptrs[bin], newTemp); 1101 1102 inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; 1103 LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 1104 bin, inv_obj.temp_ptrs[bin], gyroBias[0]); 1105 inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; 1106 LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 1107 bin, inv_obj.temp_ptrs[bin], gyroBias[1]); 1108 inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; 1109 LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 1110 bin, inv_obj.temp_ptrs[bin], gyroBias[2]); 1111 inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; 1112 LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); 1113 1114 if (inv_obj.temp_ptrs[bin] == 0) 1115 inv_obj.temp_valid_data[bin] = TRUE; 1116 LOADCAL_LOG("temp_valid_data[%d] = %ld\n", 1117 bin, inv_obj.temp_valid_data[bin]); 1118 1119 /* load accel biases (represented in 2 ^ 16 == 1 gee) 1120 and apply immediately */ 1121 for (i = 0; i < 3; i++) { 1122 int32_t tmp = 0; 1123 tmp += (int32_t) calData[ptr++] * (1L << 24); 1124 tmp += (int32_t) calData[ptr++] * (1L << 16); 1125 tmp += (int32_t) calData[ptr++] * (1L << 8); 1126 tmp += (int32_t) calData[ptr++]; 1127 accelBias[i] = (long)tmp; 1128 LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); 1129 } 1130 if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { 1131 LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); 1132 return inv_set_array(INV_ACCEL_BIAS, accelBias); 1133 } 1134 1135 inv_obj.got_no_motion_bias = TRUE; 1136 LOADCAL_LOG("got_no_motion_bias = 1\n"); 1137 inv_obj.cal_loaded_flag = TRUE; 1138 LOADCAL_LOG("cal_loaded_flag = 1\n"); 1139 1140 LOADCAL_LOG("Exiting inv_load_cal_V5\n"); 1141 return INV_SUCCESS; 1142 } 1143 1144 /** 1145 * @brief Loads a set of calibration data. 1146 * It parses a binary data set containing calibration data. 1147 * The binary data set is intended to be loaded from a file. 1148 * 1149 * @pre inv_dmp_open() 1150 * @ifnot MPL_MF 1151 * or inv_open_low_power_pedometer() 1152 * or inv_eis_open_dmp() 1153 * @endif 1154 * must have been called. 1155 * 1156 * @param calData 1157 * A pointer to an array of bytes to be parsed. 1158 * 1159 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1160 */ 1161 inv_error_t inv_load_cal(unsigned char *calData) 1162 { 1163 INVENSENSE_FUNC_START; 1164 int calType = 0; 1165 int len = 0; 1166 int ptr; 1167 uint32_t chk = 0; 1168 uint32_t cmp_chk = 0; 1169 1170 tMLLoadFunc loaders[] = { 1171 inv_load_cal_V0, 1172 inv_load_cal_V1, 1173 inv_load_cal_V2, 1174 inv_load_cal_V3, 1175 inv_load_cal_V4, 1176 inv_load_cal_V5 1177 }; 1178 1179 if (inv_get_state() < INV_STATE_DMP_OPENED) 1180 return INV_ERROR_SM_IMPROPER_STATE; 1181 1182 /* read the header (type and len) 1183 len is the total record length including header and checksum */ 1184 len = 0; 1185 len += 16777216L * ((int)calData[0]); 1186 len += 65536L * ((int)calData[1]); 1187 len += 256 * ((int)calData[2]); 1188 len += (int)calData[3]; 1189 1190 calType = ((int)calData[4]) * 256 + ((int)calData[5]); 1191 if (calType > 5) { 1192 MPL_LOGE("Unsupported calibration file format %d. " 1193 "Valid types 0..5\n", calType); 1194 return INV_ERROR_INVALID_PARAMETER; 1195 } 1196 1197 /* check the checksum */ 1198 chk = 0; 1199 ptr = len - INV_CAL_CHK_LEN; 1200 1201 chk += 16777216L * ((uint32_t) calData[ptr++]); 1202 chk += 65536L * ((uint32_t) calData[ptr++]); 1203 chk += 256 * ((uint32_t) calData[ptr++]); 1204 chk += (uint32_t) calData[ptr++]; 1205 1206 cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN, 1207 len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); 1208 1209 if (chk != cmp_chk) { 1210 return INV_ERROR_CALIBRATION_CHECKSUM; 1211 } 1212 1213 /* call the proper method to read in the data */ 1214 return loaders[calType] (calData, len); 1215 } 1216 1217 /** 1218 * @brief Stores a set of calibration data. 1219 * It generates a binary data set containing calibration data. 1220 * The binary data set is intended to be stored into a file. 1221 * 1222 * @pre inv_dmp_open() 1223 * 1224 * @param calData 1225 * A pointer to an array of bytes to be stored. 1226 * @param length 1227 * The amount of bytes available in the array. 1228 * 1229 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1230 */ 1231 inv_error_t inv_store_cal(unsigned char *calData, int length) 1232 { 1233 INVENSENSE_FUNC_START; 1234 int ptr = 0; 1235 int i = 0; 1236 int j = 0; 1237 long long tmp; 1238 uint32_t chk; 1239 long bias[3]; 1240 //unsigned char state; 1241 union doubleToLongLong { 1242 double db; 1243 unsigned long long ll; 1244 } dToLL; 1245 1246 if (inv_get_state() < INV_STATE_DMP_OPENED) 1247 return INV_ERROR_SM_IMPROPER_STATE; 1248 1249 STORECAL_LOG("Entering inv_store_cal\n"); 1250 1251 // length 1252 calData[0] = (unsigned char)((length >> 24) & 0xff); 1253 calData[1] = (unsigned char)((length >> 16) & 0xff); 1254 calData[2] = (unsigned char)((length >> 8) & 0xff); 1255 calData[3] = (unsigned char)(length & 0xff); 1256 STORECAL_LOG("calLen = %d\n", length); 1257 1258 // calibration data format type 1259 calData[4] = 0; 1260 calData[5] = 4; 1261 STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]); 1262 1263 // data 1264 ptr = 6; 1265 for (i = 0; i < BINS; i++) { 1266 tmp = (int)inv_obj.temp_ptrs[i]; 1267 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1268 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1269 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1270 calData[ptr++] = (unsigned char)(tmp & 0xff); 1271 STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp); 1272 } 1273 1274 for (i = 0; i < BINS; i++) { 1275 tmp = (int)inv_obj.temp_valid_data[i]; 1276 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1277 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1278 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1279 calData[ptr++] = (unsigned char)(tmp & 0xff); 1280 STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp); 1281 } 1282 for (i = 0; i < BINS; i++) { 1283 for (j = 0; j < PTS_PER_BIN; j++) { 1284 tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f); 1285 if (tmp < 0) { 1286 tmp += 4294967296LL; 1287 } 1288 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1289 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1290 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1291 calData[ptr++] = (unsigned char)(tmp & 0xff); 1292 STORECAL_LOG("temp_data[%d][%d] = %f\n", 1293 i, j, inv_obj.temp_data[i][j]); 1294 } 1295 } 1296 1297 for (i = 0; i < BINS; i++) { 1298 for (j = 0; j < PTS_PER_BIN; j++) { 1299 tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f); 1300 if (tmp < 0) { 1301 tmp += 4294967296LL; 1302 } 1303 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1304 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1305 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1306 calData[ptr++] = (unsigned char)(tmp & 0xff); 1307 STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", 1308 i, j, inv_obj.x_gyro_temp_data[i][j]); 1309 } 1310 } 1311 for (i = 0; i < BINS; i++) { 1312 for (j = 0; j < PTS_PER_BIN; j++) { 1313 tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f); 1314 if (tmp < 0) { 1315 tmp += 4294967296LL; 1316 } 1317 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1318 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1319 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1320 calData[ptr++] = (unsigned char)(tmp & 0xff); 1321 STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", 1322 i, j, inv_obj.y_gyro_temp_data[i][j]); 1323 } 1324 } 1325 for (i = 0; i < BINS; i++) { 1326 for (j = 0; j < PTS_PER_BIN; j++) { 1327 tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f); 1328 if (tmp < 0) { 1329 tmp += 4294967296LL; 1330 } 1331 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); 1332 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); 1333 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); 1334 calData[ptr++] = (unsigned char)(tmp & 0xff); 1335 STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", 1336 i, j, inv_obj.z_gyro_temp_data[i][j]); 1337 } 1338 } 1339 1340 inv_get_array(INV_ACCEL_BIAS, bias); 1341 1342 /* write the accel biases */ 1343 for (i = 0; i < 3; i++) { 1344 uint32_t t = (uint32_t) bias[i]; 1345 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1346 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1347 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1348 calData[ptr++] = (unsigned char)(t & 0xff); 1349 STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); 1350 } 1351 1352 /* write the compass calibration state */ 1353 calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias); 1354 STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); 1355 calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias); 1356 STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); 1357 if (inv_obj.compass_state == SF_UNCALIBRATED) { 1358 calData[ptr++] = SF_UNCALIBRATED; 1359 } else { 1360 calData[ptr++] = SF_STARTUP_SETTLE; 1361 } 1362 STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state); 1363 1364 for (i = 0; i < 3; i++) { 1365 uint32_t t = (uint32_t) inv_obj.compass_bias_error[i]; 1366 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1367 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1368 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1369 calData[ptr++] = (unsigned char)(t & 0xff); 1370 STORECAL_LOG("compass_bias_error[%d] = %ld\n", 1371 i, inv_obj.compass_bias_error[i]); 1372 } 1373 for (i = 0; i < 3; i++) { 1374 uint32_t t = (uint32_t) inv_obj.init_compass_bias[i]; 1375 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1376 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1377 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1378 calData[ptr++] = (unsigned char)(t & 0xff); 1379 STORECAL_LOG("init_compass_bias[%d] = %ld\n", i, 1380 inv_obj.init_compass_bias[i]); 1381 } 1382 for (i = 0; i < 3; i++) { 1383 uint32_t t = (uint32_t) inv_obj.compass_bias[i]; 1384 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1385 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1386 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1387 calData[ptr++] = (unsigned char)(t & 0xff); 1388 STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); 1389 } 1390 for (i = 0; i < 18; i++) { 1391 uint32_t t = (uint32_t) inv_obj.compass_peaks[i]; 1392 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1393 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1394 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1395 calData[ptr++] = (unsigned char)(t & 0xff); 1396 STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); 1397 } 1398 for (i = 0; i < 3; i++) { 1399 dToLL.db = inv_obj.compass_bias_v[i]; 1400 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1401 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1402 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1403 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1404 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1405 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1406 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1407 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1408 STORECAL_LOG("compass_bias_v[%d] = %lf\n", i, 1409 inv_obj.compass_bias_v[i]); 1410 } 1411 for (i = 0; i < 9; i++) { 1412 dToLL.db = inv_obj.compass_bias_ptr[i]; 1413 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1414 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1415 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1416 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1417 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1418 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1419 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1420 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1421 STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i, 1422 inv_obj.compass_bias_ptr[i]); 1423 } 1424 for (i = 0; i < 3; i++) { 1425 uint32_t t = (uint32_t) inv_obj.compass_scale[i]; 1426 calData[ptr++] = (unsigned char)((t >> 24) & 0xff); 1427 calData[ptr++] = (unsigned char)((t >> 16) & 0xff); 1428 calData[ptr++] = (unsigned char)((t >> 8) & 0xff); 1429 calData[ptr++] = (unsigned char)(t & 0xff); 1430 STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]); 1431 } 1432 for (i = 0; i < 6; i++) { 1433 dToLL.db = inv_obj.compass_prev_xty[i]; 1434 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1435 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1436 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1437 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1438 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1439 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1440 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1441 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1442 STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i, 1443 inv_obj.compass_prev_xty[i]); 1444 } 1445 for (i = 0; i < 36; i++) { 1446 dToLL.db = inv_obj.compass_prev_m[i]; 1447 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); 1448 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); 1449 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); 1450 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); 1451 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); 1452 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); 1453 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); 1454 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); 1455 STORECAL_LOG("compass_prev_m[%d] = %lf\n", i, 1456 inv_obj.compass_prev_m[i]); 1457 } 1458 1459 /* store the compass offsets and validity flag */ 1460 calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID]; 1461 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0]; 1462 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1]; 1463 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2]; 1464 1465 /* store the compass accuracy */ 1466 calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy); 1467 1468 /* add a checksum */ 1469 chk = inv_checksum(calData + INV_CAL_HDR_LEN, 1470 length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); 1471 calData[ptr++] = (unsigned char)((chk >> 24) & 0xff); 1472 calData[ptr++] = (unsigned char)((chk >> 16) & 0xff); 1473 calData[ptr++] = (unsigned char)((chk >> 8) & 0xff); 1474 calData[ptr++] = (unsigned char)(chk & 0xff); 1475 1476 STORECAL_LOG("Exiting inv_store_cal\n"); 1477 return INV_SUCCESS; 1478 } 1479 1480 /** 1481 * @brief Load a calibration file. 1482 * 1483 * @pre Must be in INV_STATE_DMP_OPENED state. 1484 * inv_dmp_open() or inv_dmp_stop() must have been called. 1485 * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 1486 * been called. 1487 * 1488 * @return 0 or error code. 1489 */ 1490 inv_error_t inv_load_calibration(void) 1491 { 1492 unsigned char *calData; 1493 inv_error_t result; 1494 unsigned int length; 1495 1496 if (inv_get_state() < INV_STATE_DMP_OPENED) 1497 return INV_ERROR_SM_IMPROPER_STATE; 1498 1499 result = inv_serial_get_cal_length(&length); 1500 if (result == INV_ERROR_FILE_OPEN) { 1501 MPL_LOGI("Calibration data not loaded\n"); 1502 return INV_SUCCESS; 1503 } 1504 if (result || length <= 0) { 1505 MPL_LOGE("Could not get file calibration length - " 1506 "error %d - aborting\n", result); 1507 return result; 1508 } 1509 calData = (unsigned char *)inv_malloc(length); 1510 if (!calData) { 1511 MPL_LOGE("Could not allocate buffer of %d bytes - " 1512 "aborting\n", length); 1513 return INV_ERROR_MEMORY_EXAUSTED; 1514 } 1515 result = inv_serial_read_cal(calData, length); 1516 if (result) { 1517 MPL_LOGE("Could not read the calibration data from file - " 1518 "error %d - aborting\n", result); 1519 goto free_mem_n_exit; 1520 1521 } 1522 result = inv_load_cal(calData); 1523 if (result) { 1524 MPL_LOGE("Could not load the calibration data - " 1525 "error %d - aborting\n", result); 1526 goto free_mem_n_exit; 1527 1528 } 1529 1530 1531 1532 free_mem_n_exit: 1533 inv_free(calData); 1534 return INV_SUCCESS; 1535 } 1536 1537 /** 1538 * @brief Store runtime calibration data to a file 1539 * 1540 * @pre Must be in INV_STATE_DMP_OPENED state. 1541 * inv_dmp_open() or inv_dmp_stop() must have been called. 1542 * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> 1543 * been called. 1544 * 1545 * @return 0 or error code. 1546 */ 1547 inv_error_t inv_store_calibration(void) 1548 { 1549 unsigned char *calData; 1550 inv_error_t result; 1551 unsigned int length; 1552 1553 if (inv_get_state() < INV_STATE_DMP_OPENED) 1554 return INV_ERROR_SM_IMPROPER_STATE; 1555 1556 length = inv_get_cal_length(); 1557 calData = (unsigned char *)inv_malloc(length); 1558 if (!calData) { 1559 MPL_LOGE("Could not allocate buffer of %d bytes - " 1560 "aborting\n", length); 1561 return INV_ERROR_MEMORY_EXAUSTED; 1562 } 1563 result = inv_store_cal(calData, length); 1564 if (result) { 1565 MPL_LOGE("Could not store calibrated data on file - " 1566 "error %d - aborting\n", result); 1567 goto free_mem_n_exit; 1568 1569 } 1570 result = inv_serial_write_cal(calData, length); 1571 if (result) { 1572 MPL_LOGE("Could not write calibration data - " "error %d\n", result); 1573 goto free_mem_n_exit; 1574 1575 } 1576 1577 1578 1579 free_mem_n_exit: 1580 inv_free(calData); 1581 return INV_SUCCESS; 1582 } 1583 1584 /** 1585 * @} 1586 */ 1587
