/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
/******************************************************************************
 *
 * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $
 *
 *****************************************************************************/
#undef MPL_LOG_NDEBUG
#ifdef UNITTESTING
#define MPL_LOG_NDEBUG 1
#else
#define MPL_LOG_NDEBUG 0
#endif

/**
 *  @defgroup MLSETUP
 *  @brief  The Motion Library external slaves setup override suite.
 *
 *          Use these APIs to override the kernel/default settings in the
 *          corresponding data structures for gyros, accel, and compass.
 *
 *  @{
 *      @file mlsetup.c
 *      @brief The Motion Library external slaves setup override suite.
 */

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

/*
    Defines
*/
/* these have to appear before inclusion of mpu.h */
#define CONFIG_MPU_SENSORS_KXSD9         y   // Kionix accel
#define CONFIG_MPU_SENSORS_KXTF9         y   // Kionix accel
#define CONFIG_MPU_SENSORS_LIS331DLH     y   // ST accelerometer
#define CONFIG_MPU_SENSORS_LSM303DLX_A   y   // ST accelerometer in LSM303DLx combo
#define CONFIG_MPU_SENSORS_LIS3DH        y   // ST accelerometer
#define CONFIG_MPU_SENSORS_BMA150        y   // Bosch 150 accelerometer
#define CONFIG_MPU_SENSORS_BMA222        y   // Bosch 222 accelerometer
#define CONFIG_MPU_SENSORS_BMA250        y   // Bosch 250 accelerometer
#define CONFIG_MPU_SENSORS_ADXL34X       y   // AD 345 or 346 accelerometer
#define CONFIG_MPU_SENSORS_MMA8450       y   // Freescale MMA8450 accelerometer
#define CONFIG_MPU_SENSORS_MMA845X       y   // Freescale MMA845X accelerometer
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y   // Invensense MPU6050 built-in accelerometer
#endif

#define CONFIG_MPU_SENSORS_AK8975        y   // AKM compass
#define CONFIG_MPU_SENSORS_AMI30X        y   // AICHI AMI304/305 compass
#define CONFIG_MPU_SENSORS_AMI306        y   // AICHI AMI306 compass
#define CONFIG_MPU_SENSORS_HMC5883       y   // Honeywell compass
#define CONFIG_MPU_SENSORS_LSM303DLX_M   y   // ST compass in LSM303DLx combo
#define CONFIG_MPU_SENSORS_YAS529        y   // Yamaha compass
#define CONFIG_MPU_SENSORS_YAS530        y   // Yamaha compass
#define CONFIG_MPU_SENSORS_MMC314X       y   // MEMSIC compass
#define CONFIG_MPU_SENSORS_HSCDTD002B    y   // ALPS compass
#define CONFIG_MPU_SENSORS_HSCDTD004A    y   // ALPS HSCDTD004A compass

#define CONFIG_MPU_SENSORS_BMA085        y   // Bosch 085 pressure

#include "external_hardware.h"

#include <stdio.h>
#include <string.h>

#include "slave.h"
#include "compass.h"
#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "MPL-mlsetup"

#include "linux/mpu.h"

#include "mlsetup.h"

#ifdef LINUX
#include "errno.h"
#endif

/* Override these structures from mldl.c */
extern struct ext_slave_descr          g_slave_accel;
extern struct ext_slave_descr          g_slave_compass;
//extern struct ext_slave_descr          g_slave_pressure;
/* Platform Data */
//extern struct mpu_platform_data        g_pdata;
extern struct ext_slave_platform_data  g_pdata_slave_accel;
extern struct ext_slave_platform_data  g_pdata_slave_compass;
//extern struct ext_slave_platform_data  g_pdata_slave_pressure;
signed char g_gyro_orientation[9];

/*
    Typedefs
*/
typedef void tSetupFuncAccel(void);
typedef void tSetupFuncCompass(void);
typedef void tSetupFuncPressure(void);

#ifdef LINUX
#include <sys/ioctl.h>
#endif

/*********************************************************************
              Dragon - PLATFORM_ID_DRAGON_PROTOTYPE
*********************************************************************/
/**
 * @internal
 * @brief  performs a 180' rotation around Z axis to reflect
 *         usage of the multi sensor board (MSB) with the
 *         beagleboard
 * @note   assumes well formed mounting matrix, with only
 *         one 1 for each row.
 */
static void Rotate180DegAroundZAxis(signed char matrix[])
{
    int ii;
    for(ii=0; ii<6; ii++) {
        matrix[ii] = -matrix[ii];
    }
}

/**
 * @internal
 * Sets the orientation based on the position of the mounting.  For different
 * devices the relative position to pin 1 will be different.
 *
 * Positions are:
 * - 0-3 are Z up
 * - 4-7 are Z down
 * - 8-11 are Z right
 * - 12-15 are Z left
 * - 16-19 are Z front
 * - 20-23 are Z back
 *
 * @param position The position of the orientation
 * @param orientation the location to store the new oreintation
 */
static inv_error_t SetupOrientation(unsigned int position,
                                 signed char *orientation)
{
    memset(orientation, 0, 9);
    switch (position){
    case 0:
        /*-------------------------*/
        orientation[0] = +1;
        orientation[4] =     +1;
        orientation[8] =         +1;
        break;
    case 1:
        /*-------------------------*/
        orientation[1] =     +1;
        orientation[3] = -1;
        orientation[8] =         +1;
        break;
    case 2:
        /*-------------------------*/
        orientation[0] = -1;
        orientation[4] =     -1;
        orientation[8] =         +1;
        break;
    case 3:
        /*-------------------------*/
        orientation[1] =     -1;
        orientation[3] = +1;
        orientation[8] =         +1;
        break;
    case 4:
        /*-------------------------*/
        orientation[0] = -1;
        orientation[4] =     +1;
        orientation[8] =         -1;
        break;
    case 5:
        /*-------------------------*/
        orientation[1] =     -1;
        orientation[3] = -1;
        orientation[8] =         -1;
        break;
    case 6:
        /*-------------------------*/
        orientation[0] = +1;
        orientation[4] =     -1;
        orientation[8] =         -1;
        break;
    case 7:
        /*-------------------------*/
        orientation[1] =     +1;
        orientation[3] = +1;
        orientation[8] =         -1;
        break;
    case 8:
        /*-------------------------*/
        orientation[2] =         +1;
        orientation[3] = +1;
        orientation[7] =     +1;
        break;
    case 9:
        /*-------------------------*/
        orientation[2] = +1;
        orientation[4] = +1;
        orientation[6] = -1;
        break;
    case 10:
        orientation[2] = +1;
        orientation[3] = -1;
        orientation[7] = -1;
        break;
    case 11:
        orientation[2] = +1;
        orientation[4] = -1;
        orientation[6] = +1;
        break;
    case 12:
        orientation[2] = -1;
        orientation[3] = -1;
        orientation[7] = +1;
        break;
    case 13:
        orientation[2] = -1;
        orientation[4] = -1;
        orientation[6] = -1;
        break;
    case 14:
        orientation[2] = -1;
        orientation[3] = +1;
        orientation[7] = -1;
        break;
    case 15:
        orientation[2] = -1;
        orientation[4] = +1;
        orientation[6] = +1;
        break;
    case 16:
        orientation[0] = -1;
        orientation[5] = +1;
        orientation[7] = +1;
        break;
    case 17:
        orientation[1] = -1;
        orientation[5] = +1;
        orientation[6] = -1;
        break;
    case 18:
        orientation[0] = +1;
        orientation[5] = -1;
        orientation[7] = -1;
        break;
    case 19:
        orientation[1] = -1;
        orientation[5] = +1;
        orientation[6] = +1;
        break;
    case 20:
        orientation[0] = +1;
        orientation[5] = -1;
        orientation[7] = +1;
        break;
    case 21:
        orientation[1] = -1;
        orientation[5] = -1;
        orientation[6] = +1;
        break;
    case 22:
        orientation[0] = -1;
        orientation[5] = -1;
        orientation[7] = -1;
        break;
    case 23:
        orientation[1] = +1;
        orientation[5] = -1;
        orientation[6] = -1;
        break;
    default:
        MPL_LOGE("Invalid position %d\n", position);
        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
        return INV_ERROR_INVALID_PARAMETER;
    }

    return INV_SUCCESS;
}

static void PrintMountingOrientation(
    const char * header, signed char *orientation)
{
    MPL_LOGV("%s:\n", header);
    MPL_LOGV("\t[[%3d, %3d, %3d]\n",
             orientation[0], orientation[1], orientation[2]);
    MPL_LOGV("\t [%3d, %3d, %3d]\n",
             orientation[3], orientation[4], orientation[5]);
    MPL_LOGV("\t [%3d, %3d, %3d]]\n",
             orientation[6], orientation[7], orientation[8]);
}

/*****************************
 *   Accel Setup Functions   *
 *****************************/

static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 5;
        break;
    case PLATFORM_ID_ST_6AXIS:
        position = 0;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *lis331_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS331;
    return INV_SUCCESS;
}

static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 1;
        break;
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 3;
        break;
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *lis3dh_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS3DH;
    return result;
}

static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 7;
        break;
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *kxsd9_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXSD9;
    return result;
}

static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB_EVB:
        position =0;
            break;
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 7;
        break;
#ifdef WIN32
    case PLATFORM_ID_DONGLE:
        position = 1;
        break;
#endif
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 1;
        break;
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *kxtf9_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXTF9;
    return result;
}

static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
        position = 3;
        break;
    case PLATFORM_ID_MSB_V2:
        position = 1;
        break;
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *lsm303dlx_a_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LSM303;
    return result;
}

static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 6;
        break;
#ifdef WIN32
    case PLATFORM_ID_DONGLE:
        position = 3;
        break;
#endif
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *bma150_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA150;
    return result;
}

static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 0;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *bma222_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA222;
    return result;
}

static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 0;
        break;
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 3;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:

    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *bma250_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA250;
    return result;
}

static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 6;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *adxl34x_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_ADXL34X;
    return result;
}


static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 5;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *mma8450_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA8450;
    return result;
}


static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 5;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_accel = *mma845x_get_slave_descr();
#endif
    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA845X;
    return result;
}


/**
 * @internal
 * Sets up the orientation matrix according to how the gyro was
 * mounted.
 *
 * @param platforId Platform identification for mounting information
 * @return INV_SUCCESS or non-zero error code
 */
static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_MANTIS_MSB:
        position = 6;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
	case PLATFORM_ID_DRAGON_USB_DONGLE:
        position = 1;
        break;
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    case PLATFORM_ID_MANTIS_EVB:
        position = 0;
        break;
    case PLATFORM_ID_MSB_V3:
        position = 2;
        break;
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        return INV_ERROR_INVALID_PARAMETER;
    };

    SetupOrientation(position, g_pdata_slave_accel.orientation);
    /* Interrupt */
#ifndef LINUX
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
   // g_slave_accel = // fixme *mpu6050_get_slave_descr();
#endif
#endif
    g_pdata_slave_accel.address         = 0x68;
    return result;
}

/*****************************
    Compass Setup Functions
******************************/
static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_MANTIS_MSB:
        position = 2;
        break;
#ifdef WIN32
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
        position = 4;
        break;
#endif
    case PLATFORM_ID_SPIDER_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
        position = 7;
        break;
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V3:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 6;
        break;
    case PLATFORM_ID_DRAGON_USB_DONGLE:
    case PLATFORM_ID_MSB_EVB:
        position = 5;
        break;
    case PLATFORM_ID_MANTIS_EVB:
        position = 4;
        break;
	 case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *ak8975_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AKM;
    return result;
}

static inv_error_t SetupCompassMMCCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 7;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *mmc314x_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_MMC314X;
    return result;
}

static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 4;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI304;
#ifndef LINUX
    g_slave_compass = *ami30x_get_slave_descr();
#endif
    return result;
}

static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 3;
        break;
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 1;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *ami306_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI306;
    return result;
}

static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 6;
        break;
#ifdef WIN32
    case PLATFORM_ID_DONGLE:
        position = 2;
        break;
#endif
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };

    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *hmc5883_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
    return result;
}


static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_10AXIS:
        position = 1;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:

    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
#ifndef LINUX
    g_slave_compass = *lsm303dlx_m_get_slave_descr();
    g_slave_compass.id = COMPASS_ID_LSM303DLH;
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
    return result;
}

static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
        position = 8;
        break;
    case PLATFORM_ID_MSB_V2:
        position = 12;
        break;
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *lsm303dlx_m_get_slave_descr();
    g_slave_compass.id = COMPASS_ID_LSM303DLM;
#endif
    g_pdata_slave_compass.address       = COMPASS_SLAVEADDR_HMC5883;
    return result;
}

static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
        position = 1;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *yas530_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS530;
    return result;
}

static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 6;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *yas529_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS529;
    return result;
}


static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 2;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *hscdtd002b_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
    return result;
}

static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId)
{
    inv_error_t result = INV_SUCCESS;
    unsigned int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
        position = 1;
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_MANTIS_MSB:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    };
    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

#ifndef LINUX
    g_slave_compass = *hscdtd004a_get_slave_descr();
#endif
    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
    return result;
}


/*****************************
    Pressure Setup Functions
******************************/
#if 0
static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId)
{
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation));

    g_pdata_slave_pressure.bus             = EXT_SLAVE_BUS_PRIMARY;
#ifndef LINUX
    g_slave_pressure = *bma085_get_slave_descr();
#endif
    g_pdata_slave_pressure.address         = PRESSURE_SLAVEADDR_BMA085;
    return INV_SUCCESS;
}
#endif
/**
 * @internal
 * Sets up the orientation matrix according to how the part was
 * mounted.
 *
 * @param platforId Platform identification for mounting information
 * @return INV_SUCCESS or non-zero error code
 */
static inv_error_t SetupAccelCalibration(unsigned short platformId,
                                      unsigned short accelId)
{
    /*----  setup the accels ----*/
    switch(accelId) {
    case ACCEL_ID_LSM303DLX:
        SetupAccelLSM303Calibration(platformId);
        break;
    case ACCEL_ID_LIS331:
        SetupAccelSTLIS331Calibration(platformId);
        break;
    case ACCEL_ID_KXSD9:
        SetupAccelKionixKXSD9Calibration(platformId);
        break;
    case ACCEL_ID_KXTF9:
        SetupAccelKionixKXTF9Calibration(platformId);
        break;
    case ACCEL_ID_BMA150:
        SetupAccelBMA150Calibration(platformId);
        break;
    case ACCEL_ID_BMA222:
        SetupAccelBMA222Calibration(platformId);
        break;
    case ACCEL_ID_BMA250:
        SetupAccelBMA250Calibration(platformId);
        break;
    case ACCEL_ID_ADXL34X:
        SetupAccelADXL34XCalibration(platformId);
        break;
    case ACCEL_ID_MMA8450:
        SetupAccelMMA8450Calibration(platformId);
        break;
    case ACCEL_ID_MMA845X:
        SetupAccelMMA845XCalibration(platformId);
        break;
    case ACCEL_ID_LIS3DH:
        SetupAccelSTLIS3DHCalibration(platformId);
        break;
    case ACCEL_ID_MPU6050:
        SetupAccelMPU6050Calibration(platformId);
        break;
    case ID_INVALID:
        break;
    default:
        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
        return INV_ERROR_INVALID_PARAMETER;
    }

    if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) {
        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_SECONDARY;
    } else if (accelId != ACCEL_ID_MPU6050) {
        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_PRIMARY;
    }

#ifndef WIN32
    if (accelId != ID_INVALID)
        Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation);
#endif

    return INV_SUCCESS;
}

/**
 * @internal
 * Sets up the orientation matrix according to how the part was
 * mounted.
 *
 * @param platforId Platform identification for mounting information
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t SetupCompassCalibration(unsigned short platformId,
                                 unsigned short compassId)
{
    /*----  setup the compass ----*/
    switch(compassId) {
    case COMPASS_ID_AK8975:
        SetupCompassAKM8975Calibration(platformId);
        break;
    case COMPASS_ID_AMI30X:
        SetupCompassAMI304Calibration(platformId);
        break;
    case COMPASS_ID_AMI306:
        SetupCompassAMI306Calibration(platformId);
        break;
    case COMPASS_ID_LSM303DLH:
        SetupCompassLSM303DLHCalibration(platformId);
        break;
    case COMPASS_ID_LSM303DLM:
        SetupCompassLSM303DLMCalibration(platformId);
        break;
    case COMPASS_ID_HMC5883:
        SetupCompassHMC5883Calibration(platformId);
        break;
    case COMPASS_ID_YAS529:
        SetupCompassYAS529Calibration(platformId);
        break;
    case COMPASS_ID_YAS530:
        SetupCompassYAS530Calibration(platformId);
        break;
    case COMPASS_ID_MMC314X:
        SetupCompassMMCCalibration(platformId);
        break;
    case COMPASS_ID_HSCDTD002B:
        SetupCompassHSCDTD002BCalibration(platformId);
        break;
    case COMPASS_ID_HSCDTD004A:
        SetupCompassHSCDTD004ACalibration(platformId);
        break;
    case ID_INVALID:
        break;
    default:
        if (INV_ERROR_INVALID_PARAMETER) {
            LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
            return INV_ERROR_INVALID_PARAMETER;
        }
        break;
    }

    if (platformId == PLATFORM_ID_MSB_V2_MANTIS     ||
        platformId == PLATFORM_ID_MANTIS_MSB        ||
        platformId == PLATFORM_ID_MANTIS_USB_DONGLE ||
        platformId == PLATFORM_ID_MANTIS_PROTOTYPE  ||
        platformId == PLATFORM_ID_DRAGON_PROTOTYPE) {
        switch (compassId) {
        case ID_INVALID:
            g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID;
            break;
        case COMPASS_ID_AK8975:
        case COMPASS_ID_AMI306:
            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_SECONDARY;
            break;
        default:
            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
        };
    } else {
        g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
    }

#ifndef WIN32
    if (compassId != ID_INVALID)
        Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation);
#endif

    return INV_SUCCESS;
}

/**
 * @internal
 * Sets up the orientation matrix according to how the part was
 * mounted.
 *
 * @param platforId Platform identification for mounting information
 * @return INV_SUCCESS or non-zero error code
 */
#if 0
inv_error_t SetupPressureCalibration(unsigned short platformId,
                                  unsigned short pressureId)
{
    inv_error_t result = INV_SUCCESS;
    /*----  setup the compass ----*/
    switch(pressureId) {
        case PRESSURE_ID_BMA085:
            result = SetupPressureBMA085Calibration(platformId);
            break;
        default:
            if (INV_ERROR_INVALID_PARAMETER) {
                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
                return INV_ERROR_INVALID_PARAMETER;
            }
    };

    return result;
}
#endif
/**
 * @internal
 * Sets up the orientation matrix according to how the gyro was
 * mounted.
 *
 * @param platforId Platform identification for mounting information
 * @return INV_SUCCESS or non-zero error code
 */
static inv_error_t SetupGyroCalibration(unsigned short platformId)
{
    int position;
    MPL_LOGV("Calibrating '%s'\n", __func__);

    /* Orientation */
    switch (platformId) {
    case PLATFORM_ID_SPIDER_PROTOTYPE:
        position = 2;
        break;
    case PLATFORM_ID_MSB:
    case PLATFORM_ID_MSB_10AXIS:
    case PLATFORM_ID_MANTIS_MSB:
#ifndef WIN32
        position = 4;
#else
        position = 6;
#endif
        break;
    case PLATFORM_ID_DONGLE:
    case PLATFORM_ID_MANTIS_USB_DONGLE:
        position = 1;
        break;
    case PLATFORM_ID_DRAGON_USB_DONGLE:
        position = 3;
        break;
    case PLATFORM_ID_MANTIS_PROTOTYPE:
    case PLATFORM_ID_DRAGON_PROTOTYPE:
    case PLATFORM_ID_ST_6AXIS:
    case PLATFORM_ID_MSB_V2:
    case PLATFORM_ID_MSB_V2_MANTIS:
#ifndef WIN32
        position = 2;
#else
        position = 0;
#endif
        break;
    case PLATFORM_ID_MANTIS_EVB:
    case PLATFORM_ID_MSB_EVB:
        position = 0;
        break;
    case PLATFORM_ID_MSB_V3:
        position = 2;
        break;
    default:
        MPL_LOGE("Unsupported platform %d\n", platformId);
        return INV_ERROR_INVALID_PARAMETER;
    };

    SetupOrientation(position, g_gyro_orientation);

    return INV_SUCCESS;
}

/**
 *  @brief  Setup the Hw orientation and full scale.
 *  @param  platfromId
 *              an user defined Id to distinguish the Hw platform in
 *              use from others.
 *  @param  accelId
 *              the accelerometer specific id, as specified in the MPL.
 *  @param  compassId
 *              the compass specific id, as specified in the MPL.
 *  @return INV_SUCCESS or a non-zero error code.
 */
inv_error_t SetupPlatform(
                unsigned short platformId,
                unsigned short accelId,
                unsigned short compassId)
{
    int result;

    memset(&g_slave_accel, 0, sizeof(g_slave_accel));
    memset(&g_slave_compass, 0, sizeof(g_slave_compass));
//    memset(&g_slave_pressure, 0, sizeof(g_slave_pressure));
//    memset(&g_pdata, 0, sizeof(g_pdata));

#ifdef LINUX
    /* On Linux initialize the global platform data with the driver defaults */
    {
        void *mpu_handle;
        int ii;

        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;

        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;

        MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n");
        result = inv_serial_open("/dev/mpu",&mpu_handle);
        if (result) {
            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
        }
        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
            if (!slave[ii])
                continue;
            slave[ii]->type = ii;
            result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR,
                           slave[ii]);
            if (result)
                result = errno;
            if(result == INV_ERROR_INVALID_MODULE) {
                slave[ii] = NULL;
                result = 0;
            } else if (result) {
                LOG_RESULT_LOCATION(result);
                LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE);
                return result;
            }
        }
        //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata);
        if (result) {
            result = errno;
            LOG_RESULT_LOCATION(result);
            return result;
        }
        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
            if (!pdata_slave[ii])
                continue;
            pdata_slave[ii]->type = ii;
            result = ioctl(
                (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA,
                pdata_slave[ii]);
            if (result)
                result = errno;
            if (result == INV_ERROR_INVALID_MODULE) {
                pdata_slave[ii] = NULL;
                result = 0;
            } else if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
        if (result) {
            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
        }
        inv_serial_close(mpu_handle);
    }
#endif

    result = SetupGyroCalibration(platformId);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    PrintMountingOrientation("Gyroscope", g_gyro_orientation);
    result = SetupAccelCalibration(platformId, accelId);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation);
    result = SetupCompassCalibration(platformId, compassId);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation);
#if 0
    if (platformId == PLATFORM_ID_MSB_10AXIS) {
        result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation);
    }
#endif
#ifdef LINUX
    /* On Linux override the orientation, level shifter etc */
    {
        void *mpu_handle;
        int ii;
        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;

        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;

        MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n");
        result = inv_serial_open("/dev/mpu",&mpu_handle);
        if (result) {
            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
        }
        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
            if (!slave[ii])
                continue;
            slave[ii]->type = ii;
            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
                           slave[ii]);
            if (result)
                result = errno;
            if (result == INV_ERROR_INVALID_MODULE) {
                slave[ii] = NULL;
                result = 0;
            } else if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
        //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata);
        if (result) {
            result = errno;
            LOG_RESULT_LOCATION(result);
            return result;
        }
        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
            if (!pdata_slave[ii])
                continue;
            pdata_slave[ii]->type = ii;
            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
                pdata_slave[ii]);
            if (result)
                result = errno;
            if (result == INV_ERROR_INVALID_MODULE) {
                pdata_slave[ii] = NULL;
                result = 0;
            } else if (result) {
                LOG_RESULT_LOCATION(result);
                return result;
            }
        }
        if (result) {
            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
        }
        inv_serial_close(mpu_handle);
    }
#endif
    return INV_SUCCESS;
}

/**
 * @}
 */