/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
/******************************************************************************
 *
 * $Id: ml.c 5773 2011-07-14 22:28:32Z nroyer $
 *
 *****************************************************************************/

/**
 *  @defgroup ML
 *  @brief  Motion Library APIs.
 *          The Motion Library processes gyroscopes, accelerometers, and
 *          compasses to provide a physical model of the movement for the
 *          sensors.
 *          The results of this processing may be used to control objects
 *          within a user interface environment, detect gestures, track 3D
 *          movement for gaming applications, and analyze the blur created
 *          due to hand movement while taking a picture.
 *
 *  @{
 *      @file   ml.c
 *      @brief  The Motion Library APIs.
 */

/* ------------------ */
/* - Include Files. - */
/* ------------------ */

#include <string.h>

#include "ml.h"
#include "mldl.h"
#include "mltypes.h"
#include "mlinclude.h"
#include "compass.h"
#include "dmpKey.h"
#include "dmpDefault.h"
#include "mlstates.h"
#include "mlFIFO.h"
#include "mlFIFOHW.h"
#include "mlMathFunc.h"
#include "mlsupervisor.h"
#include "mlmath.h"
#include "mlcontrol.h"
#include "mldl_cfg.h"
#include "mpu.h"
#include "accel.h"
#include "mlos.h"
#include "mlsl.h"
#include "mlos.h"
#include "mlBiasNoMotion.h"
#include "temp_comp.h"
#include "log.h"

#undef MPL_LOG_TAG
#define MPL_LOG_TAG "MPL-ml"

#define ML_MOT_TYPE_NONE 0
#define ML_MOT_TYPE_NO_MOTION 1
#define ML_MOT_TYPE_MOTION_DETECTED 2

#define ML_MOT_STATE_MOVING 0
#define ML_MOT_STATE_NO_MOTION 1
#define ML_MOT_STATE_BIAS_IN_PROG 2

#define _mlDebug(x)             //{x}

/* Global Variables */
const unsigned char mlVer[] = { INV_VERSION };

struct inv_params_obj inv_params_obj = {
    INV_BIAS_UPDATE_FUNC_DEFAULT,   // bias_mode
    INV_ORIENTATION_MASK_DEFAULT,   // orientation_mask
    INV_PROCESSED_DATA_CALLBACK_DEFAULT,    // fifo_processed_func
    INV_ORIENTATION_CALLBACK_DEFAULT,   // orientation_cb_func
    INV_MOTION_CALLBACK_DEFAULT,    // motion_cb_func
    INV_STATE_SERIAL_CLOSED     // starting state
};

void *g_mlsl_handle;

typedef struct {
    // These describe callbacks happening everythime a DMP interrupt is processed
    int_fast8_t numInterruptProcesses;
    // Array of function pointers, function being void function taking void
    inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES];

} tMLXCallbackInterrupt;        // MLX_callback_t

tMLXCallbackInterrupt mlxCallbackInterrupt;

void inv_init_ml_cb (void)
{
    memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); 
}


/* --------------- */
/* -  Functions. - */
/* --------------- */

inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient);
inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient);
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
void inv_mpu6050_accel(unsigned short orient, int mode, unsigned char *instr);

/**
 *  @brief  Open serial connection with the MPU device.
 *          This is the entry point of the MPL and must be
 *          called prior to any other function call.
 *
 *  @param  port     System handle for 'port' MPU device is found on.
 *                   The significance of this parameter varies by
 *                   platform. It is passed as 'port' to MLSLSerialOpen.
 *
 *  @return INV_SUCCESS or error code.
 */
inv_error_t inv_serial_start(char const *port)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;

    if (inv_get_state() >= INV_STATE_SERIAL_OPENED)
        return INV_SUCCESS;

    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    result = inv_serial_open(port, &g_mlsl_handle);
    if (INV_SUCCESS != result) {
        (void)inv_state_transition(INV_STATE_SERIAL_CLOSED);
    }

    return result;
}

/**
 *  @brief  Close the serial communication.
 *          This function needs to be called explicitly to shut down
 *          the communication with the device.  Calling inv_dmp_close()
 *          won't affect the established serial communication.
 *  @return INV_SUCCESS; non-zero error code otherwise.
 */
inv_error_t inv_serial_stop(void)
{
    INVENSENSE_FUNC_START;
    inv_error_t result = INV_SUCCESS;

    if (inv_get_state() == INV_STATE_SERIAL_CLOSED)
        return INV_SUCCESS;

    result = inv_state_transition(INV_STATE_SERIAL_CLOSED);
    if (INV_SUCCESS != result) {
        MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result);
    }
    result = inv_serial_close(g_mlsl_handle);
    if (INV_SUCCESS != result) {
        MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result);
    }
    return result;
}

/**
 *  @brief  Get the serial file handle to the device.
 *  @return The serial file handle.
 */
void *inv_get_serial_handle(void)
{
    INVENSENSE_FUNC_START;
    return g_mlsl_handle;
}


inv_error_t inv_read_compass_asa( long * asa )
{
    INVENSENSE_FUNC_START;
    int ii;
    long tmp[COMPASS_NUM_AXES];
    inv_error_t result;

    result = inv_compass_read_scale(tmp);
    if (result == INV_SUCCESS) {
        for (ii = 0; ii < COMPASS_NUM_AXES; ii++)
            asa[ii] = tmp[ii];
    }
    return result;
}

/**
 *  @brief  apply the choosen orientation and full scale range
 *          for gyroscopes, accelerometer, and compass.
 *  @return INV_SUCCESS if successful, a non-zero code otherwise.
 */
inv_error_t inv_apply_calibration(void)
{
    INVENSENSE_FUNC_START;
    signed char gyroCal[9] = { 0 };
    signed char accelCal[9] = { 0 };
    signed char magCal[9] = { 0 };
    float gyroScale = 2000.f;
    float accelScale = 0.f;
    float magScale = 0.f;

    inv_error_t result;
    int ii;
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();

    for (ii = 0; ii < 9; ii++) {
        gyroCal[ii] = mldl_cfg->pdata->orientation[ii];
    }
    if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]) {
        for (ii = 0; ii < 9; ii++) {
            accelCal[ii] =
                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->orientation[ii];
        }
        RANGE_FIXEDPOINT_TO_FLOAT(
            mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->range, accelScale);
        inv_obj.accel->sens = (long)(accelScale * 65536L);
        /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
        if (ACCEL_ID_MPU6050 == mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) {
            /* the line below is an optimized version of:
               accel_sens      = accel_sens / 2 * (16384 / accel_sens_trim) */
            inv_obj.accel->sens = inv_obj.accel->sens /
                mldl_cfg->mpu_chip_info->accel_sens_trim * 8192;
        } else
            inv_obj.accel->sens /= 2;
#else
        inv_obj.accel->sens /= 2;
#endif
    }
    if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]) {
        for (ii = 0; ii < 9; ii++) {
            magCal[ii] =
                mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->orientation[ii];
        }
        RANGE_FIXEDPOINT_TO_FLOAT(
            mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->range, magScale);
        inv_obj.mag->sens = (long)(magScale * 32768);

    }

    switch (mldl_cfg->mpu_gyro_cfg->full_scale) {
    case MPU_FS_250DPS:
        gyroScale = 250.f;
        break;
    case MPU_FS_500DPS:
        gyroScale = 500.f;
        break;
    case MPU_FS_1000DPS:
        gyroScale = 1000.f;
        break;
    case MPU_FS_2000DPS:
        gyroScale = 2000.f;
        break;
    default:
        MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n",
                 mldl_cfg->mpu_gyro_cfg->full_scale);
        return INV_ERROR_INVALID_PARAMETER;
        break;
    }

    if (inv_get_state() == INV_STATE_DMP_OPENED) {
        result = inv_set_gyro_calibration(gyroScale, gyroCal);
        if (INV_SUCCESS != result) {
            MPL_LOGE("Unable to set Gyro Calibration\n");
            return result;
        }
        result = inv_set_accel_calibration(accelScale, accelCal);
        if (INV_SUCCESS != result) {
            MPL_LOGE("Unable to set Accel Calibration\n");
            return result;
        }
        result = inv_set_compass_calibration(magScale, magCal);
        if (INV_SUCCESS != result) {
            MPL_LOGE("Unable to set Mag Calibration\n");
            return result;
        }
    }
    return INV_SUCCESS;
}

/**
 *  @brief  Setup the DMP to handle the accelerometer endianess.
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t inv_apply_endian_accel(void)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[4] = { 0 };
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
    int endian;

    if (!mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]) {
        LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
        return INV_ERROR_INVALID_CONFIGURATION;
    }
        
    endian = mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->endian;

    if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->bus !=
        EXT_SLAVE_BUS_SECONDARY) {
        endian = EXT_SLAVE_BIG_ENDIAN;
    }
    switch (endian) {
    case EXT_SLAVE_FS8_BIG_ENDIAN:
    case EXT_SLAVE_FS16_BIG_ENDIAN:
    case EXT_SLAVE_LITTLE_ENDIAN:
        regs[0] = 0;
        regs[1] = 64;
        regs[2] = 0;
        regs[3] = 0;
        break;
    case EXT_SLAVE_BIG_ENDIAN:
    default:
        regs[0] = 0;
        regs[1] = 0;
        regs[2] = 64;
        regs[3] = 0;
    }

    return inv_set_mpu_memory(KEY_D_1_236, 4, regs);
}

/**
 * @internal
 * @brief   This registers a function to be called for each time the DMP
 *          generates an an interrupt.
 *          It will be called after inv_register_fifo_rate_process() which gets called
 *          every time the FIFO data is processed.
 *          The FIFO does not have to be on for this callback.
 * @param func Function to be called when a DMP interrupt occurs.
 * @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func)
{
    INVENSENSE_FUNC_START;
    // Make sure we have not filled up our number of allowable callbacks
    if (mlxCallbackInterrupt.numInterruptProcesses <=
        MAX_INTERRUPT_PROCESSES - 1) {
        int kk;
        // Make sure we haven't registered this function already
        for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
            if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
                return INV_ERROR_INVALID_PARAMETER;
            }
        }
        // Add new callback
        mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt.
                                                numInterruptProcesses] = func;
        mlxCallbackInterrupt.numInterruptProcesses++;
        return INV_SUCCESS;
    }
    return INV_ERROR_MEMORY_EXAUSTED;
}

/**
 * @internal
 * @brief This unregisters a function to be called for each DMP interrupt.
 * @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func)
{
    INVENSENSE_FUNC_START;
    int kk, jj;
    // Make sure we haven't registered this function already
    for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
        if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
            // FIXME, we may need a thread block here
            for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses;
                 ++jj) {
                mlxCallbackInterrupt.processInterruptCb[jj - 1] =
                    mlxCallbackInterrupt.processInterruptCb[jj];
            }
            mlxCallbackInterrupt.numInterruptProcesses--;
            return INV_SUCCESS;
        }
    }
    return INV_ERROR_INVALID_PARAMETER;
}

/**
 *  @internal
 *  @brief  Run the recorded interrupt process callbacks in the event
 *          of an interrupt.
 */
void inv_run_dmp_interupt_cb(void)
{
    int kk;
    for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
        if (mlxCallbackInterrupt.processInterruptCb[kk])
            mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj);
    }
}

/** @internal
* Resets the Motion/No Motion state which should be called at startup and resume.
*/
inv_error_t inv_reset_motion(void)
{
    unsigned char regs[8];
    inv_error_t result;

    inv_obj.lite_fusion->motion_state = INV_MOTION;
    inv_obj.sys->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
    inv_obj.lite_fusion->no_motion_accel_time = inv_get_tick_count();
#if defined CONFIG_MPU_SENSORS_MPU3050
    regs[0] = DINAD8 + 2;
    regs[1] = DINA0C;
    regs[2] = DINAD8 + 1;
    result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
#else
#endif
    regs[0] = (unsigned char)((inv_obj.lite_fusion->motion_duration >> 8) & 0xff);
    regs[1] = (unsigned char)(inv_obj.lite_fusion->motion_duration & 0xff);
    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    memset(regs, 0, 8);
    result = inv_set_mpu_memory(KEY_D_1_96, 8, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    result =
        inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs));
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

/**
 * @internal
 * @brief   inv_start_bias_calc starts the bias calculation on the MPU.
 */
void inv_start_bias_calc(void)
{
    INVENSENSE_FUNC_START;

    inv_obj.adv_fusion->biascalc_suspend = 1;
}

/**
 * @internal
 * @brief   inv_stop_bias_calc stops the bias calculation on the MPU.
 */
void inv_stop_bias_calc(void)
{
    INVENSENSE_FUNC_START;

    inv_obj.adv_fusion->biascalc_suspend = 0;
}

/**
 *  @brief  inv_update_data fetches data from the fifo and updates the
 *          motion algorithms.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          and inv_dmp_start() must have been called.
 *
 *  @note   Motion algorithm data is constant between calls to inv_update_data
 *
 * @return
 * - INV_SUCCESS
 * - Non-zero error code
 */
inv_error_t inv_update_data(void)
{
    INVENSENSE_FUNC_START;
    inv_error_t result = INV_SUCCESS;
    int_fast8_t got, ftry;
    uint_fast8_t mpu_interrupt;
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();

    if (inv_get_state() != INV_STATE_DMP_STARTED)
        return INV_ERROR_SM_IMPROPER_STATE;

    // Set the maximum number of FIFO packets we want to process
    if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) {
        ftry = 100;             // Large enough to process all packets
    } else {
        ftry = 1;
    }

    // Go and process at most ftry number of packets, probably less than ftry
    result = inv_read_and_process_fifo(ftry, &got);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    // Process all interrupts
    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1);
    if (mpu_interrupt) {
        inv_clear_interrupt_trigger(INTSRC_AUX1);
    }
    // Check if interrupt was from MPU
    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU);
    if (mpu_interrupt) {
        inv_clear_interrupt_trigger(INTSRC_MPU);
    }
    // Take care of the callbacks that want to be notified when there was an MPU interrupt
    if (mpu_interrupt) {
        inv_run_dmp_interupt_cb();
    }

    result = inv_get_fifo_status();
    return result;
}

/**
 *  @brief  inv_check_flag returns the value of a flag.
 *          inv_check_flag can be used to check a number of flags,
 *          allowing users to poll flags rather than register callback
 *          functions. If a flag is set to True when inv_check_flag is called,
 *          the flag is automatically reset.
 *          The flags are:
 *          - INV_RAW_DATA_READY
 *          Indicates that new raw data is available.
 *          - INV_PROCESSED_DATA_READY
 *          Indicates that new processed data is available.
 *          - INV_GOT_GESTURE
 *          Indicates that a gesture has been detected by the gesture engine.
 *          - INV_MOTION_STATE_CHANGE
 *          Indicates that a change has been made from motion to no motion,
 *          or vice versa.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          and inv_dmp_start() must have been called.
 *
 *  @param flag The flag to check.
 *
 * @return true or false state of the flag
 */
int inv_check_flag(int flag)
{
    INVENSENSE_FUNC_START;
    int flagReturn = inv_obj.sys->flags[flag];

    inv_obj.sys->flags[flag] = 0;
    return flagReturn;
}

/**
 *  @brief  Enable generation of the DMP interrupt when Motion or no-motion
 *          is detected
 *  @param on
 *          Boolean to turn the interrupt on or off.
 *  @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_set_motion_interrupt(unsigned char on)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;
    unsigned char regs[2] = { 0 };

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (on) {
        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        inv_obj.sys->interrupt_sources |= INV_INT_MOTION;
    } else {
        inv_obj.sys->interrupt_sources &= ~INV_INT_MOTION;
        if (!inv_obj.sys->interrupt_sources) {
            result = inv_get_dl_cfg_int(0);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
    }

    if (on) {
        regs[0] = DINAFE;
    } else {
        regs[0] = DINAD8;
    }
    result = inv_set_mpu_memory(KEY_CFG_7, 1, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    return result;
}

/**
 * Enable generation of the DMP interrupt when a FIFO packet is ready
 *
 * @param on Boolean to turn the interrupt on or off
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_set_fifo_interrupt(unsigned char on)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;
    unsigned char regs[2] = { 0 };

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (on) {
        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        inv_obj.sys->interrupt_sources |= INV_INT_FIFO;
    } else {
        inv_obj.sys->interrupt_sources &= ~INV_INT_FIFO;
        if (!inv_obj.sys->interrupt_sources) {
            result = inv_get_dl_cfg_int(0);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
    }

    if (on) {
        regs[0] = DINAFE;
    } else {
        regs[0] = DINAD8;
    }
    result = inv_set_mpu_memory(KEY_CFG_6, 1, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    return result;
}

/**
 * @brief   Get the current set of DMP interrupt sources.
 *          These interrupts are generated by the DMP and can be
 *          routed to the MPU interrupt line via internal
 *          settings.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *
 * @return  Currently enabled interrupt sources.  The possible interrupts are:
 *          - INV_INT_FIFO,
 *          - INV_INT_MOTION,
 *          - INV_INT_TAP
 */
int inv_get_interrupts(void)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return 0;               // error

    return inv_obj.sys->interrupt_sources;
}

void inv_mpu6050_accel(unsigned short orient, int mode, unsigned char *regs)
{
    unsigned char tmp[3];
    unsigned char tmp1;
    unsigned char tmp2;

    regs[0] = DINA90 + 7;
    regs[1] = DINA90 + 7;
    regs[2] = DINA90 + 7;
    regs[3] = DINA90 + 7;
    regs[4] = DINA90 + 7;
    regs[5] = DINA90 + 7;

    switch (mode) {
        case 0:
            tmp[0] = DINA0C;
            tmp[1] = DINAC9;
            tmp[2] = DINA2C;
            regs[0] = tmp[orient & 3];
            regs[1] = tmp[(orient >> 3) & 3];
            regs[2] = tmp[(orient >> 6) & 3];
            break;
        case 1:
            tmp[0] = DINA4C;
            tmp[1] = DINACD;
            tmp[2] = DINA6C;
            tmp1 = DINA80 + 1;
            regs[0] = tmp1;
            regs[1] = tmp[orient & 3];
            regs[2] = tmp[(orient >> 3) & 3];
            regs[3] = tmp[(orient >> 6) & 3];
            break;
        case 2:
            tmp[0] = DINACF;
            tmp[1] = DINA0C;
            tmp[2] = DINAC9;
            tmp1 = DINA80;
            tmp2 = DINA80 + 1;
            if (orient & 3) { //X is Z or Y
                regs[0] = tmp2;
                regs[1] = tmp[orient & 3];
                if ((orient >> 3) & 3) { //ZYX or YZX
                    regs[2] = tmp[(orient >> 3) & 3];
                    regs[3] = tmp1;
                    regs[4] = tmp[(orient >> 6) & 3];
                } else { //ZXY or YXZ
                    regs[2] = tmp1;
                    regs[3] = tmp[(orient >> 3) & 3];
                    regs[4] = tmp2;
                    regs[5] = tmp[(orient >> 6) & 3];
                }
            } else { //XYZ or XZY
                regs[0] = tmp1;
                regs[1] = tmp[orient & 3];
                regs[2] = tmp2;
                regs[3] = tmp[(orient >> 3) & 3];
                regs[4] = tmp[(orient >> 6) & 3];
            }
            break;
        default:
            break;
    }
}

/**
 *  @brief  Sets up the Accelerometer calibration and scale factor.
 *
 *          Please refer to the provided "9-Axis Sensor Fusion Application
 *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
 *          offers a good coverage on the mounting matrices and explains how
 *          to use them.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *  @pre    inv_dmp_start() must <b>NOT</b> have been called.
 *
 *  @see    inv_set_gyro_calibration().
 *  @see    inv_set_compass_calibration().
 *
 *  @param[in]  range
 *                  The range of the accelerometers in g's. An accelerometer
 *                  that has a range of +2g's to -2g's should pass in 2.
 *  @param[in]  orientation
 *                  A 9 element matrix that represents how the accelerometers
 *                  are oriented with respect to the device they are mounted
 *                  in and the reference axis system.
 *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
 *                  This example corresponds to a 3 x 3 identity matrix.
 *
 *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
 */
inv_error_t inv_set_accel_calibration(float range, signed char *orientation)
{
    INVENSENSE_FUNC_START;
    float cal[9];
    float scale = range / 32768.f;
    int kk;
    unsigned long sf;
    inv_error_t result;
    unsigned char regs[6] = { 0, 0, 0, 0, 0, 0};
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
    unsigned char tmp[3];
    unsigned short orient;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (!mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]) {
        LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
        return INV_ERROR_INVALID_CONFIGURATION;
    }

    /* Apply zero g-offset values */
    if (ACCEL_ID_KXSD9 == mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) {
        regs[0] = 0x80;
        regs[1] = 0x0;
        regs[2] = 0x80;
        regs[3] = 0x0;
    }

    if (inv_dmpkey_supported(KEY_D_1_152)) {
        result = inv_set_mpu_memory(KEY_D_1_152, 4, &regs[0]);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    if (scale == 0) {
        inv_obj.accel->sens = 0;
    }

    if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) {

        tmp[0] = DINA4C;
        tmp[1] = DINACD;
        tmp[2] = DINA6C;

        for (kk = 0; kk < 9; ++kk) {
            cal[kk] = scale * orientation[kk];
            inv_obj.calmat->accel[kk] = (long)(cal[kk] * (float)(1L << 30));
        }

        orient = inv_orientation_matrix_to_scalar(orientation);


#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
        if (ACCEL_ID_MPU6050 == mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) {
            inv_mpu6050_accel(orient, 0, regs);
            result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
        } else {
            if (inv_compass_present()) {
                inv_mpu6050_accel(orient, 1, regs);
                result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
            } else {
                inv_mpu6050_accel(orient, 2, regs);
                result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
            }
        }
#else
        regs[0] = tmp[orient & 3];
        regs[1] = tmp[(orient >> 3) & 3];
        regs[2] = tmp[(orient >> 6) & 3];
        result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs);
#endif


        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        regs[0] = DINA26;
        regs[1] = DINA46;
#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
        regs[2] = DINA66;
#else
        if (MPL_PROD_KEY(mldl_cfg->mpu_chip_info->product_id,
                         mldl_cfg->mpu_chip_info->product_revision)
            == MPU_PRODUCT_KEY_B1_E1_5)
            regs[2] = DINA76;
        else
            regs[2] = DINA66;
#endif
        if (orient & 4)
            regs[0] |= 1;
        if (orient & 0x20)
            regs[1] |= 1;
        if (orient & 0x100)
            regs[2] |= 1;

        result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id ==
            ACCEL_ID_MMA845X) {
            result = inv_freescale_sensor_fusion_16bit(orient);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        } else if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id ==
                   ACCEL_ID_MMA8450) {
            result = inv_freescale_sensor_fusion_8bit(orient);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
    }

    if (inv_obj.accel->sens != 0) {
        sf = (1073741824L / inv_obj.accel->sens);
    } else {
        sf = 0;
    }
    regs[0] = (unsigned char)((sf >> 8) & 0xff);
    regs[1] = (unsigned char)(sf & 0xff);
    result = inv_set_mpu_memory(KEY_D_0_108, 2, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

/**
 *  @brief  Sets up the Gyro calibration and scale factor.
 *
 *          Please refer to the provided "9-Axis Sensor Fusion Application
 *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
 *          offers a good coverage on the mounting matrices and explains
 *          how to use them.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
 *
 *  @see    inv_set_accel_calibration().
 *  @see    inv_set_compass_calibration().
 *
 *  @param[in]  range
 *                  The range of the gyros in degrees per second. A gyro
 *                  that has a range of +2000 dps to -2000 dps should pass in
 *                  2000.
 *  @param[in] orientation
 *                  A 9 element matrix that represents how the gyro are oriented
 *                  with respect to the device they are mounted in. A typical
 *                  set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This
 *                  example corresponds to a 3 x 3 identity matrix.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_gyro_calibration(float range, signed char *orientation)
{
    INVENSENSE_FUNC_START;

    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
    int kk;
    float scale;
    inv_error_t result;

    unsigned char regs[12] = { 0 };
    unsigned char maxVal = 0;
    unsigned char tmpPtr = 0;
    unsigned char tmpSign = 0;
    unsigned char i = 0;
    int sf = 0;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (mldl_cfg->mpu_chip_info->gyro_sens_trim != 0) {
        /* adjust the declared range to consider the gyro_sens_trim
           of this part when different from the default (first dividend) */
        range *= (32768.f / 250) / mldl_cfg->mpu_chip_info->gyro_sens_trim;
    }

    scale = range / 32768.f;    // inverse of sensitivity for the given full scale range
    inv_obj.gyro->sens = (long)(range * 32768L);

    for (kk = 0; kk < 9; ++kk) {
        inv_obj.calmat->gyro[kk] = (long)(scale * orientation[kk] * (1L << 30));
        inv_obj.calmat->gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30));
    }

    {
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
    defined CONFIG_MPU_SENSORS_MPU6050B1
        unsigned char tmpD = DINA4C;
        unsigned char tmpE = DINACD;
        unsigned char tmpF = DINA6C;
#else
        unsigned char tmpD = DINAC9;
        unsigned char tmpE = DINA2C;
        unsigned char tmpF = DINACB;
#endif
        regs[3] = DINA36;
        regs[4] = DINA56;
        regs[5] = DINA76;

        for (i = 0; i < 3; i++) {
            maxVal = 0;
            tmpSign = 0;
            if (inv_obj.calmat->gyro_orient[0 + 3 * i] < 0)
                tmpSign = 1;
            if (ABS(inv_obj.calmat->gyro_orient[1 + 3 * i]) >
                ABS(inv_obj.calmat->gyro_orient[0 + 3 * i])) {
                maxVal = 1;
                if (inv_obj.calmat->gyro_orient[1 + 3 * i] < 0)
                    tmpSign = 1;
            }
            if (ABS(inv_obj.calmat->gyro_orient[2 + 3 * i]) >
                ABS(inv_obj.calmat->gyro_orient[1 + 3 * i])) {
                tmpSign = 0;
                maxVal = 2;
                if (inv_obj.calmat->gyro_orient[2 + 3 * i] < 0)
                    tmpSign = 1;
            }
            if (maxVal == 0) {
                regs[tmpPtr++] = tmpD;
                if (tmpSign)
                    regs[tmpPtr + 2] |= 0x01;
            } else if (maxVal == 1) {
                regs[tmpPtr++] = tmpE;
                if (tmpSign)
                    regs[tmpPtr + 2] |= 0x01;
            } else {
                regs[tmpPtr++] = tmpF;
                if (tmpSign)
                    regs[tmpPtr + 2] |= 0x01;
            }
        }
        result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_set_mpu_memory(KEY_FCFG_3, 3, &regs[3]);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384
        inv_obj.gyro->sf =
            (long)(((long long)inv_obj.gyro->sens * 767603923LL) / 1073741824LL);
        result =
            inv_set_mpu_memory(KEY_D_0_104, 4,
                               inv_int32_to_big8(inv_obj.gyro->sf, regs));
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        if (inv_obj.gyro->sens != 0) {
            sf = (long)((long long)23832619764371LL / inv_obj.gyro->sens);
        } else {
            sf = 0;
        }

        result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs));
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }
    return INV_SUCCESS;
}

/**
 *  @brief  Sets up the Compass calibration and scale factor.
 *
 *          Please refer to the provided "9-Axis Sensor Fusion Application
 *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
 *          offers a good coverage on the mounting matrices and explains
 *          how to use them.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
 *
 *  @see    inv_set_gyro_calibration().
 *  @see    inv_set_accel_calibration().
 *
 *  @param[in] range
 *                  The range of the compass.
 *  @param[in] orientation
 *                  A 9 element matrix that represents how the compass is
 *                  oriented with respect to the device they are mounted in.
 *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
 *                  This example corresponds to a 3 x 3 identity matrix.
 *                  The matrix describes how to go from the chip mounting to
 *                  the body of the device.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_compass_calibration(float range, signed char *orientation)
{
    INVENSENSE_FUNC_START;
    float cal[9];
    float scale = range / 32768.f;
    int kk;
    unsigned short compassId = 0;

    compassId = inv_get_compass_id();

    if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883)
        || (compassId == COMPASS_ID_LSM303DLH)  || (compassId == COMPASS_ID_LSM303DLM)) {
        scale /= 32.0f;
    }

    for (kk = 0; kk < 9; ++kk) {
        cal[kk] = scale * orientation[kk];
        inv_obj.calmat->compass[kk] = (long)(cal[kk] * (float)(1L << 30));
    }

    inv_obj.mag->sens = (long)(scale * 1073741824L);

    if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) {
        unsigned char reg0[4] = { 0, 0, 0, 0 };
        unsigned char regp[4] = { 64, 0, 0, 0 };
        unsigned char regn[4] = { 64 + 128, 0, 0, 0 };
        unsigned char *reg;
        int_fast8_t kk;
        unsigned short keyList[9] =
            { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02,
            KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12,
            KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22
        };

        for (kk = 0; kk < 9; ++kk) {

            if (orientation[kk] == 1)
                reg = regp;
            else if (orientation[kk] == -1)
                reg = regn;
            else
                reg = reg0;
            inv_set_mpu_memory(keyList[kk], 4, reg);
        }
    }

    return INV_SUCCESS;
}

/**
* @internal
* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup.
*/
inv_error_t inv_set_dead_zone(void)
{
    unsigned char reg;
    inv_error_t result;
    extern struct control_params cntrl_params;

    if (cntrl_params.functions & INV_DEAD_ZONE) {
        reg = 0x08;
    } else {
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
    defined CONFIG_MPU_SENSORS_MPU6050B1
        reg = 0;
#else
        if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
            reg = 0x2;
        } else {
            reg = 0;
        }
#endif
    }

    result = inv_set_mpu_memory(KEY_D_0_163, 1, &reg);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    return result;
}

/**
 *  @brief  inv_set_bias_update is used to register which algorithms will be
 *          used to automatically reset the gyroscope bias.
 *          The engine INV_BIAS_UPDATE must be enabled for these algorithms to
 *          run.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          and inv_dmp_start()
 *          must <b>NOT</b> have been called.
 *
 *  @param  function    A function or bitwise OR of functions that determine
 *                      how the gyroscope bias will be automatically updated.
 *                      Functions include:
 *                      - INV_NONE or 0,
 *                      - INV_BIAS_FROM_NO_MOTION,
 *                      - INV_BIAS_FROM_GRAVITY,
 *                      - INV_BIAS_FROM_TEMPERATURE,
                    \ifnot UMPL
 *                      - INV_BIAS_FROM_LPF,
 *                      - INV_MAG_BIAS_FROM_MOTION,
 *                      - INV_MAG_BIAS_FROM_GYRO,
 *                      - INV_ALL.
 *                   \endif
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_bias_update(unsigned short function)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[4];
    long tmp[3] = { 0, 0, 0 };
    inv_error_t result = INV_SUCCESS;
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    /* do not allow progressive no motion bias tracker to run -
       it's not fully debugged */
    function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround
    MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n");

    // You must use EnableFastNoMotion() to get this feature
    function &= ~INV_BIAS_FROM_FAST_NO_MOTION;

    // You must use DisableFastNoMotion() to turn this feature off
    if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION)
        function |= INV_BIAS_FROM_FAST_NO_MOTION;

    /*--- remove magnetic components from bias tracking
          if there is no compass ---*/
    if (!inv_compass_present()) {
        function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION);
    } else {
        function &= ~(INV_BIAS_FROM_LPF);
    }

    // Enable function sets bias from no motion
    inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION);

    if (function & INV_BIAS_FROM_NO_MOTION) {
        inv_enable_bias_no_motion();
    } else {
        inv_disable_bias_no_motion();
    }

    if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
        regs[0] = DINA80 + 2;
        regs[1] = DINA2D;
        regs[2] = DINA55;
        regs[3] = DINA7D;
    } else {
        regs[0] = DINA80 + 7;
        regs[1] = DINA2D;
        regs[2] = DINA35;
        regs[3] = DINA3D;
    }
    result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    result = inv_set_dead_zone();
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) &&
        !inv_compass_present()) {
        result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    } else {
        result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

#ifdef INV_FEATURE_GYROTC_LEGACY
    inv_obj.gyro_tc->factory_temp_comp = 0;  // FIXME, workaround
    if ((mldl_cfg->mpu_offsets->tc[0] != 0) ||
        (mldl_cfg->mpu_offsets->tc[1] != 0) ||
        (mldl_cfg->mpu_offsets->tc[2] != 0)) {
        inv_obj.gyro_tc->factory_temp_comp = 1;
    }

    if (inv_obj.gyro_tc->factory_temp_comp == 0) {
        if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) {
            result = inv_set_gyro_temp_slope(inv_obj.gyro_tc->temp_slope);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        } else {
            result = inv_set_gyro_temp_slope(tmp);
            if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
    } else {
        inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE;
        MPL_LOGV("factory temperature compensation coefficients available - "
                 "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n");
    }
#endif /* INV_FEATURE_GYROTC_LEGACY */

    /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on
           compass and accel data, is to have accelerometer data delivered in the
           FIFO ----*/
    if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY)
         && inv_compass_present())
        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO)
        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) {
        inv_send_accel(INV_ALL, INV_32_BIT);
        inv_send_gyro(INV_ALL, INV_32_BIT);
    }

    return result;
}

/**
 *  @brief  inv_get_motion_state is used to determine if the device is in
 *          a 'motion' or 'no motion' state.
 *          inv_get_motion_state returns INV_MOTION of the device is moving,
 *          or INV_NO_MOTION if the device is not moving.
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          and inv_dmp_start()
 *          must have been called.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
int inv_get_motion_state(void)
{
    INVENSENSE_FUNC_START;
    return inv_obj.lite_fusion->motion_state;
}

/**
 *  @brief  inv_set_no_motion_thresh is used to set the threshold for
 *          detecting INV_NO_MOTION
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *
 *  @param  thresh  A threshold scaled in degrees per second.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_no_motion_thresh(float thresh)
{
    inv_error_t result = INV_SUCCESS;
    unsigned char regs[4] = { 0 };
    long tmp;
    INVENSENSE_FUNC_START;

    tmp = (long)(thresh * thresh * 2.045f);
    if (tmp < 0) {
        return INV_ERROR;
    } else if (tmp > 8180000L) {
        return INV_ERROR;
    }
    
    regs[0] = (unsigned char)(tmp >> 24);
    regs[1] = (unsigned char)((tmp >> 16) & 0xff);
    regs[2] = (unsigned char)((tmp >> 8) & 0xff);
    regs[3] = (unsigned char)(tmp & 0xff);

    result = inv_set_mpu_memory(KEY_D_1_108, 4, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    result = inv_reset_motion();
    return result;
}

/**
 *  @brief  inv_set_no_motion_threshAccel is used to set the threshold for
 *          detecting INV_NO_MOTION with accelerometers when Gyros have
 *          been turned off
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *
 *  @param  thresh  A threshold in g's scaled by 2^32
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_no_motion_threshAccel(long thresh)
{
    INVENSENSE_FUNC_START;

    inv_obj.lite_fusion->no_motion_accel_threshold = thresh;

    return INV_SUCCESS;
}

/**
 *  @brief  inv_set_no_motion_time is used to set the time required for
 *          detecting INV_NO_MOTION
 *
 *  @pre    inv_dmp_open()
 *          @ifnot MPL_MF
 *              or inv_open_low_power_pedometer()
 *              or inv_eis_open_dmp()
 *          @endif
 *          must have been called.
 *
 *  @param  time    A time in seconds.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_set_no_motion_time(float time)
{
    inv_error_t result = INV_SUCCESS;
    unsigned char regs[2] = { 0 };
    long tmp;

    INVENSENSE_FUNC_START;

    tmp = (long)(time * 200);
    if (tmp < 0) {
        return INV_ERROR;
    } else if (tmp > 65535L) {
        return INV_ERROR;
    }
    inv_obj.lite_fusion->motion_duration = (unsigned short)tmp;

    regs[0] = (unsigned char)((inv_obj.lite_fusion->motion_duration >> 8) & 0xff);
    regs[1] = (unsigned char)(inv_obj.lite_fusion->motion_duration & 0xff);
    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    result = inv_reset_motion();
    return result;
}

/**
 *  @brief  inv_get_version is used to get the ML version.
 *
 *  @pre    inv_get_version can be called at any time.
 *
 *  @param  version     inv_get_version writes the ML version
 *                      string pointer to version.
 *
 *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
 */
inv_error_t inv_get_version(unsigned char **version)
{
    INVENSENSE_FUNC_START;

    *version = (unsigned char *)mlVer;  //fixme we are wiping const

    return INV_SUCCESS;
}

/**
 * @brief Check for the presence of the gyro sensor.
 *
 * This is not a physical check but a logical check and the value can change
 * dynamically based on calls to inv_set_mpu_sensors().
 *
 * @return  true if the gyro is enabled false otherwise.
 */
int inv_get_gyro_present(void)
{
    return inv_get_dl_config()->inv_mpu_cfg->requested_sensors &
        (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO);
}

static unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;                  // error
    return b;
}

unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{
    unsigned short scalar;
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;

    return scalar;
}

/* Setups up the Freescale 16-bit accel for Sensor Fusion
* @param[in] orient A scalar representation of the orientation
*  that describes how to go from the Chip Orientation
*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
*/
inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient)
{
    unsigned char rr[3];
    inv_error_t result;

    orient = orient & 0xdb;
    switch (orient) {
    default:
        // Typically 0x88
        rr[0] = DINACC;
        rr[1] = DINACF;
        rr[2] = DINA0E;
        break;
    case 0x50:
        rr[0] = DINACE;
        rr[1] = DINA0E;
        rr[2] = DINACD;
        break;
    case 0x81:
        rr[0] = DINACE;
        rr[1] = DINACB;
        rr[2] = DINA0E;
        break;
    case 0x11:
        rr[0] = DINACC;
        rr[1] = DINA0E;
        rr[2] = DINACB;
        break;
    case 0x42:
        rr[0] = DINA0A;
        rr[1] = DINACF;
        rr[2] = DINACB;
        break;
    case 0x0a:
        rr[0] = DINA0A;
        rr[1] = DINACB;
        rr[2] = DINACD;
        break;
    }
    result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr);
    return result;
}

/* Setups up the Freescale 8-bit accel for Sensor Fusion
* @param[in] orient A scalar representation of the orientation
*  that describes how to go from the Chip Orientation
*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
*/
inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient)
{
    unsigned char regs[27];
    inv_error_t result;
    uint_fast8_t kk;

    orient = orient & 0xdb;
    kk = 0;

    regs[kk++] = DINAC3;
    regs[kk++] = DINA90 + 14;
    regs[kk++] = DINAA0 + 9;
    regs[kk++] = DINA3E;
    regs[kk++] = DINA5E;
    regs[kk++] = DINA7E;

    regs[kk++] = DINAC2;
    regs[kk++] = DINAA0 + 9;
    regs[kk++] = DINA90 + 9;
    regs[kk++] = DINAF8 + 2;

    switch (orient) {
    default:
        // Typically 0x88
        regs[kk++] = DINACB;

        regs[kk++] = DINA54;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;

        regs[kk++] = DINACD;
        break;
    case 0x50:
        regs[kk++] = DINACB;

        regs[kk++] = DINACF;

        regs[kk++] = DINA7C;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        break;
    case 0x81:
        regs[kk++] = DINA2C;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;

        regs[kk++] = DINACD;

        regs[kk++] = DINACB;
        break;
    case 0x11:
        regs[kk++] = DINA2C;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINA28;
        regs[kk++] = DINACB;
        regs[kk++] = DINACF;
        break;
    case 0x42:
        regs[kk++] = DINACF;
        regs[kk++] = DINACD;

        regs[kk++] = DINA7C;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        regs[kk++] = DINA78;
        break;
    case 0x0a:
        regs[kk++] = DINACD;

        regs[kk++] = DINA54;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;
        regs[kk++] = DINA50;

        regs[kk++] = DINACF;
        break;
    }

    regs[kk++] = DINA90 + 7;
    regs[kk++] = DINAF8 + 3;
    regs[kk++] = DINAA0 + 9;
    regs[kk++] = DINA0E;
    regs[kk++] = DINA0E;
    regs[kk++] = DINA0E;

    regs[kk++] = DINAF8 + 1;    // filler

    result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

/**
 * Controlls each sensor and each axis when the motion processing unit is
 * running.  When it is not running, simply records the state for later.
 *
 * NOTE: In this version only full sensors controll is allowed.  Independent
 * axis control will return an error.
 *
 * @param sensors Bit field of each axis desired to be turned on or off
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_set_mpu_sensors(unsigned long sensors)
{
    INVENSENSE_FUNC_START;
    unsigned char state = inv_get_state();
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
    inv_error_t result = INV_SUCCESS;
    unsigned short fifoRate;

    if (state < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) &&
        ((sensors & INV_THREE_AXIS_ACCEL) != 0)) {
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    }
    if (((sensors & INV_THREE_AXIS_ACCEL) != 0) &&
        (!mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL])) {
        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
    }

    if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) &&
        ((sensors & INV_THREE_AXIS_COMPASS) != 0)) {
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    }
    if (((sensors & INV_THREE_AXIS_COMPASS) != 0) &&
        (!mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS])) {
        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
    }

    if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) &&
        ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) {
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    }
    if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) &&
        (!mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE])) {
        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
    }

    /* DMP was off, and is turning on */
    if (sensors & INV_DMP_PROCESSOR &&
        !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
        struct ext_slave_config config;
        long odr;
        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
        config.len = sizeof(long);
        config.apply = !(mldl_cfg->inv_mpu_state->status &
                         MPU_ACCEL_IS_SUSPENDED);
        config.data = &odr;

        odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg->mpu_gyro_cfg) * 1000);
        result = inv_mpu_config_accel(mldl_cfg,
                                      inv_get_serial_handle(),
                                      inv_get_serial_handle(), &config);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
        odr = MPU_SLAVE_IRQ_TYPE_NONE;
        result = inv_mpu_config_accel(mldl_cfg,
                                      inv_get_serial_handle(),
                                      inv_get_serial_handle(), &config);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        inv_init_fifo_hardare();
    }

#ifdef INV_FEATURE_ADVFUSION
    if (inv_obj.adv_fusion->mode_change_cb) {
        result = inv_obj.adv_fusion->mode_change_cb(
            mldl_cfg->inv_mpu_cfg->requested_sensors, sensors);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }
#endif

#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
    {
        unsigned char regs[6];
        unsigned short orient;

        orient = inv_orientation_matrix_to_scalar(
            mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->orientation);

        if (sensors & INV_THREE_AXIS_ACCEL) {
            if (ACCEL_ID_MPU6050 == mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) {
                inv_mpu6050_accel(orient, 0, regs);
                result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
            } else {
                if (sensors & INV_THREE_AXIS_COMPASS) {
                    inv_mpu6050_accel(orient, 1, regs);
                    result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
                } else {
                    inv_mpu6050_accel(orient, 2, regs);
                    result = inv_set_mpu_memory(KEY_FCFG_2, 6, regs);
                }
            }
        }
    }
#endif

    /* Get the fifo rate before changing sensors so if we need to match it */
    fifoRate = inv_get_fifo_rate();
    mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;

    /* inv_dmp_start will turn the sensors on */
    if (state == INV_STATE_DMP_STARTED) {
        result = inv_dl_start(sensors);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_reset_motion();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_dl_stop(~sensors);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
        struct ext_slave_config config;
        long data;

        inv_set_fifo_rate(fifoRate);

        config.len = sizeof(long);
        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
        config.apply = !(mldl_cfg->inv_mpu_state->status &
                         MPU_ACCEL_IS_SUSPENDED);
        config.data = &data;
        data = MPU_SLAVE_IRQ_TYPE_DATA_READY;
        result = inv_mpu_config_accel(mldl_cfg,
                                      inv_get_serial_handle(),
                                      inv_get_serial_handle(), &config);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    return result;
}

void inv_set_mode_change(inv_error_t(*mode_change_func)
                         (unsigned long, unsigned long))
{
    inv_obj.adv_fusion->mode_change_cb = mode_change_func;
}

/**
* Mantis setup
*/
#ifdef CONFIG_MPU_SENSORS_MPU6050B1
inv_error_t inv_set_mpu_6050_config(void)
{
    long temp;
    inv_error_t result;
    unsigned char big8[4];
    unsigned char atc[4];
    long s[3], s2[3];
    int kk;
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();

    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
                             0x0d, 4, atc);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    temp = atc[3] & 0x3f;
    if (temp >= 32)
        temp = temp - 64;
    temp = (temp << 21) | 0x100000;
    temp += (1L << 29);
    temp = -temp;
    result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    for (kk = 0; kk < 3; ++kk) {
        s[kk] = atc[kk] & 0x3f;
        if (s[kk] > 32)
            s[kk] = s[kk] - 64;
        s[kk] *= 2516582L;
    }

    for (kk = 0; kk < 3; ++kk) {
        s2[kk] = mldl_cfg->pdata->orientation[kk * 3] * s[0] +
            mldl_cfg->pdata->orientation[kk * 3 + 1] * s[1] +
            mldl_cfg->pdata->orientation[kk * 3 + 2] * s[2];
    }
    result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}
#endif

/**
 * @}
 */
