diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/ml_stored_data.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/ml_stored_data.c | 1586 |
1 files changed, 1586 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/ml_stored_data.c b/libsensors/mlsdk/mllite/ml_stored_data.c new file mode 100644 index 0000000..35da3e8 --- /dev/null +++ b/libsensors/mlsdk/mllite/ml_stored_data.c @@ -0,0 +1,1586 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_STORED_DATA + * + * @{ + * @file ml_stored_data.c + * @brief functions for reading and writing stored data sets. + * Typically, these functions process stored calibration data. + */ + +#include "ml_stored_data.h" +#include "ml.h" +#include "mlFIFO.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "compass.h" +#include "dmpKey.h" +#include "dmpDefault.h" +#include "mlstates.h" +#include "checksum.h" +#include "mlsupervisor.h" + +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + +/* + Typedefs +*/ +typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short); + +/* + Globals +*/ +extern struct inv_obj_t inv_obj; + +/* + Debugging Definitions + set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields + being read or stored in human-readable format. + When set to 0, the compiler will optimize and remove the printouts + from the code. +*/ +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define LOADCAL_LOG(...) \ + if(LOADCAL_DEBUG) \ + MPL_LOGI("LOADCAL: " __VA_ARGS__) +#define STORECAL_LOG(...) \ + if(STORECAL_DEBUG) \ + MPL_LOGI("STORECAL: " __VA_ARGS__) + +/** + * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl + * advanced algorithms library. To remove cross-dependency, for now, + * we reimplement the same function here. + * @param temp + * the temperature (1 count == 1 degree C). + */ +int FindTempBin(float temp) +{ + int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN); + if (bin < 0) + bin = 0; + if (bin > BINS - 1) + bin = BINS - 1; + return bin; +} + +/** + * @brief Returns the length of the <b>MPL internal calibration data</b>. + * Should be called before allocating the memory required to store + * this data to a file. + * This function returns the total size required to store the cal + * data including the header (4 bytes) and the checksum (2 bytes). + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return the length of the internal calibrated data format. + */ +unsigned int inv_get_cal_length(void) +{ + INVENSENSE_FUNC_START; + unsigned int length; + length = INV_CAL_HDR_LEN + // header + BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal + INV_CAL_ACCEL_LEN + // accel cal + INV_CAL_COMPASS_LEN + // compass cal + INV_CAL_CHK_LEN; // checksum + return length; +} + +/** + * @brief Loads a type 0 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 18 bytes (header and checksum included). + * Calibration format type 0 is currently <b>NOT</b> used and + * is substituted by type 5 : inv_load_cal_V5(). + * + * @note This calibration data format is obsoleted and no longer supported + * by the rest of the MPL + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 18; + long newGyroData[3] = { 0, 0, 0 }; + float newTemp; + int bin; + + LOADCAL_LOG("Entering inv_load_cal_V0\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); + return INV_ERROR_FILE_READ; + } + + newTemp = (float)inv_decode_temperature( + (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); + if (newGyroData[0] > 32767L) + newGyroData[0] -= 65536L; + LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); + newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); + if (newGyroData[1] > 32767L) + newGyroData[1] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); + if (newGyroData[2] > 32767L) + newGyroData[2] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + + bin = FindTempBin(newTemp); + LOADCAL_LOG("bin = %d", bin); + + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f", + bin, inv_obj.temp_ptrs[bin], + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V0\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 1 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 24 bytes (header and checksum included). + * Calibration format type 1 is currently <b>NOT</b> used and + * is substituted by type 5 : inv_load_cal_V5(). + * + * @note In order to successfully work, the gyro bias must be stored + * expressed in 250 dps full scale (131.072 sensitivity). Other full + * scale range will produce unpredictable results in the gyro biases. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 24; + long newGyroData[3] = {0, 0, 0}; + long accelBias[3] = {0, 0, 0}; + float gyroBias[3] = {0, 0, 0}; + float newTemp = 0; + int bin; + + LOADCAL_LOG("Entering inv_load_cal_V1\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); + return INV_ERROR_FILE_READ; + } + + newTemp = (float)inv_decode_temperature( + (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); + if (newGyroData[0] > 32767L) + newGyroData[0] -= 65536L; + LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); + newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); + if (newGyroData[1] > 32767L) + newGyroData[1] -= 65536L; + LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]); + newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); + if (newGyroData[2] > 32767L) + newGyroData[2] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + + bin = FindTempBin(newTemp); + LOADCAL_LOG("bin = %d\n", bin); + + gyroBias[0] = ((float)newGyroData[0]) / 131.072f; + gyroBias[1] = ((float)newGyroData[1]) / 131.072f; + gyroBias[2] = ((float)newGyroData[2]) / 131.072f; + LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]); + LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]); + LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]); + + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f", + bin, inv_obj.temp_ptrs[bin], + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + /* load accel biases and apply immediately */ + accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]); + if (accelBias[0] > 32767) + accelBias[0] -= 65536L; + accelBias[0] = (long)((long long)accelBias[0] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]); + accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]); + if (accelBias[1] > 32767) + accelBias[1] -= 65536L; + accelBias[1] = (long)((long long)accelBias[1] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]); + accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]); + if (accelBias[2] > 32767) + accelBias[2] -= 65536L; + accelBias[2] = (long)((long long)accelBias[2] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); + if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); + return inv_set_array(INV_ACCEL_BIAS, accelBias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V1\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 2 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * This calibration data is produced internally by the MPL and its + * size is 2222 bytes (header and checksum included). + * Calibration format type 2 is currently <b>NOT</b> used and + * is substituted by type 4 : inv_load_cal_V4(). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 2222; + long accel_bias[3]; + int ptr = INV_CAL_HDR_LEN; + + int i, j; + long long tmp; + + LOADCAL_LOG("Entering inv_load_cal_V2\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + accel_bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); + } + + if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); + return inv_set_array(INV_ACCEL_BIAS, accel_bias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V2\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 3 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * - compass biases for X, Y, Z axes and bias tracking algorithm + * mock-up. + * This calibration data is produced internally by the MPL and its + * size is 2429 bytes (header and checksum included). + * Calibration format type 3 is currently <b>NOT</b> used and + * is substituted by type 4 : inv_load_cal_V4(). + + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + const short expLen = 2429; + long bias[3]; + int i, j; + int ptr = INV_CAL_HDR_LEN; + long long tmp; + + LOADCAL_LOG("Entering inv_load_cal_V3\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); + return inv_set_array(INV_ACCEL_BIAS, bias); + } + + /* read the compass biases */ + inv_obj.got_compass_bias = (int)calData[ptr++]; + inv_obj.got_init_compass_bias = (int)calData[ptr++]; + inv_obj.compass_state = (int)calData[ptr++]; + + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias_error[i] = (int32_t) t; + LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, + inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.init_compass_bias[i] = (int32_t) t; + LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias[i] = (int32_t) t; + LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_peaks[i] = (int32_t) t; + LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.ll = 0; + dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_v[i] = dToLL.db; + LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.ll = 0; + dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_ptr[i] = dToLL.db; + LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V3\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 4 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * - compass biases for X, Y, Z axes, compass scale, and bias + * tracking algorithm mock-up. + * This calibration data is produced internally by the MPL and its + * size is 2777 bytes (header and checksum included). + * Calibration format type 4 is currently used and + * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + const unsigned int expLen = 2782; + long bias[3]; + int ptr = INV_CAL_HDR_LEN; + int i, j; + long long tmp; + inv_error_t result; + + LOADCAL_LOG("Entering inv_load_cal_V4\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); + return inv_set_array(INV_ACCEL_BIAS, bias); + } + + /* read the compass biases */ + inv_reset_compass_calibration(); + + inv_obj.got_compass_bias = (int)calData[ptr++]; + LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); + inv_obj.got_init_compass_bias = (int)calData[ptr++]; + LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); + inv_obj.compass_state = (int)calData[ptr++]; + LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state); + + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias_error[i] = (int32_t) t; + LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, + inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.init_compass_bias[i] = (int32_t) t; + LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias[i] = (int32_t) t; + LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_peaks[i] = (int32_t) t; + LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_v[i] = dToLL.db; + LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_ptr[i] = dToLL.db; + LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_scale[i] = (int32_t) t; + LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t); + } + for (i = 0; i < 6; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_prev_xty[i] = dToLL.db; + LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db); + } + for (i = 0; i < 36; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_prev_m[i] = dToLL.db; + LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db); + } + + /* Load the compass offset flag and values */ + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++]; + inv_obj.compass_offsets[0] = calData[ptr++]; + inv_obj.compass_offsets[1] = calData[ptr++]; + inv_obj.compass_offsets[2] = calData[ptr++]; + + inv_obj.compass_accuracy = calData[ptr++]; + /* push the compass offset values to the device */ + result = inv_set_compass_offset(); + + if (result == INV_SUCCESS) { + if (inv_compass_check_range() != INV_SUCCESS) { + MPL_LOGI("range check fail"); + inv_reset_compass_calibration(); + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; + inv_set_compass_offset(); + } + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V4\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 5 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 36 bytes (header and checksum included). + * Calibration format type 5 is produced by the MPU Self Test and + * substitutes the type 1 : inv_load_cal_V1(). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 36; + long accelBias[3] = { 0, 0, 0 }; + float gyroBias[3] = { 0, 0, 0 }; + + int ptr = INV_CAL_HDR_LEN; + unsigned short temp; + float newTemp; + int bin; + int i; + + LOADCAL_LOG("Entering inv_load_cal_V5\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + /* load the temperature */ + temp = (unsigned short)calData[ptr++] * (1L << 8); + temp += calData[ptr++]; + newTemp = (float)inv_decode_temperature(temp) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */ + for (i = 0; i < 3; i++) { + int32_t tmp = 0; + tmp += (int32_t) calData[ptr++] * (1L << 24); + tmp += (int32_t) calData[ptr++] * (1L << 16); + tmp += (int32_t) calData[ptr++] * (1L << 8); + tmp += (int32_t) calData[ptr++]; + gyroBias[i] = (float)tmp / 65536.0f; + LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]); + } + /* find the temperature bin */ + bin = FindTempBin(newTemp); + + /* populate the temp comp data structure */ + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], newTemp); + + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[0]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[1]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[2]); + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + /* load accel biases (represented in 2 ^ 16 == 1 gee) + and apply immediately */ + for (i = 0; i < 3; i++) { + int32_t tmp = 0; + tmp += (int32_t) calData[ptr++] * (1L << 24); + tmp += (int32_t) calData[ptr++] * (1L << 16); + tmp += (int32_t) calData[ptr++] * (1L << 8); + tmp += (int32_t) calData[ptr++]; + accelBias[i] = (long)tmp; + LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); + return inv_set_array(INV_ACCEL_BIAS, accelBias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V5\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal(unsigned char *calData) +{ + INVENSENSE_FUNC_START; + int calType = 0; + int len = 0; + int ptr; + uint32_t chk = 0; + uint32_t cmp_chk = 0; + + tMLLoadFunc loaders[] = { + inv_load_cal_V0, + inv_load_cal_V1, + inv_load_cal_V2, + inv_load_cal_V3, + inv_load_cal_V4, + inv_load_cal_V5 + }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* read the header (type and len) + len is the total record length including header and checksum */ + len = 0; + len += 16777216L * ((int)calData[0]); + len += 65536L * ((int)calData[1]); + len += 256 * ((int)calData[2]); + len += (int)calData[3]; + + calType = ((int)calData[4]) * 256 + ((int)calData[5]); + if (calType > 5) { + MPL_LOGE("Unsupported calibration file format %d. " + "Valid types 0..5\n", calType); + return INV_ERROR_INVALID_PARAMETER; + } + + /* check the checksum */ + chk = 0; + ptr = len - INV_CAL_CHK_LEN; + + chk += 16777216L * ((uint32_t) calData[ptr++]); + chk += 65536L * ((uint32_t) calData[ptr++]); + chk += 256 * ((uint32_t) calData[ptr++]); + chk += (uint32_t) calData[ptr++]; + + cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN, + len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); + + if (chk != cmp_chk) { + return INV_ERROR_CALIBRATION_CHECKSUM; + } + + /* call the proper method to read in the data */ + return loaders[calType] (calData, len); +} + +/** + * @brief Stores a set of calibration data. + * It generates a binary data set containing calibration data. + * The binary data set is intended to be stored into a file. + * + * @pre inv_dmp_open() + * + * @param calData + * A pointer to an array of bytes to be stored. + * @param length + * The amount of bytes available in the array. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_store_cal(unsigned char *calData, int length) +{ + INVENSENSE_FUNC_START; + int ptr = 0; + int i = 0; + int j = 0; + long long tmp; + uint32_t chk; + long bias[3]; + //unsigned char state; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + STORECAL_LOG("Entering inv_store_cal\n"); + + // length + calData[0] = (unsigned char)((length >> 24) & 0xff); + calData[1] = (unsigned char)((length >> 16) & 0xff); + calData[2] = (unsigned char)((length >> 8) & 0xff); + calData[3] = (unsigned char)(length & 0xff); + STORECAL_LOG("calLen = %d\n", length); + + // calibration data format type + calData[4] = 0; + calData[5] = 4; + STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]); + + // data + ptr = 6; + for (i = 0; i < BINS; i++) { + tmp = (int)inv_obj.temp_ptrs[i]; + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp); + } + + for (i = 0; i < BINS; i++) { + tmp = (int)inv_obj.temp_valid_data[i]; + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp); + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + inv_get_array(INV_ACCEL_BIAS, bias); + + /* write the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + + /* write the compass calibration state */ + calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias); + STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); + calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias); + STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); + if (inv_obj.compass_state == SF_UNCALIBRATED) { + calData[ptr++] = SF_UNCALIBRATED; + } else { + calData[ptr++] = SF_STARTUP_SETTLE; + } + STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state); + + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_bias_error[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_bias_error[%d] = %ld\n", + i, inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.init_compass_bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = (uint32_t) inv_obj.compass_peaks[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.db = inv_obj.compass_bias_v[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_bias_v[%d] = %lf\n", i, + inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.db = inv_obj.compass_bias_ptr[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_scale[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]); + } + for (i = 0; i < 6; i++) { + dToLL.db = inv_obj.compass_prev_xty[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i, + inv_obj.compass_prev_xty[i]); + } + for (i = 0; i < 36; i++) { + dToLL.db = inv_obj.compass_prev_m[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_prev_m[%d] = %lf\n", i, + inv_obj.compass_prev_m[i]); + } + + /* store the compass offsets and validity flag */ + calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2]; + + /* store the compass accuracy */ + calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy); + + /* add a checksum */ + chk = inv_checksum(calData + INV_CAL_HDR_LEN, + length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); + calData[ptr++] = (unsigned char)((chk >> 24) & 0xff); + calData[ptr++] = (unsigned char)((chk >> 16) & 0xff); + calData[ptr++] = (unsigned char)((chk >> 8) & 0xff); + calData[ptr++] = (unsigned char)(chk & 0xff); + + STORECAL_LOG("Exiting inv_store_cal\n"); + return INV_SUCCESS; +} + +/** + * @brief Load a calibration file. + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_load_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + unsigned int length; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + result = inv_serial_get_cal_length(&length); + if (result == INV_ERROR_FILE_OPEN) { + MPL_LOGI("Calibration data not loaded\n"); + return INV_SUCCESS; + } + if (result || length <= 0) { + MPL_LOGE("Could not get file calibration length - " + "error %d - aborting\n", result); + return result; + } + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + result = inv_serial_read_cal(calData, length); + if (result) { + MPL_LOGE("Could not read the calibration data from file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + result = inv_load_cal(calData); + if (result) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + + + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + unsigned int length; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + length = inv_get_cal_length(); + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + result = inv_store_cal(calData, length); + if (result) { + MPL_LOGE("Could not store calibrated data on file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + result = inv_serial_write_cal(calData, length); + if (result) { + MPL_LOGE("Could not write calibration data - " "error %d\n", result); + goto free_mem_n_exit; + + } + + + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @} + */ |