diff options
Diffstat (limited to 'libsensors_iio/software/simple_apps/common/mlsetup.c')
| -rw-r--r-- | libsensors_iio/software/simple_apps/common/mlsetup.c | 1722 |
1 files changed, 1722 insertions, 0 deletions
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c new file mode 100644 index 0000000..f11bce9 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlsetup.c @@ -0,0 +1,1722 @@ +/* + $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; +} + +/** + * @} + */ + + |