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