/*
 $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;
}

/**
 *  @}
 */