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, 0 insertions, 1722 deletions
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c deleted file mode 100644 index f11bce9..0000000 --- a/libsensors_iio/software/simple_apps/common/mlsetup.c +++ /dev/null @@ -1,1722 +0,0 @@ -/* - $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; -} - -/** - * @} - */ - - |