diff options
Diffstat (limited to 'libsensors_iio/software/core')
45 files changed, 785 insertions, 2851 deletions
diff --git a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h deleted file mode 100644 index fce28ae..0000000 --- a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#ifndef _INV_SYSFS_UTILS_H_ -#define _INV_SYSFS_UTILS_H_ - -/** - * struct inv_sysfs_names_s - Files needed by user applications. - * @buffer: Ring buffer attached to FIFO. - * @enable: Turns on HW-to-ring buffer flow. - * @raw_data: Raw data from registers. - * @temperature: Temperature data from register. - * @fifo_rate: FIFO rate/ODR. - * @power_state: Power state (this is a five-star comment). - * @fsr: Full-scale range. - * @lpf: Digital low pass filter. - * @scale: LSBs / dps (or LSBs / Gs). - * @temp_scale: LSBs / degrees C. - * @temp_offset: Offset in LSBs. - */ -struct inv_sysfs_names_s { - - //Sysfs for ITG3500 & MPU6050 - const char *buffer; - const char *enable; - const char *raw_data; //Raw Gyro data - const char *temperature; - const char *fifo_rate; - const char *power_state; - const char *fsr; - const char *lpf; - const char *scale; //Gyro scale - const char *temp_scale; - const char *temp_offset; - const char *self_test; - //Starting Sysfs available for MPU6050 only - const char *accel_en; - const char *accel_fifo_en; - const char *accel_fs; - const char *clock_source; - const char *early_suspend_en; - const char *firmware_loaded; - const char *gyro_en; - const char *gyro_fifo_en; - const char *key; - const char *raw_accel; - const char *reg_dump; - const char *tap_on; - const char *dmp_firmware; -}; - -/* File IO. Typically won't be called directly by user application, but they'll - * be here for your enjoyment. - */ -int inv_sysfs_write(char *filename, long data); -int inv_sysfs_read(char *filename, long num_bytes, char *data); - -/* Helper APIs to extract specific data. */ -int inv_read_buffer(int fd, long *data, long long *timestamp); -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp); -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); - -/* Scaled data. */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); - -#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ - - diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h deleted file mode 100644 index ce06b07..0000000 --- a/libsensors_iio/software/core/HAL/include/mlos.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef _MLOS_H -#define _MLOS_H - -#ifndef __KERNEL__ -#include <stdio.h> -#endif - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(LINUX) || defined(__KERNEL__) -typedef unsigned int HANDLE; -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* - MLOSCreateFile defines. - */ - -#define MLOS_GENERIC_READ ((unsigned int)0x80000000) -#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) -#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) -#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) -#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) - - /* ---------- */ - /* - Enums. - */ - /* ---------- */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - -#ifndef __KERNEL__ -#include <string.h> - void *inv_malloc(unsigned int numBytes); - inv_error_t inv_free(void *ptr); - inv_error_t inv_create_mutex(HANDLE *mutex); - inv_error_t inv_lock_mutex(HANDLE mutex); - inv_error_t inv_unlock_mutex(HANDLE mutex); - FILE *inv_fopen(char *filename); - void inv_fclose(FILE *fp); - - // Binary Semaphore - inv_error_t inv_create_binary_semaphore(HANDLE *semaphore); - inv_error_t inv_destroy_binary_semaphore(HANDLE handle); - inv_error_t inv_increment_binary_semaphore(HANDLE handle); - inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms); - - inv_error_t inv_destroy_mutex(HANDLE handle); - - void inv_sleep(int mSecs); - inv_time_t inv_get_tick_count(void); - - /* Kernel implmentations */ -#define GFP_KERNEL (0x70) - static inline void *kmalloc(size_t size, - unsigned int gfp_flags) - { - (void)gfp_flags; - return inv_malloc((unsigned int)size); - } - static inline void *kzalloc(size_t size, unsigned int gfp_flags) - { - void *tmp = inv_malloc((unsigned int)size); - (void)gfp_flags; - if (tmp) - memset(tmp, 0, size); - return tmp; - } - static inline void kfree(void *ptr) - { - inv_free(ptr); - } - static inline void msleep(long msecs) - { - inv_sleep(msecs); - } - static inline void udelay(unsigned long usecs) - { - inv_sleep((usecs + 999) / 1000); - } -#else -#include <linux/delay.h> -#endif - -#ifdef __cplusplus -} -#endif -#endif /* _MLOS_H */ diff --git a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c deleted file mode 100644 index c45badd..0000000 --- a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c +++ /dev/null @@ -1,317 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#include <string.h> -#include <stdlib.h> -#include <ctype.h> -#include <stdio.h> -#include <stdint.h> -#include <dirent.h> -#include <errno.h> -#include <unistd.h> -#include "inv_sysfs_utils.h" - -/* General TODO list: - * Select more reasonable string lengths or use fseek and malloc. - */ - -/** - * inv_sysfs_write() - Write an integer to a file. - * @filename: Path to file. - * @data: Value to write to file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} - -/** - * inv_sysfs_read() - Read a string from a file. - * @filename: Path to file. - * @num_bytes: Number of bytes to read. - * @data: Data from file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -/** - * inv_read_buffer() - Read data from ring buffer. - * @fd: File descriptor for buffer file. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_buffer(int fd, long *data, long long *timestamp) -{ - char str[35]; - int count; - - count = read(fd, str, sizeof(str)); - if (!count) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_raw() - Read raw data. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - char str[40]; - int count; - - count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); - if (count < 0) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_temperature_raw() - Read temperature. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp) -{ - char str[25]; - int count; - - count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd%lld", &data[0], timestamp); - if (count < 2) - return -EAGAIN; - return count; -} - -/** - * inv_read_fifo_rate() - Read fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) -{ - char str[8]; - int count; - - count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_power_state() - Read power state. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) -{ - char str[2]; - int count; - - count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", (short*)data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_scale() - Read scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) -{ - char str[5]; - int count; - - count = inv_sysfs_read((char*)names->scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%f", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_scale() - Read temperature scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_offset() - Read temperature offset. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_q16() - Get data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count; - short raw[3]; - float scale; - count = inv_read_raw(names, (long*)raw, timestamp); - count += inv_read_scale(names, &scale); - data[0] = (long)(raw[0] * (65536.f / scale)); - data[1] = (long)(raw[1] * (65536.f / scale)); - data[2] = (long)(raw[2] * (65536.f / scale)); - return count; -} - -/** - * inv_read_q16() - Get temperature data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes read or a negative error code. - */ -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count = 0; - short raw; - static short scale, offset; - static unsigned char first_read = 1; - - if (first_read) { - count += inv_read_temp_scale(names, &scale); - count += inv_read_temp_offset(names, &offset); - first_read = 0; - } - count += inv_read_temperature_raw(names, &raw, timestamp); - data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); - - return count; -} - -/** - * inv_write_fifo_rate() - Write fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) -{ - return inv_sysfs_write((char*)names->fifo_rate, (long)data); -} - -/** - * inv_write_buffer_enable() - Enable/disable buffer in /dev. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->enable, (long)data); -} - -/** - * inv_write_power_state() - Turn device on/off. - * @names: Names of sysfs files. - * @data: 1 to turn on. - * Returns number of bytes written or a negative error code. - */ -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->power_state, (long)data); -} - - diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c deleted file mode 100644 index 75f062e..0000000 --- a/libsensors_iio/software/core/HAL/linux/mlos_linux.c +++ /dev/null @@ -1,194 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup MLOS - * @brief OS Interface. - * - * @{ - * @file mlos.c - * @brief OS Interface. -**/ - -/* ------------- */ -/* - Includes. - */ -/* ------------- */ - -#include <sys/time.h> -#include <unistd.h> -#include <pthread.h> -#include <stdlib.h> - -#include "stdint_invensense.h" - -#include "mlos.h" -#include <errno.h> - - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Allocate space - * @param numBytes number of bytes - * @return pointer to allocated space -**/ -void *inv_malloc(unsigned int numBytes) -{ - // Allocate space. - void *allocPtr = malloc(numBytes); - return allocPtr; -} - - -/** - * @brief Free allocated space - * @param ptr pointer to space to deallocate - * @return error code. -**/ -inv_error_t inv_free(void *ptr) -{ - // Deallocate space. - free(ptr); - - return INV_SUCCESS; -} - - -/** - * @brief Mutex create function - * @param mutex pointer to mutex handle - * @return error code. - */ -inv_error_t inv_create_mutex(HANDLE *mutex) -{ - int res; - pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); - if(pm == NULL) - return INV_ERROR; - - res = pthread_mutex_init(pm, NULL); - if(res == -1) { - free(pm); - return INV_ERROR_OS_CREATE_FAILED; - } - - *mutex = (HANDLE)pm; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex lock function - * @param mutex Mutex handle - * @return error code. - */ -inv_error_t inv_lock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_lock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex unlock function - * @param mutex mutex handle - * @return error code. -**/ -inv_error_t inv_unlock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_unlock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief open file - * @param filename name of the file to open. - * @return error code. - */ -FILE *inv_fopen(char *filename) -{ - FILE *fp = fopen(filename,"r"); - return fp; -} - - -/** - * @brief close the file. - * @param fp handle to file to close. - * @return error code. - */ -void inv_fclose(FILE *fp) -{ - fclose(fp); -} - -/** - * @brief Close Handle - * @param handle handle to the resource. - * @return Zero if success, an error code otherwise. - */ -inv_error_t inv_destroy_mutex(HANDLE handle) -{ - int error; - pthread_mutex_t *pm = (pthread_mutex_t*)handle; - error = pthread_mutex_destroy(pm); - if (error) { - return errno; - } - free((void*) handle); - - return INV_SUCCESS;} - - -/** - * @brief Sleep function. - */ -void inv_sleep(int mSecs) -{ - usleep(mSecs*1000); -} - - -/** - * @brief get system's internal tick count. - * Used for time reference. - * @return current tick count. - */ -unsigned long inv_get_tick_count() -{ - struct timeval tv; - - if (gettimeofday(&tv, NULL) !=0) - return 0; - - return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); -} - - /**********************/ - /** @} */ /* defgroup */ -/**********************/ - diff --git a/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h index 9b29695..4391226 100644 --- a/libsensors_iio/software/core/driver/include/linux/mpu.h +++ b/libsensors_iio/software/core/driver/include/linux/mpu.h @@ -27,135 +27,8 @@ #ifdef __KERNEL__ #include <linux/types.h> #include <linux/ioctl.h> -#elif defined LINUX -#include <sys/ioctl.h> -#include <linux/types.h> -#else -#include "mltypes.h" #endif -struct mpu_read_write { - /* Memory address or register address depending on ioctl */ - __u16 address; - __u16 length; - __u8 *data; -}; - -enum mpuirq_data_type { - MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ, - MPUIRQ_DATA_TYPE_MPU_FIFO_READY_IRQ, - MPUIRQ_DATA_TYPE_SLAVE_IRQ, - MPUIRQ_DATA_TYPE_PM_EVENT, - MPUIRQ_DATA_TYPE_NUM_TYPES, -}; - -/* User space PM event notification */ -#define MPU_PM_EVENT_SUSPEND_PREPARE (3) -#define MPU_PM_EVENT_POST_SUSPEND (4) - -/** - * struct mpuirq_data - structure to report what and when - * @interruptcount : The number of times this IRQ has occured since open - * @irqtime : monotonic time of the IRQ in ns - * @data_type : The type of this IRQ enum mpuirq_data_type - * @data : Data associated with this IRQ - */ -struct mpuirq_data { - __u32 interruptcount; - __s64 irqtime_ns; - __u32 data_type; - __s32 data; -}; - -enum ext_slave_config_key { - /* TODO: Remove these first six. */ - MPU_SLAVE_CONFIG_ODR_SUSPEND, - MPU_SLAVE_CONFIG_ODR_RESUME, - MPU_SLAVE_CONFIG_FSR_SUSPEND, - MPU_SLAVE_CONFIG_FSR_RESUME, - MPU_SLAVE_CONFIG_IRQ_SUSPEND, - MPU_SLAVE_CONFIG_IRQ_RESUME, - MPU_SLAVE_CONFIG_ODR, - MPU_SLAVE_CONFIG_FSR, - MPU_SLAVE_CONFIG_MOT_THS, - MPU_SLAVE_CONFIG_NMOT_THS, - MPU_SLAVE_CONFIG_MOT_DUR, - MPU_SLAVE_CONFIG_NMOT_DUR, - MPU_SLAVE_CONFIG_IRQ, - MPU_SLAVE_WRITE_REGISTERS, - MPU_SLAVE_READ_REGISTERS, - MPU_SLAVE_CONFIG_INTERNAL_REFERENCE, - /* AMI 306 specific config keys */ - MPU_SLAVE_PARAM, - MPU_SLAVE_WINDOW, - MPU_SLAVE_READWINPARAMS, - MPU_SLAVE_SEARCHOFFSET, - /* MPU3050 and MPU6050 Keys */ - MPU_SLAVE_INT_CONFIG, - MPU_SLAVE_EXT_SYNC, - MPU_SLAVE_FULL_SCALE, - MPU_SLAVE_LPF, - MPU_SLAVE_CLK_SRC, - MPU_SLAVE_DIVIDER, - MPU_SLAVE_DMP_ENABLE, - MPU_SLAVE_FIFO_ENABLE, - MPU_SLAVE_DMP_CFG1, - MPU_SLAVE_DMP_CFG2, - MPU_SLAVE_TC, - MPU_SLAVE_GYRO, - MPU_SLAVE_ADDR, - MPU_SLAVE_PRODUCT_REVISION, - MPU_SLAVE_SILICON_REVISION, - MPU_SLAVE_PRODUCT_ID, - MPU_SLAVE_GYRO_SENS_TRIM, - MPU_SLAVE_ACCEL_SENS_TRIM, - MPU_SLAVE_RAM, - /* -------------------------- */ - MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS -}; - -/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */ -enum ext_slave_config_irq_type { - MPU_SLAVE_IRQ_TYPE_NONE, - MPU_SLAVE_IRQ_TYPE_MOTION, - MPU_SLAVE_IRQ_TYPE_DATA_READY, -}; - -/* Structure for the following IOCTS's - * MPU_CONFIG_GYRO - * MPU_CONFIG_ACCEL - * MPU_CONFIG_COMPASS - * MPU_CONFIG_PRESSURE - * MPU_GET_CONFIG_GYRO - * MPU_GET_CONFIG_ACCEL - * MPU_GET_CONFIG_COMPASS - * MPU_GET_CONFIG_PRESSURE - * - * @key one of enum ext_slave_config_key - * @len length of data pointed to by data - * @apply zero if communication with the chip is not necessary, false otherwise - * This flag can be used to select cached data or to refresh cashed data - * cache data to be pushed later or push immediately. If true and the - * slave is on the secondary bus the MPU will first enger bypass mode - * before calling the slaves .config or .get_config funcion - * @data pointer to the data to confgure or get - */ -struct ext_slave_config { - __u8 key; - __u16 len; - __u8 apply; - void *data; -}; - -enum ext_slave_type { - EXT_SLAVE_TYPE_GYROSCOPE, - EXT_SLAVE_TYPE_ACCEL, - EXT_SLAVE_TYPE_COMPASS, - EXT_SLAVE_TYPE_PRESSURE, - /*EXT_SLAVE_TYPE_TEMPERATURE */ - - EXT_SLAVE_NUM_TYPES -}; enum secondary_slave_type { SECONDARY_SLAVE_TYPE_NONE, SECONDARY_SLAVE_TYPE_ACCEL, @@ -204,127 +77,6 @@ enum ext_slave_id { }; #define INV_PROD_KEY(ver, rev) (ver * 100 + rev) - -enum ext_slave_endian { - EXT_SLAVE_BIG_ENDIAN, - EXT_SLAVE_LITTLE_ENDIAN, - EXT_SLAVE_FS8_BIG_ENDIAN, - EXT_SLAVE_FS16_BIG_ENDIAN, -}; - -enum ext_slave_bus { - EXT_SLAVE_BUS_INVALID = -1, - EXT_SLAVE_BUS_PRIMARY = 0, - EXT_SLAVE_BUS_SECONDARY = 1 -}; - - -/** - * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050 - * slave devices - * - * @type: the type of slave device based on the enum ext_slave_type - * definitions. - * @irq: the irq number attached to the slave if any. - * @adapt_num: the I2C adapter number. - * @bus: the bus the slave is attached to: enum ext_slave_bus - * @address: the I2C slave address of the slave device. - * @orientation: the mounting matrix of the device relative to MPU. - * @irq_data: private data for the slave irq handler - * @private_data: additional data, user customizable. Not touched by the MPU - * driver. - * - * The orientation matricies are 3x3 rotation matricies - * that are applied to the data to rotate from the mounting orientation to the - * platform orientation. The values must be one of 0, 1, or -1 and each row and - * column should have exactly 1 non-zero value. - */ -struct ext_slave_platform_data { - __u8 type; - __u32 irq; - __u32 adapt_num; - __u32 bus; - __u8 address; - __s8 orientation[9]; - void *irq_data; - void *private_data; -}; - -struct fix_pnt_range { - __s32 mantissa; - __s32 fraction; -}; - -static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng) -{ - return (long)(rng.mantissa * 1000 + rng.fraction / 10); -} - -struct ext_slave_read_trigger { - __u8 reg; - __u8 value; -}; - -/** - * struct ext_slave_descr - Description of the slave device for programming. - * - * @suspend: function pointer to put the device in suspended state - * @resume: function pointer to put the device in running state - * @read: function that reads the device data - * @init: function used to preallocate memory used by the driver - * @exit: function used to free memory allocated for the driver - * @config: function used to configure the device - * @get_config:function used to get the device's configuration - * - * @name: text name of the device - * @type: device type. enum ext_slave_type - * @id: enum ext_slave_id - * @read_reg: starting register address to retrieve data. - * @read_len: length in bytes of the sensor data. Typically 6. - * @endian: byte order of the data. enum ext_slave_endian - * @range: full scale range of the slave ouput: struct fix_pnt_range - * @trigger: If reading data first requires writing a register this is the - * data to write. - * - * Defines the functions and information about the slave the mpu3050 and - * mpu6050 needs to use the slave device. - */ -struct ext_slave_descr { - int (*init) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*exit) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*suspend) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*resume) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*read) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - __u8 *data); - int (*config) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - struct ext_slave_config *config); - int (*get_config) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - struct ext_slave_config *config); - - char *name; - __u8 type; - __u8 id; - __u8 read_reg; - __u8 read_len; - __u8 endian; - struct fix_pnt_range range; - struct ext_slave_read_trigger *trigger; -}; - /** * struct mpu_platform_data - Platform data for the mpu driver * @int_config: Bits [7:3] of the int config register. @@ -334,6 +86,7 @@ struct ext_slave_descr { * @sec_slave_id: id of the secondary slave device * @secondary_i2c_address: secondary device's i2c address * @secondary_orientation: secondary device's orientation matrix + * @key: key for MPL library. * * Contains platform specific information on how to configure the MPU3050 to * work on this platform. The orientation matricies are 3x3 rotation matricies diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h index 74c49f3..0a747ce 100644 --- a/libsensors_iio/software/core/driver/include/log.h +++ b/libsensors_iio/software/core/driver/include/log.h @@ -352,9 +352,9 @@ static inline void __print_result_location(int result, #define INV_ERROR_CHECK(r_1329) \ - if (r_1329) { \
- LOG_RESULT_LOCATION(r_1329); \
- return r_1329; \
+ if (r_1329) { \ + LOG_RESULT_LOCATION(r_1329); \ + return r_1329; \ } #ifdef __cplusplus diff --git a/libsensors_iio/software/core/mllite/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt deleted file mode 100644 index 8650553..0000000 --- a/libsensors_iio/software/core/mllite/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -## CMakeLists for mlsdk/mldmp - -project(mldmp C) - -if (NOT CMAKE_BUILD_ENGINEERING) - -# # just copy the library from mldmp/mpl/$PLATFORM to mldmp/ -# if (CMAKE_SYSTEM_NAME MATCHES Windows) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Android) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Linux) -# message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a") -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Catalina) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# endif() -# # better way that doesn't work for now -# # add_custom_command( -# # TARGET mpl -# # PRE_BUILD -# # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR} -# # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}" -# # ) - -else (NOT CMAKE_BUILD_ENGINEERING) - - set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) - include(Engineering) - -endif (NOT CMAKE_BUILD_ENGINEERING) diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake deleted file mode 100644 index 42f2429..0000000 --- a/libsensors_iio/software/core/mllite/Engineering.cmake +++ /dev/null @@ -1,150 +0,0 @@ -## Engineering CMakeLists for software/core/mllite - -include_directories( - . - ${SOLUTION_SOURCE_DIR}/core/mpl - ${SOLUTION_SOURCE_DIR}/core/HAL - ${SOLUTION_SOURCE_DIR}/driver/include -) - -add_definitions ( - -DINV_INCLUDE_LEGACY_HEADERS -) - -if (CMAKE_SYSTEM_NAME MATCHES Android) - - include_directories( - ${SOLUTION_SOURCE_DIR}/core/mllite/linux - ${SOLUTION_SOURCE_DIR}/driver/include/linux - ) - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - linux/mlos.h - linux/ml_stored_data.h - linux/ml_load_dmp.h - linux/ml_sysfs_helper.h - ) - set (SOURCES - ${SOURCES} - linux/mlos_linux.c - linux/ml_stored_data.c - linux/ml_load_dmp.c - linux/ml_sysfs_helper.c - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Linux) - - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Windows) - - add_definitions( - -DAIO - -DUART_COM - -D_CRT_SECURE_NO_WARNINGS - -D_CRT_SECURE_NO_DEPRECATE - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -endif () - -set (HEADERS - ${HEADERS} - data_builder.h - hal_outputs.h - message_layer.h - ml_math_func.h - mpl.h - results_holder.h - start_manager.h - storage_manager.h -) -set (SOURCES - ${SOURCES} - data_builder.c - hal_outputs.c - message_layer.c - ml_math_func.c - mpl.c - results_holder.c - start_manager.c - storage_manager.c -) - -# coveniently add this file to the resources for easy access within IDEs -set (RESOURCES - Engineering.cmake -) - -if (CMAKE_TESTING_SUPPORT) - - message("Including Testing support") - include_directories( - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport - ) - add_definitions( - -DSELF_VERIFICATION - ) - set( - HEADERS - ${HEADERS} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h - ) - set( - SOURCES - ${SOURCES} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c - ) - -endif () - -############ -## TARGET ## -############ -add_library( - mllite STATIC - ${SOURCES} - ${HEADERS} - ${RESOURCES} -) -set_target_properties( - mllite - PROPERTIES CLEAN_DIRECT_OUTPUT 1 -) - -if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)") - - add_library( - mllite_shared SHARED - ${SOURCES} - ${HEADERS} - ${RESOURCES} - ) - FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite") - - install ( - TARGETS mllite_shared - DESTINATION lib - ) - -endif () - - diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/libsensors_iio/software/core/mllite/build/android/libmllite.so Binary files differnew file mode 100644 index 0000000..98147a2 --- /dev/null +++ b/libsensors_iio/software/core/mllite/build/android/libmllite.so diff --git a/libsensors_iio/software/core/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk deleted file mode 100644 index 6ad45de..0000000 --- a/libsensors_iio/software/core/mllite/build/android/static.mk +++ /dev/null @@ -1,110 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -OBJFOLDER = $(CURDIR)/obj - -CROSS = arm-none-linux-gnueabi- -COMP= $(CROSS)gcc -LINK= $(CROSS)ar cr - -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform - -include $(MLSDK_ROOT)/Android-common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -fpic -CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -DLINUX -DANDROID -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(MLPLATFORM_DIR)/include -CFLAGS += -I$(MLSDK_ROOT)/mlutils -CFLAGS += -I$(MLSDK_ROOT)/mlapps/common -CFLAGS += $(MLSDK_INCLUDES) -CFLAGS += $(MLSDK_DEFINES) - -VPATH += $(MLLITE_DIR) -VPATH += $(MLSDK_ROOT)/mlutils -VPATH += $(MLLITE_DIR)/accel -VPATH += $(MLLITE_DIR)/compass -VPATH += $(MLLITE_DIR)/pressure -VPATH += $(MLLITE_DIR)/mlapps/common - -#################################################################################################### -## sources - -ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) - -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c -ML_SOURCES += $(MLLITE_DIR)/accel.c -ML_SOURCES += $(MLLITE_DIR)/compass.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c -ML_SOURCES += $(MLLITE_DIR)/key0_96.c -ML_SOURCES += $(MLLITE_DIR)/pressure.c -ML_SOURCES += $(MLLITE_DIR)/ml.c -ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c -ML_SOURCES += $(MLLITE_DIR)/ml_init.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c -ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c -ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c -ML_SOURCES += $(MLLITE_DIR)/mldl.c -ML_SOURCES += $(MLLITE_DIR)/mldmp.c -ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c -ML_SOURCES += $(MLLITE_DIR)/mlstates.c -ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c -ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c -ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c -ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c -ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c -ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c -ifeq ($(MPU_NAME),MPU3050) -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c -else -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c -endif - -ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) -ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all clean cleanall - -all: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n") - $(LINK) $(LIBRARY) $(ML_OBJS_DST) - $(CROSS)ranlib $(LIBRARY) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c index f70be7c..b139771 100644 --- a/libsensors_iio/software/core/mllite/data_builder.c +++ b/libsensors_iio/software/core/mllite/data_builder.c @@ -23,6 +23,7 @@ #include "mlmath.h" #include "storage_manager.h" #include "message_layer.h" +#include "results_holder.h" #include "log.h" #undef MPL_LOG_TAG @@ -48,6 +49,10 @@ struct inv_db_save_t { /** Temperature when accel bias was stored. */ long accel_temp; long gyro_temp_slope[3]; + /** Sensor Accuracy */ + int gyro_accuracy; + int accel_accuracy; + int compass_accuracy; }; struct inv_data_builder_t { @@ -69,7 +74,7 @@ static struct inv_data_builder_t inv_data_builder; static struct inv_sensor_cal_t sensors; /** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53394 +#define INV_DB_SAVE_KEY 53395 #ifdef INV_PLAYBACK_DBG @@ -98,6 +103,14 @@ void inv_turn_off_data_logging() static inv_error_t inv_db_load_func(const unsigned char *data) { memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); + // copy in the saved accuracy in the actual sensors accuracy + sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; + sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; + sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; + // TODO + if (sensors.compass.accuracy == 3) { + inv_set_compass_bias_found(1); + } return INV_SUCCESS; } @@ -162,119 +175,7 @@ void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, sensor->sensitivity = sensitivity; sensor->orientation = orientation; } -void inv_get_quaternion_transformation(int orientation, long *q) -{ - long s = 759250125; // sqrt(.5)*2^30 - - switch (orientation) - { - case 0x70: - q[0] = 759250125; - q[1] = 759250125; - q[2] = 0; - q[3] = 0; - break; - case 0x10a: - q[0] = 759250125; - q[1] = 0; - q[2] = 759250125; - q[3] = 0; - break; - case 0x85: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = 759250125; - break; - case 0x181: - q[0] = 0; - q[1] = 759250125; - q[2] = 759250125; - q[3] = 0; - break; - case 0x2a: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = 759250125; - break; - case 0x54: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = 759250125; - break; - case 0x150: - q[0] = 759250125; - q[1] = -759250125; - q[2] = 0; - q[3] = 0; - break; - case 0xe: - q[0] = 759250125; - q[1] = 0; - q[2] = -759250125; - q[3] = 0; - break; - case 0xa1: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = -759250125; - break; - case 0x1a5: - q[0] = 0; - q[1] = 759250125; - q[2] = -759250125; - q[3] = 0; - break; - case 0x12e: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = -759250125; - break; - case 0x174: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = -759250125; - break; - - - case 0x88: - q[0] = 1073741824; - q[1] = 0; - q[2] = 0; - q[3] = 0; - break; - - - case 0x1a8: - q[0] = 0; - q[1] = 1073741824; - q[2] = 0; - q[3] = 0; - break; - - case 0x18c: - q[0] = 0; - q[1] = 0; - q[2] = 1073741824; - q[3] = 0; - break; - - case 0xac: - q[0] = 0; - q[1] = 0; - q[2] = 0; - q[3] = 1073741824; - break; - - default: - break; - } -} + /** Sets the Orientation and Sensitivity of the gyro data. * @param[in] orientation A scalar defining the transformation from chip mounting * to the body frame. The function inv_orientation_matrix_to_scalar() @@ -354,6 +255,22 @@ void inv_set_compass_sample_rate(long sample_rate_us) sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); } } + +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.gyro.sample_rate_ms; +} + +void inv_get_accel_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.accel.sample_rate_ms; +} + +void inv_get_compass_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.compass.sample_rate_ms; +} + /** Set Quat Sample rate in micro seconds. * @param[in] sample_rate_us Set Quat Sample rate in us */ @@ -498,7 +415,7 @@ void inv_matrix_vector_mult(const long *A, const long *x, long *y) } /** Takes raw data stored in the sensor, removes bias, and converts it to -* calibrated data in the body frame. +* calibrated data in the body frame. Also store raw data for body frame. * @param[in,out] sensor structure to modify * @param[in] bias bias in the mounting frame, in hardware units scaled by * 2^16. Length 3. @@ -508,15 +425,17 @@ void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) long raw32[3]; // Convert raw to calibrated - raw32[0] = (long)sensor->raw[0] << 16; - raw32[1] = (long)sensor->raw[1] << 16; - raw32[2] = (long)sensor->raw[2] << 16; + raw32[0] = (long)sensor->raw[0] << 15; + raw32[1] = (long)sensor->raw[1] << 15; + raw32[2] = (long)sensor->raw[2] << 15; - raw32[0] -= bias[0]; - raw32[1] -= bias[1]; - raw32[2] -= bias[2]; + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_data); - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated); + raw32[0] -= bias[0] >> 1; + raw32[1] -= bias[1] >> 1; + raw32[2] -= bias[2] >> 1; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); sensor->status |= INV_CALIBRATED; } @@ -539,6 +458,7 @@ void inv_set_compass_bias(const long *bias, int accuracy) inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); } sensors.compass.accuracy = accuracy; + inv_data_builder.save.compass_accuracy = accuracy; inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); } @@ -566,6 +486,7 @@ void inv_set_accel_bias(const long *bias, int accuracy) } } sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); } @@ -590,6 +511,7 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); } sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; } @@ -607,6 +529,8 @@ void inv_set_gyro_bias(const long *bias, int accuracy) } } sensors.gyro.accuracy = accuracy; + inv_data_builder.save.gyro_accuracy = accuracy; + /* TODO: What should we do if there's no temperature data? */ if (sensors.temp.calibrated[0]) inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; @@ -687,6 +611,7 @@ inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) sensors.accel.calibrated[2] = accel[2]; sensors.accel.status |= INV_CALIBRATED; sensors.accel.accuracy = status & 3; + inv_data_builder.save.accel_accuracy = status & 3; } sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; sensors.accel.timestamp_prev = sensors.accel.timestamp; @@ -757,6 +682,7 @@ inv_error_t inv_build_compass(const long *compass, int status, sensors.compass.calibrated[2] = compass[2]; sensors.compass.status |= INV_CALIBRATED; sensors.compass.accuracy = status & 3; + inv_data_builder.save.compass_accuracy = status & 3; } sensors.compass.timestamp_prev = sensors.compass.timestamp; sensors.compass.timestamp = timestamp; @@ -1077,6 +1003,22 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) } } +/** Gets a whole set of gyro raw data including data, accuracy and timestamp. + * @param[out] data Gyro Data where 1 dps = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.raw_data, sizeof(sensors.gyro.raw_data)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.gyro.accuracy; + } +} + /** Get's latest gyro data. * @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. */ diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h index b2d0881..4263922 100644 --- a/libsensors_iio/software/core/mllite/data_builder.h +++ b/libsensors_iio/software/core/mllite/data_builder.h @@ -56,6 +56,7 @@ extern "C" { #define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850 #define INV_PRIORITY_HAL_OUTPUTS 900 #define INV_PRIORITY_GLYPH 950 +#define INV_PRIORITY_SHAKE 975 #define INV_PRIORITY_SM 1000 struct inv_single_sensor_t { @@ -69,6 +70,8 @@ struct inv_single_sensor_t { int orientation; /** The raw data in raw data units in the mounting frame */ short raw[3]; + /** Raw data in body frame */ + long raw_data[3]; /** Calibrated data */ long calibrated[3]; long sensitivity; @@ -160,6 +163,10 @@ void inv_set_gyro_bandwidth(int bandwidth_hz); void inv_set_accel_bandwidth(int bandwidth_hz); void inv_set_compass_bandwidth(int bandwidth_hz); +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms); +void inv_get_accel_sample_rate_ms(long *sample_rate_ms); +void inv_get_compass_sample_rate_ms(long *sample_rate_ms); + inv_error_t inv_register_data_cb(inv_error_t (*func) (struct inv_sensor_cal_t * data), int priority, int sensor_type); @@ -197,6 +204,7 @@ long inv_get_compass_sensitivity(void); void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp); void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp); +void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); void inv_get_gyro(long *gyro); diff --git a/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt deleted file mode 100644 index 4643ed5..0000000 --- a/libsensors_iio/software/core/mllite/dmpconfig.txt +++ /dev/null @@ -1,3 +0,0 @@ -major version = 1 -minor version = 0 -startAddr = 0 diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c index 1cd3b81..5e7b2cc 100644 --- a/libsensors_iio/software/core/mllite/hal_outputs.c +++ b/libsensors_iio/software/core/mllite/hal_outputs.c @@ -1,425 +1,475 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/** - * @defgroup HAL_Outputs hal_outputs - * @brief Motion Library - HAL Outputs - * Sets up common outputs for HAL - * - * @{ - * @file hal_outputs.c - * @brief HAL Outputs. - */ -#include "hal_outputs.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" -#include "start_manager.h" -#include "data_builder.h" -#include "results_holder.h" - -struct hal_output_t { - int accuracy_mag; /**< Compass accuracy */ - int accuracy_gyro; /**< Gyro Accuracy */ - int accuracy_accel; /**< Accel Accuracy */ - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - inv_time_t accel_timestamp; - long nav_quat[4]; - int gyro_status; - int accel_status; - int compass_status; - int nine_axis_status; -}; - -static struct hal_output_t hal_out; - -/** Acceleration (m/s^2) in body frame. -* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it -* should return a vector of magnitude near 9.81 m/s^2 -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - int status; - /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. - * So this 9.80665 / 2^16 */ -#define ACCEL_CONVERSION 0.000149637603759766f - long accel[3]; - inv_get_accel_set(accel, accuracy, timestamp); - values[0] = accel[0] * ACCEL_CONVERSION; - values[1] = accel[1] * ACCEL_CONVERSION; - values[2] = accel[2] * ACCEL_CONVERSION; - if (hal_out.accel_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - -/** Linear Acceleration (m/s^2) in Body Frame. -* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show -* accel biases while at rest. -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long gravity[3], accel[3]; - - inv_get_accel_set(accel, accuracy, timestamp); - inv_get_gravity(gravity); - accel[0] -= gravity[0] >> 14; - accel[1] -= gravity[1] >> 14; - accel[2] -= gravity[2] >> 14; - values[0] = accel[0] * ACCEL_CONVERSION; - values[1] = accel[1] * ACCEL_CONVERSION; - values[2] = accel[2] * ACCEL_CONVERSION; - - return hal_out.nine_axis_status; -} - -/** Gravity vector (m/s^2) in Body Frame. -* @param[out] values Gravity vector in body frame, length 3, (m/s^2) -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long gravity[3]; - int status; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - inv_get_gravity(gravity); - values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; - values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; - values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; - if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) - status = 1; - else - status = 0; - return status; -} - -/** Gyroscope data (rad/s) in body frame. -* @param[out] values Rotation Rate in rad/sec. -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_gyro(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. - * So this is: pi / 2^16 / 180 */ -#define GYRO_CONVERSION 2.66316109007924e-007f - long gyro[3]; - int status; - - inv_get_gyro_set(gyro, accuracy, timestamp); - values[0] = gyro[0] * GYRO_CONVERSION; - values[1] = gyro[1] * GYRO_CONVERSION; - values[2] = gyro[2] * GYRO_CONVERSION; - if (hal_out.gyro_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - -/** -* This corresponds to Sensor.TYPE_ROTATION_VECTOR. -* The rotation vector represents the orientation of the device as a combination -* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ -* around an axis {x, y, z}. <br> -* The three elements of the rotation vector are -* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation -* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is -* equal to the direction of the axis of rotation. -* -* The three elements of the rotation vector are equal to the last three components of a unit quaternion -* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2). -* -* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. -* The reference coordinate system is defined as a direct orthonormal basis, where: - - -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). - -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. - -Z points towards the sky and is perpendicular to the ground. -* @param[out] values Length 4. -* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate -* @param[out] timestamp Timestamp. In (ns) for Android. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - - if (hal_out.nav_quat[0] >= 0) { - values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; - values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; - values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; - values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; - } else { - values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; - values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; - values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; - values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; - } - values[4] = inv_get_heading_confidence_interval(); - - return hal_out.nine_axis_status; -} - - -/** Compass data (uT) in body frame. -* @param[out] values Compass data in (uT), length 3. May be calibrated by having -* biases removed and sensitivity adjusted -* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate -* @param[out] timestamp Timestamp. In (ns) for Android. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - int status; - /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. - * So this is: 1 / 2^16*/ -#define COMPASS_CONVERSION 1.52587890625e-005f - long compass[3]; - inv_get_compass_set(compass, accuracy, timestamp); - values[0] = compass[0] * COMPASS_CONVERSION; - values[1] = compass[1] * COMPASS_CONVERSION; - values[2] = compass[2] * COMPASS_CONVERSION; - if (hal_out.compass_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - - -/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. -* @param[out] values Length 3, Degrees.<br> -* - values[0]: Azimuth, angle between the magnetic north direction -* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> -* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values -* when the z-axis moves toward the y-axis.<br> -* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive -* values when the x-axis moves toward the z-axis.<br> -* -* @note This definition is different from yaw, pitch and roll used in aviation -* where the X axis is along the long side of the plane (tail to nose). -* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() -* in conjunction with remapCoordinateSystem() and getOrientation() to compute -* these values instead. -* Important note: For historical reasons the roll angle is positive in the -* clockwise direction (mathematically speaking, it should be positive in -* the counter-clockwise direction). -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long t1, t2, t3; - long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - - q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]); - q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]); - q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]); - q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]); - q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]); - q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]); - q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]); - q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]); - q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]); - q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]); - - /* X component of the Ybody axis in World frame */ - t1 = q12 - q03; - - /* Y component of the Ybody axis in World frame */ - t2 = q22 + q00 - (1L << 30); - values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; - if (values[0] < 0) - values[0] += 360; - - /* Z component of the Ybody axis in World frame */ - t3 = q23 + q01; - values[1] = - -atan2f((float) t3, - sqrtf((float) t1 * t1 + - (float) t2 * t2)) * 180.f / (float) M_PI; - /* Z component of the Zbody axis in World frame */ - t2 = q33 + q00 - (1L << 30); - if (t2 < 0) { - if (values[1] >= 0) - values[1] = 180.f - values[1]; - else - values[1] = -180.f - values[1]; - } - - /* X component of the Xbody axis in World frame */ - t1 = q11 + q00 - (1L << 30); - /* Y component of the Xbody axis in World frame */ - t2 = q12 + q03; - /* Z component of the Xbody axis in World frame */ - t3 = q13 - q02; - //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI; - - values[2] = - -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * - 180.f / (float) M_PI - 90); - if (values[2] >= 90) - values[2] = 180 - values[2]; - - if (values[2] < -90) - values[2] = -180 - values[2]; - - return hal_out.nine_axis_status; -} - -/** Main callback to generate HAL outputs. Typically not called by library users. -* @param[in] sensor_cal Input variable to take sensor data whenever there is new -* sensor data. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) -{ - int use_sensor = 0; - long sr; - (void) sensor_cal; - inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag, - &hal_out.nav_timestamp); - hal_out.gyro_status = sensor_cal->gyro.status; - hal_out.accel_status = sensor_cal->accel.status; - hal_out.compass_status = sensor_cal->compass.status; - - // Find the highest sample rate and tie generating 9-axis to that one. - if (sensor_cal->gyro.status & INV_SENSOR_ON) { - sr = sensor_cal->gyro.sample_rate_ms; - use_sensor = 0; - } - if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { - sr = sensor_cal->accel.sample_rate_ms; - use_sensor = 1; - } - if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { - sr = sensor_cal->compass.sample_rate_ms; - use_sensor = 2; - } - if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { - sr = sensor_cal->quat.sample_rate_ms; - use_sensor = 3; - } - - switch (use_sensor) { - default: - case 0: - hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->gyro.timestamp; - break; - case 1: - hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->accel.timestamp; - break; - case 2: - hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->compass.timestamp; - break; - case 3: - hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->quat.timestamp; - break; - } - - return INV_SUCCESS; -} - -/** Turns off generation of HAL outputs. -* @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_stop_hal_outputs(void) -{ - inv_error_t result; - result = inv_unregister_data_cb(inv_generate_hal_outputs); - return result; -} - -/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() -* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_start_hal_outputs(void) -{ - inv_error_t result; - result = - inv_register_data_cb(inv_generate_hal_outputs, - INV_PRIORITY_HAL_OUTPUTS, - INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); - return result; -} - -/** Initializes hal outputs class. This is called automatically by the -* enable function. It may be called any time the feature is enabled, but -* is typically not needed to be called by outside callers. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_init_hal_outputs(void) -{ - memset(&hal_out, 0, sizeof(hal_out)); - return INV_SUCCESS; -} - -/** Turns on creation and storage of HAL type results. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_enable_hal_outputs(void) -{ - inv_error_t result; - - // don't need to check the result for inv_init_hal_outputs - // since it's always INV_SUCCESS - inv_init_hal_outputs(); - - result = inv_register_mpl_start_notification(inv_start_hal_outputs); - return result; -} - -/** Turns off creation and storage of HAL type results. -*/ -inv_error_t inv_disable_hal_outputs(void) -{ - inv_error_t result; - - inv_stop_hal_outputs(); // Ignore error if we have already stopped this - result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); - return result; -} - -/** - * @} - */ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/**
+ * @defgroup HAL_Outputs hal_outputs
+ * @brief Motion Library - HAL Outputs
+ * Sets up common outputs for HAL
+ *
+ * @{
+ * @file hal_outputs.c
+ * @brief HAL Outputs.
+ */
+#include "hal_outputs.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+
+struct hal_output_t {
+ int accuracy_mag; /**< Compass accuracy */
+// int accuracy_gyro; /**< Gyro Accuracy */
+// int accuracy_accel; /**< Accel Accuracy */
+ int accuracy_quat; /**< quat Accuracy */
+
+ inv_time_t nav_timestamp;
+ inv_time_t gam_timestamp;
+// inv_time_t accel_timestamp;
+ inv_time_t mag_timestamp;
+ long nav_quat[4];
+ int gyro_status;
+ int accel_status;
+ int compass_status;
+ int nine_axis_status;
+ inv_biquad_filter_t lp_filter[3];
+ float compass_float[3];
+};
+
+static struct hal_output_t hal_out;
+
+/** Acceleration (m/s^2) in body frame.
+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
+* should return a vector of magnitude near 9.81 m/s^2
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ int status;
+ /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
+ * So this 9.80665 / 2^16 */
+#define ACCEL_CONVERSION 0.000149637603759766f
+ long accel[3];
+ inv_get_accel_set(accel, accuracy, timestamp);
+ values[0] = accel[0] * ACCEL_CONVERSION;
+ values[1] = accel[1] * ACCEL_CONVERSION;
+ values[2] = accel[2] * ACCEL_CONVERSION;
+ if (hal_out.accel_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/** Linear Acceleration (m/s^2) in Body Frame.
+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
+* accel biases while at rest.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gravity[3], accel[3];
+
+ inv_get_accel_set(accel, accuracy, timestamp);
+ inv_get_gravity(gravity);
+ accel[0] -= gravity[0] >> 14;
+ accel[1] -= gravity[1] >> 14;
+ accel[2] -= gravity[2] >> 14;
+ values[0] = accel[0] * ACCEL_CONVERSION;
+ values[1] = accel[1] * ACCEL_CONVERSION;
+ values[2] = accel[2] * ACCEL_CONVERSION;
+
+ return hal_out.nine_axis_status;
+}
+
+/** Gravity vector (m/s^2) in Body Frame.
+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gravity[3];
+ int status;
+
+ *accuracy = (int8_t) hal_out.accuracy_quat;
+ *timestamp = hal_out.nav_timestamp;
+ inv_get_gravity(gravity);
+ values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
+ values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
+ values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
+ if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA))
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/** Gyroscope calibrated data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_gyro().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+ long gyro[3];
+ int status;
+
+ inv_get_gyro_set(gyro, accuracy, timestamp);
+ values[0] = gyro[0] * GYRO_CONVERSION;
+ values[1] = gyro[1] * GYRO_CONVERSION;
+ values[2] = gyro[2] * GYRO_CONVERSION;
+ if (hal_out.gyro_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/** Gyroscope raw data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_gyro().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+ long gyro[3];
+ int status;
+
+ inv_get_gyro_set_raw(gyro, accuracy, timestamp);
+ values[0] = gyro[0] * GYRO_CONVERSION;
+ values[1] = gyro[1] * GYRO_CONVERSION;
+ values[2] = gyro[2] * GYRO_CONVERSION;
+ if (hal_out.gyro_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/**
+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
+* The rotation vector represents the orientation of the device as a combination
+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
+* around an axis {x, y, z}. <br>
+* The three elements of the rotation vector are
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
+* equal to the direction of the axis of rotation.
+*
+* The three elements of the rotation vector are equal to the last three components of a unit quaternion
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
+*
+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
+* The reference coordinate system is defined as a direct orthonormal basis, where:
+
+ -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
+ -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
+ -Z points towards the sky and is perpendicular to the ground.
+* @param[out] values Length 4.
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ *accuracy = (int8_t) hal_out.accuracy_quat;
+ *timestamp = hal_out.nav_timestamp;
+
+ if (hal_out.nav_quat[0] >= 0) {
+ values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
+ values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
+ values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
+ values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
+ } else {
+ values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
+ values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
+ values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
+ values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
+ }
+ values[4] = inv_get_heading_confidence_interval();
+
+ return hal_out.nine_axis_status;
+}
+
+
+/** Compass data (uT) in body frame.
+* @param[out] values Compass data in (uT), length 3. May be calibrated by having
+* biases removed and sensitivity adjusted
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ int status;
+ /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+ * So this is: 1 / 2^16*/
+//#define COMPASS_CONVERSION 1.52587890625e-005f
+ int i;
+
+ *timestamp = hal_out.mag_timestamp;
+ *accuracy = (int8_t) hal_out.accuracy_mag;
+
+ for (i=0; i<3; i++) {
+ values[i] = hal_out.compass_float[i];
+ }
+ if (hal_out.compass_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+static void inv_get_rotation(float r[3][3])
+{
+ long rot[9];
+ float conv = 1.f / (1L<<30);
+
+ inv_quaternion_to_rotation(hal_out.nav_quat, rot);
+ r[0][0] = rot[0]*conv;
+ r[0][1] = rot[1]*conv;
+ r[0][2] = rot[2]*conv;
+ r[1][0] = rot[3]*conv;
+ r[1][1] = rot[4]*conv;
+ r[1][2] = rot[5]*conv;
+ r[2][0] = rot[6]*conv;
+ r[2][1] = rot[7]*conv;
+ r[2][2] = rot[8]*conv;
+}
+
+static void google_orientation(float *g)
+{
+ float rad2deg = (float)(180.0 / M_PI);
+ float R[3][3];
+
+ inv_get_rotation(R);
+
+ g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+ g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+ g[2] = asinf ( R[2][0]) * rad2deg;
+ if (g[0] < 0)
+ g[0] += 360;
+}
+
+
+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
+* @param[out] values Length 3, Degrees.<br>
+* - values[0]: Azimuth, angle between the magnetic north direction
+* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
+* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
+* when the z-axis moves toward the y-axis.<br>
+* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
+* values when the x-axis moves toward the z-axis.<br>
+*
+* @note This definition is different from yaw, pitch and roll used in aviation
+* where the X axis is along the long side of the plane (tail to nose).
+* Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
+* in conjunction with remapCoordinateSystem() and getOrientation() to compute
+* these values instead.
+* Important note: For historical reasons the roll angle is positive in the
+* clockwise direction (mathematically speaking, it should be positive in
+* the counter-clockwise direction).
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor.
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ *accuracy = (int8_t) hal_out.accuracy_quat;
+ *timestamp = hal_out.nav_timestamp;
+
+ google_orientation(values);
+
+ return hal_out.nine_axis_status;
+}
+
+/** Main callback to generate HAL outputs. Typically not called by library users.
+* @param[in] sensor_cal Input variable to take sensor data whenever there is new
+* sensor data.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
+{
+ int use_sensor = 0;
+ long sr = 1000;
+ long compass[3];
+ int8_t accuracy;
+ int i;
+ (void) sensor_cal;
+
+ inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
+ &hal_out.nav_timestamp);
+ hal_out.gyro_status = sensor_cal->gyro.status;
+ hal_out.accel_status = sensor_cal->accel.status;
+ hal_out.compass_status = sensor_cal->compass.status;
+
+ // Find the highest sample rate and tie generating 9-axis to that one.
+ if (sensor_cal->gyro.status & INV_SENSOR_ON) {
+ sr = sensor_cal->gyro.sample_rate_ms;
+ use_sensor = 0;
+ }
+ if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
+ sr = sensor_cal->accel.sample_rate_ms;
+ use_sensor = 1;
+ }
+ if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
+ sr = sensor_cal->compass.sample_rate_ms;
+ use_sensor = 2;
+ }
+ if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
+ sr = sensor_cal->quat.sample_rate_ms;
+ use_sensor = 3;
+ }
+
+ switch (use_sensor) {
+ default:
+ case 0:
+ hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
+ break;
+ case 1:
+ hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->accel.timestamp;
+ break;
+ case 2:
+ hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->compass.timestamp;
+ break;
+ case 3:
+ hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->quat.timestamp;
+ break;
+ }
+
+ /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+ * So this is: 1 / 2^16*/
+ #define COMPASS_CONVERSION 1.52587890625e-005f
+
+ inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
+ hal_out.accuracy_mag = (int ) accuracy;
+
+ for (i=0; i<3; i++) {
+ if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
+ INV_NEW_DATA ) {
+ // set the state variables to match output with input
+ inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
+ }
+
+ if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
+ (INV_NEW_DATA | INV_RAW_DATA) ) {
+
+ hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
+ (float ) compass[i]) * COMPASS_CONVERSION;
+
+ } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) {
+ hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
+ }
+
+ }
+ return INV_SUCCESS;
+}
+
+/** Turns off generation of HAL outputs.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_stop_hal_outputs(void)
+{
+ inv_error_t result;
+ result = inv_unregister_data_cb(inv_generate_hal_outputs);
+ return result;
+}
+
+/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_start_hal_outputs(void)
+{
+ inv_error_t result;
+ result =
+ inv_register_data_cb(inv_generate_hal_outputs,
+ INV_PRIORITY_HAL_OUTPUTS,
+ INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
+ return result;
+}
+/* file name: lowPassFilterCoeff_1_6.c */
+float compass_low_pass_filter_coeff[5] =
+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
+
+/** Initializes hal outputs class. This is called automatically by the
+* enable function. It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_hal_outputs(void)
+{
+ int i;
+ memset(&hal_out, 0, sizeof(hal_out));
+ for (i=0; i<3; i++) {
+ inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
+ }
+
+ return INV_SUCCESS;
+}
+
+/** Turns on creation and storage of HAL type results.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_enable_hal_outputs(void)
+{
+ inv_error_t result;
+
+ // don't need to check the result for inv_init_hal_outputs
+ // since it's always INV_SUCCESS
+ inv_init_hal_outputs();
+
+ result = inv_register_mpl_start_notification(inv_start_hal_outputs);
+ return result;
+}
+
+/** Turns off creation and storage of HAL type results.
+*/
+inv_error_t inv_disable_hal_outputs(void)
+{
+ inv_error_t result;
+
+ inv_stop_hal_outputs(); // Ignore error if we have already stopped this
+ result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
+ return result;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h index ed4cb65..85e88f3 100644 --- a/libsensors_iio/software/core/mllite/hal_outputs.h +++ b/libsensors_iio/software/core/mllite/hal_outputs.h @@ -19,6 +19,8 @@ extern "C" { inv_time_t * timestamp); int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, inv_time_t * timestamp); + int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp); int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, inv_time_t * timestamp); int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, diff --git a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c deleted file mode 100644 index 649b917..0000000 --- a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c +++ /dev/null @@ -1,318 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#include <string.h> -#include <stdlib.h> -#include <ctype.h> -#include <stdio.h> -#include <stdint.h> -#include <dirent.h> -#include <errno.h> -#include <unistd.h> -#include "inv_sysfs_utils.h" - -/* General TODO list: - * Select more reasonable string lengths or use fseek and malloc. - */ - -/** - * inv_sysfs_write() - Write an integer to a file. - * @filename: Path to file. - * @data: Value to write to file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} - -/** - * inv_sysfs_read() - Read a string from a file. - * @filename: Path to file. - * @num_bytes: Number of bytes to read. - * @data: Data from file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -/** - * inv_read_buffer() - Read data from ring buffer. - * @fd: File descriptor for buffer file. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_buffer(int fd, long *data, long long *timestamp) -{ - char str[35]; - int count; - - count = read(fd, str, sizeof(str)); - if (!count) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_raw() - Read raw data. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - char str[40]; - int count; - - count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); - if (count < 0) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_temperature_raw() - Read temperature. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp) -{ - char str[25]; - int count; - - count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd%lld", &data[0], timestamp); - if (count < 2) - return -EAGAIN; - return count; -} - -/** - * inv_read_fifo_rate() - Read fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) -{ - char str[8]; - int count; - - count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_power_state() - Read power state. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) -{ - char str[2]; - int count; - - count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", (short*)data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_scale() - Read scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) -{ - char str[5]; - int count; - - count = inv_sysfs_read((char*)names->scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%f", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_scale() - Read temperature scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_offset() - Read temperature offset. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_q16() - Get data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count; - short raw[3]; - float scale; - count = inv_read_raw(names, (long*)raw, timestamp); - count += inv_read_scale(names, &scale); - data[0] = (long)(raw[0] * (65536.f / scale)); - data[1] = (long)(raw[1] * (65536.f / scale)); - data[2] = (long)(raw[2] * (65536.f / scale)); - return count; -} - -/** - * inv_read_q16() - Get temperature data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes read or a negative error code. - */ -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count = 0; - short raw; - static short scale, offset; - static unsigned char first_read = 1; - - if (first_read) { - count += inv_read_temp_scale(names, &scale); - count += inv_read_temp_offset(names, &offset); - first_read = 0; - } - count += inv_read_temperature_raw(names, &raw, timestamp); - data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); - - return count; -} - -/** - * inv_write_fifo_rate() - Write fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) -{ - return inv_sysfs_write((char*)names->fifo_rate, (long)data); -} - -/** - * inv_write_buffer_enable() - Enable/disable buffer in /dev. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->enable, (long)data); -} - -/** - * inv_write_power_state() - Turn device on/off. - * @names: Names of sysfs files. - * @data: 1 to turn on. - * Returns number of bytes written or a negative error code. - */ -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->power_state, (long)data); -} - - - diff --git a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h deleted file mode 100644 index 45a35f9..0000000 --- a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#ifndef _INV_SYSFS_UTILS_H_ -#define _INV_SYSFS_UTILS_H_ - -/** - * struct inv_sysfs_names_s - Files needed by user applications. - * @buffer: Ring buffer attached to FIFO. - * @enable: Turns on HW-to-ring buffer flow. - * @raw_data: Raw data from registers. - * @temperature: Temperature data from register. - * @fifo_rate: FIFO rate/ODR. - * @power_state: Power state (this is a five-star comment). - * @fsr: Full-scale range. - * @lpf: Digital low pass filter. - * @scale: LSBs / dps (or LSBs / Gs). - * @temp_scale: LSBs / degrees C. - * @temp_offset: Offset in LSBs. - */ -struct inv_sysfs_names_s { - - //Sysfs for ITG3500 & MPU6050 - const char *buffer; - const char *enable; - const char *raw_data; //Raw Gyro data - const char *temperature; - const char *fifo_rate; - const char *power_state; - const char *fsr; - const char *lpf; - const char *scale; //Gyro scale - const char *temp_scale; - const char *temp_offset; - const char *self_test; - //Starting Sysfs available for MPU6050 only - const char *accel_en; - const char *accel_fifo_en; - const char *accel_fs; - const char *clock_source; - const char *early_suspend_en; - const char *firmware_loaded; - const char *gyro_en; - const char *gyro_fifo_en; - const char *key; - const char *raw_accel; - const char *reg_dump; - const char *tap_on; - const char *dmp_firmware; -}; - -/* File IO. Typically won't be called directly by user application, but they'll - * be here for your enjoyment. - */ -int inv_sysfs_write(char *filename, long data); -int inv_sysfs_read(char *filename, long num_bytes, char *data); - -/* Helper APIs to extract specific data. */ -int inv_read_buffer(int fd, long *data, long long *timestamp); -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp); -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); - -/* Scaled data. */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); - - -#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ - - diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c index f0f078c..91c766b 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c +++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -30,10 +30,10 @@ #define LOADDMP_LOG MPL_LOGI #define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) -#define DMP_CODE_SIZE 3060 +#define DMP_CODE_SIZE 3065 static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - /* bank # 0 */ + /* bank # 0 */ 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, @@ -212,32 +212,32 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, - 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x8b, 0x93, 0xf2, 0x02, 0x02, 0xd1, 0xab, 0xda, - 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, - 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, - 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 0xa3, 0xa3, - 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, - 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, - 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 0x2c, 0x87, - 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, + 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, + 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, + 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, + 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, + 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, + 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, + 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, /* bank # 11 */ - 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, - 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, - 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 0x80, 0xd9, - 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 0x2c, 0xd8, - 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, - 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 0x0c, 0xd8, - 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 0xff, 0xd8, - 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, - 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x87, 0x31, - 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82, - 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa6, - 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0x2a, - 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, - 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, - 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, - 0x00, 0xd8, 0xf1, 0xff + 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, + 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, + 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, + 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, + 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, + 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, + 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, + 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, + 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, + 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, + 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, + 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, + 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, + 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, + 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, + 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff }; #define DMP_VERSION (dmpMemory) @@ -250,11 +250,10 @@ inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) if (len <= 0) { MPL_LOGE("Nothing to write"); return INV_ERROR_FILE_WRITE; - } - else { + } else { MPL_LOGI("dmp firmware size to write = %d", len); } - if ( fd == NULL ) { + if (fd == NULL) { return INV_ERROR_FILE_OPEN; } bytesWritten = fwrite(dmp, 1, len, fd); @@ -262,8 +261,7 @@ inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", bytesWritten, len); result = INV_ERROR_FILE_WRITE; - } - else { + } else { MPL_LOGI("Bytes written = %d", bytesWritten); } return result; diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c index c5cf2e6..24b3217 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -38,26 +38,40 @@ #define STORECAL_LOG MPL_LOGI #define LOADCAL_LOG MPL_LOGI -inv_error_t inv_read_cal(unsigned char *cal, size_t len) +inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) { FILE *fp; - int bytesRead; inv_error_t result = INV_SUCCESS; + size_t fsize; fp = fopen(MLCAL_FILE,"rb"); if (fp == NULL) { MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); return INV_ERROR_FILE_OPEN; } - bytesRead = fread(cal, 1, len, fp); - if (bytesRead != len) { - MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", - bytesRead, len); + + // obtain file size + fseek (fp, 0 , SEEK_END); + fsize = ftell (fp); + rewind (fp); + + *calData = (unsigned char *)inv_malloc(fsize); + if (*calData==NULL) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", fsize); + fclose(fp); + return INV_ERROR_MEMORY_EXAUSTED; + } + + *bytesRead = fread(*calData, 1, fsize, fp); + if (*bytesRead != fsize) { + MPL_LOGE("bytes read (%d) don't match file size (%d)\n", + *bytesRead, fsize); result = INV_ERROR_FILE_READ; goto read_cal_end; } else { - MPL_LOGI("Bytes read = %d", bytesRead); + MPL_LOGI("Bytes read = %d", *bytesRead); } read_cal_end: @@ -261,31 +275,18 @@ inv_error_t inv_store_cal(unsigned char *calData, size_t length) */ inv_error_t inv_load_calibration(void) { - unsigned char *calData; + unsigned char *calData= NULL; inv_error_t result = 0; - size_t length; - - inv_get_mpl_state_size(&length); - if (length <= 0) { - MPL_LOGE("Could not get file calibration length - " - "error %d - aborting\n", result); - return result; - } + size_t bytesRead = 0; - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - - result = inv_read_cal(calData, length); + result = inv_read_cal(&calData, &bytesRead); if(result != INV_SUCCESS) { MPL_LOGE("Could not load cal file - " "aborting\n"); + goto free_mem_n_exit; } - result = inv_load_mpl_states(calData, length); + result = inv_load_mpl_states(calData, bytesRead); if (result != INV_SUCCESS) { MPL_LOGE("Could not load the calibration data - " "error %d - aborting\n", result); @@ -294,7 +295,7 @@ inv_error_t inv_load_calibration(void) free_mem_n_exit: inv_free(calData); - return INV_SUCCESS; + return result; } /** diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h index 336f081..115b34c 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h @@ -36,7 +36,7 @@ inv_error_t inv_store_calibration(void); /* Internal APIs */ -inv_error_t inv_read_cal(unsigned char *cal, size_t len); +inv_error_t inv_read_cal(unsigned char **, size_t *); inv_error_t inv_write_cal(unsigned char *cal, size_t len); inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c index 5636a02..a12a4ed 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -5,7 +5,6 @@ #include <ctype.h> #define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" -#define CHIP_NUM 4 enum PROC_SYSFS_CMD { CMD_GET_SYSFS_PATH, CMD_GET_DMP_PATH, @@ -15,7 +14,7 @@ enum PROC_SYSFS_CMD { CMD_GET_DEVICE_NODE }; static char sysfs_path[100]; -static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"}; +static char *chip_name[] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050", "MPU6500"}; static int chip_ind; static int initialized =0; static int status = 0; @@ -27,6 +26,8 @@ static int iio_dev_num = 0; #define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" #define FORMAT_TYPE_FILE "%s_type" +#define CHIP_NUM sizeof(chip_name)/sizeof(char*) + static const char *iio_dir = "/sys/bus/iio/devices/"; /** @@ -392,9 +393,9 @@ inv_error_t inv_get_input_number(const char *name, int *num) * name. It should be zeroed before calling this function. * Or it could have unpredicable result. */ -inv_error_t inv_get_iio_trigger_path(const char *name) +inv_error_t inv_get_iio_trigger_path(char *name) { - if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0) + if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) return INV_ERROR_NOT_OPENED; else return INV_SUCCESS; @@ -407,9 +408,9 @@ inv_error_t inv_get_iio_trigger_path(const char *name) * node. It should be zeroed before calling this function. * Or it could have unpredicable result. */ -inv_error_t inv_get_iio_device_node(const char *name) +inv_error_t inv_get_iio_device_node(char *name) { - if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0) + if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) return INV_ERROR_NOT_OPENED; else return INV_SUCCESS; diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h index eb285c5..9470874 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h @@ -27,8 +27,8 @@ inv_error_t inv_get_chip_name(char *name); inv_error_t inv_get_sysfs_key(unsigned char *key); inv_error_t inv_get_handler_number(const char *name, int *num); inv_error_t inv_get_input_number(const char *name, int *num); -inv_error_t inv_get_iio_trigger_path(const char *name); -inv_error_t inv_get_iio_device_node(const char *name); +inv_error_t inv_get_iio_trigger_path(char *name); +inv_error_t inv_get_iio_device_node(char *name); #ifdef __cplusplus } diff --git a/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c index 75f062e..6c9a6ca 100644 --- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c +++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -58,7 +58,9 @@ void *inv_malloc(unsigned int numBytes) inv_error_t inv_free(void *ptr) { // Deallocate space. - free(ptr); + if (ptr) { + free(ptr); + } return INV_SUCCESS; } diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c index 86c4b41..d1fd9c4 100644 --- a/libsensors_iio/software/core/mllite/ml_math_func.c +++ b/libsensors_iio/software/core/mllite/ml_math_func.c @@ -648,13 +648,60 @@ void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity } /** find a norm for a vector -* @param[in] a vector [3x1] -* @param[out] output the norm of the input vector +* @param[in] x a vector [3x1] +* @return the normalize vector. */ double inv_vector_norm(const float *x) { return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); } + +void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { + int i; + // initial state to zero + pFilter->state[0] = 0; + pFilter->state[1] = 0; + + // set up coefficients + for (i=0; i<5; i++) { + pFilter->c[i] = pBiquadCoeff[i]; + } +} + +void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) +{ + float divider; + pFilter->input = input; + pFilter->output = input; + pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); + pFilter->state[1] = pFilter->state[0]; +} + +float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { + float stateZero; + + pFilter->input = input; + // calculate the new state; + stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] + - pFilter->c[3]*pFilter->state[1]; + + pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] + + pFilter->c[1]*pFilter->state[1]; + + // update the output and state + pFilter->output = pFilter->output * pFilter->c[4]; + pFilter->state[1] = pFilter->state[0]; + pFilter->state[0] = stateZero; + return pFilter->output; +} + +void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { + + cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; +} + /** * @} */ diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h index 916de0a..535d849 100644 --- a/libsensors_iio/software/core/mllite/ml_math_func.h +++ b/libsensors_iio/software/core/mllite/ml_math_func.h @@ -24,6 +24,13 @@ extern "C" { #endif + typedef struct { + float state[4]; + float c[5]; + float input; + float output; + } inv_biquad_filter_t; + static inline float inv_q30_to_float(long q30) { return (float) q30 / ((float)(1L << 30)); @@ -102,6 +109,11 @@ extern "C" { double quaternion_to_rotation_angle(const long *quat); double inv_vector_norm(const float *x); + void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); + float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); + void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); + void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); + #ifdef __cplusplus } #endif diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c deleted file mode 100644 index 5eb4264..0000000 --- a/libsensors_iio/software/core/mllite/mlmath.c +++ /dev/null @@ -1,68 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/******************************************************************************* - * - * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#include <math.h> - -double ml_asin(double x) -{ - return asin(x); -} - -double ml_atan(double x) -{ - return atan(x); -} - -double ml_atan2(double x, double y) -{ - return atan2(x, y); -} - -double ml_log(double x) -{ - return log(x); -} - -double ml_sqrt(double x) -{ - return sqrt(x); -} - -double ml_ceil(double x) -{ - return ceil(x); -} - -double ml_floor(double x) -{ - return floor(x); -} - -double ml_cos(double x) -{ - return cos(x); -} - -double ml_sin(double x) -{ - return sin(x); -} - -double ml_acos(double x) -{ - return acos(x); -} - -double ml_pow(double x, double y) -{ - return pow(x, y); -} diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c index 231cbfd..a8c253d 100644 --- a/libsensors_iio/software/core/mllite/mpl.c +++ b/libsensors_iio/software/core/mllite/mpl.c @@ -41,7 +41,7 @@ inv_error_t inv_init_mpl(void) return INV_SUCCESS;
}
-const char ml_ver[] = "InvenSense MPL 5.0.1";
+const char ml_ver[] = "InvenSense MPL 5.1.1 RC6";
/**
* @brief used to get the MPL version.
diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c index 97ffdec..1484f9b 100644 --- a/libsensors_iio/software/core/mllite/results_holder.c +++ b/libsensors_iio/software/core/mllite/results_holder.c @@ -14,12 +14,12 @@ * @brief Results Holder for HAL. */ #include "results_holder.h" -#include "log.h" #include "ml_math_func.h" #include "mlmath.h" #include "start_manager.h" #include "data_builder.h" #include "message_layer.h" +#include "log.h" // These 2 status bits are used to control when the 9 axis quaternion is updated #define INV_COMPASS_CORRECTION_SET 1 diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c index 4b92bfc..721f858 100644 --- a/libsensors_iio/software/core/mllite/storage_manager.c +++ b/libsensors_iio/software/core/mllite/storage_manager.c @@ -4,6 +4,7 @@ See included License.txt for License information. $ */ + /** * @defgroup Storage_Manager storage_manager * @brief Motion Library - Stores Data for functions. diff --git a/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h deleted file mode 100644 index 3f8595f..0000000 --- a/libsensors_iio/software/core/mpl/adv_func.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_ADVFUNC_H__ -#define MLDMP_ADVFUNC_H__ -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - float inv_compass_angle(const long *compass, const long *grav, const float *quat); - inv_error_t inv_set_dmp_quaternion(long *q); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_ADVFUNC_H__ diff --git a/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h deleted file mode 100644 index 3dd9827..0000000 --- a/libsensors_iio/software/core/mpl/auto_calibration.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__ -#define MLDMP_IN_USE_AUTO_CALIBRATION_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_enable_in_use_auto_calibration(void); -inv_error_t inv_disable_in_use_auto_calibration(void); -inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled); -inv_error_t inv_init_in_use_auto_calibration(void); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__ - diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differindex 116b618..fbd346f 100644 --- a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so +++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so diff --git a/libsensors_iio/software/core/mpl/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk deleted file mode 100644 index 6ad45de..0000000 --- a/libsensors_iio/software/core/mpl/build/android/static.mk +++ /dev/null @@ -1,110 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -OBJFOLDER = $(CURDIR)/obj - -CROSS = arm-none-linux-gnueabi- -COMP= $(CROSS)gcc -LINK= $(CROSS)ar cr - -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform - -include $(MLSDK_ROOT)/Android-common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -fpic -CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -DLINUX -DANDROID -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(MLPLATFORM_DIR)/include -CFLAGS += -I$(MLSDK_ROOT)/mlutils -CFLAGS += -I$(MLSDK_ROOT)/mlapps/common -CFLAGS += $(MLSDK_INCLUDES) -CFLAGS += $(MLSDK_DEFINES) - -VPATH += $(MLLITE_DIR) -VPATH += $(MLSDK_ROOT)/mlutils -VPATH += $(MLLITE_DIR)/accel -VPATH += $(MLLITE_DIR)/compass -VPATH += $(MLLITE_DIR)/pressure -VPATH += $(MLLITE_DIR)/mlapps/common - -#################################################################################################### -## sources - -ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) - -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c -ML_SOURCES += $(MLLITE_DIR)/accel.c -ML_SOURCES += $(MLLITE_DIR)/compass.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c -ML_SOURCES += $(MLLITE_DIR)/key0_96.c -ML_SOURCES += $(MLLITE_DIR)/pressure.c -ML_SOURCES += $(MLLITE_DIR)/ml.c -ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c -ML_SOURCES += $(MLLITE_DIR)/ml_init.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c -ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c -ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c -ML_SOURCES += $(MLLITE_DIR)/mldl.c -ML_SOURCES += $(MLLITE_DIR)/mldmp.c -ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c -ML_SOURCES += $(MLLITE_DIR)/mlstates.c -ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c -ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c -ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c -ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c -ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c -ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c -ifeq ($(MPU_NAME),MPU3050) -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c -else -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c -endif - -ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) -ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all clean cleanall - -all: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n") - $(LINK) $(LIBRARY) $(ML_OBJS_DST) - $(CROSS)ranlib $(LIBRARY) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h index 616694a..1ba1ebb 100644 --- a/libsensors_iio/software/core/mpl/fusion_9axis.h +++ b/libsensors_iio/software/core/mpl/fusion_9axis.h @@ -26,6 +26,8 @@ extern "C" { inv_error_t inv_disable_9x_sensor_fusion(void); inv_error_t inv_start_9x_sensor_fusion(void); inv_error_t inv_stop_9x_sensor_fusion(void); + inv_error_t inv_9x_fusion_set_mag_fb(double fb);
+ inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
#ifdef __cplusplus } diff --git a/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h deleted file mode 100644 index 5eb571d..0000000 --- a/libsensors_iio/software/core/mpl/interpolator.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef INTERPOLATOR_H -#define INTERPOLATOR_H - -#ifdef __cplusplus -extern "C" { -#endif - -//#include "mltypes.h" - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -#define MAX_INTERPOLATION (20) - -typedef struct { - long x[2]; - long y[4]; -} tInterpolate2; -typedef struct { - long x[2]; - long y[6]; -} tInterpolate3; -typedef struct { - long x[5]; - long y[10]; - int idx1; -} tInterpolate5; -typedef struct { - tInterpolate2 state1; - tInterpolate2 state2; -} tInterpolate4; -typedef struct { - tInterpolate3 state1; - tInterpolate2 state2; -} tInterpolate6; -typedef struct { - tInterpolate2 state1; - tInterpolate4 state2; -} tInterpolate8; -typedef struct { - tInterpolate3 state1; - tInterpolate3 state2; -} tInterpolate9; -typedef struct { - tInterpolate5 state1; - tInterpolate2 state2; -} tInterpolate10; -typedef struct { - tInterpolate4 state1; - tInterpolate3 state2; -} tInterpolate12; -typedef struct { - tInterpolate5 state1; - tInterpolate3 state2; -} tInterpolate15; -typedef struct { - tInterpolate2 state1; - tInterpolate8 state2; -} tInterpolate16; -typedef struct { - tInterpolate9 state1; - tInterpolate2 state2; -} tInterpolate18; -typedef struct { - tInterpolate10 state1; - tInterpolate2 state2; -} tInterpolate20; - -typedef union { - tInterpolate2 u2; - tInterpolate3 u3; - tInterpolate4 u4; - tInterpolate5 u5; - tInterpolate6 u6; - tInterpolate8 u8; - tInterpolate9 u9; - tInterpolate10 u10; - tInterpolate12 u12; - tInterpolate15 u15; - tInterpolate16 u16; - tInterpolate18 u18; - tInterpolate20 u20; -} tInterpolateState; - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ -int inv_get_interp_amount( int x ); -int inv_interpolate( int amount, long input, long *output, tInterpolateState *state ); -long inv_fxmult( long x, long y ); - - -#ifdef __cplusplus -} -#endif - -#endif /* INTERPOLATOR_H */ diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h deleted file mode 100644 index 972844b..0000000 --- a/libsensors_iio/software/core/mpl/inv_log.h +++ /dev/null @@ -1,7 +0,0 @@ -#include "mltypes.h" -#ifndef INV_INV_LOG_H__ -#define INV_INV_LOG_H__ - -#define INV_LOGE(s) - -#endif // INV_INV_LOG_H__ diff --git a/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h index 9e59c18..12932c9 100644 --- a/libsensors_iio/software/core/mpl/invensense_adv.h +++ b/libsensors_iio/software/core/mpl/invensense_adv.h @@ -28,3 +28,4 @@ #include "quaternion_supervisor.h" #include "mag_disturb.h" #include "quat_accuracy_monitor.h" +#include "shake.h" diff --git a/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h deleted file mode 100644 index a81dabb..0000000 --- a/libsensors_iio/software/core/mpl/mlsetinterrupts.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - $License: - Copyright (c) 2008 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef MLSETINTERRUPT_H -#define MLSETINTERRUPT_H - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - /* DEPRECATED - Scheduled for removal. Do not use */ - inv_error_t MLSetInterrupts(unsigned short interrupts); - -#ifdef __cplusplus -} -#endif - -#endif /* MLSETINTERRUPT_H */ diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h deleted file mode 100644 index 3779381..0000000 --- a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $ - * - *****************************************************************************/ - -#ifndef MLDMP_MLSUPERVISOR_H__ -#define MLDMP_MLSUPERVISOR_H__ - -#include "mltypes.h" - -//#include "temp_comp.h" - -struct inv_fusion_t { - int compassCount; - long quat[4]; -}; - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_enable_9x_fusion(void); -inv_error_t inv_disable_9x_fusion(void); - -inv_error_t inv_enable_9x_fusion_legacy(void); -inv_error_t inv_disable_9x_fusion_legacy(void); - -inv_error_t inv_enable_9x_fusion_new(void); -inv_error_t inv_disable_9x_fusion_new(void); - -inv_error_t inv_enable_9x_fusion_basic(void); -inv_error_t inv_disable_9x_fusion_basic(void); - -inv_error_t inv_enable_9x_fusion_external(void); -inv_error_t inv_disable_9x_fusion_external(void); - -inv_error_t inv_enable_maintain_heading(void); -inv_error_t inv_disable_maintain_heading(void); - -void inv_set_compass_state(long compassState, long accState, - unsigned long deltaTime, - int magDisturb, int gotBias, - int *new_state, - int *new_accuracy); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_MLSUPERVISOR_H__ diff --git a/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h deleted file mode 100644 index ab4e45e..0000000 --- a/libsensors_iio/software/core/mpl/orientation.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef MLDMP_ORIENTATION_H__ -#define MLDMP_ORIENTATION_H__ - -#include "mltypes.h" -/*******************************************************************************/ -/* Orientations */ -/*******************************************************************************/ - -#define INV_X_UP 0x01 -#define INV_X_DOWN 0x02 -#define INV_Y_UP 0x04 -#define INV_Y_DOWN 0x08 -#define INV_Z_UP 0x10 -#define INV_Z_DOWN 0x20 -#define INV_ORIENTATION_ALL 0x3F - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_orientation(void); - inv_error_t inv_disable_orientation(void); - inv_error_t inv_set_orientation(int orientation); - inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short)); - inv_error_t inv_get_orientation(int *orientation); - inv_error_t inv_get_orientation_state(int * state); - inv_error_t inv_set_orientation_interrupt(unsigned char on); - inv_error_t inv_set_orientation_thresh(float angle, - float hysteresis, - unsigned long time, - unsigned int axis); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_ORIENTATION_H__ diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h deleted file mode 100644 index 99333e3..0000000 --- a/libsensors_iio/software/core/mpl/progressive_no_motion.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -#ifndef MLDMP_PROG_NO_MOTION_H__ -#define MLDMP_PROG_NO_MOTION_H__ - -#include "mltypes.h" - -#define PROG_NO_MOTION 1 -#define PROG_MOTION 2 - -#ifdef __cplusplus -extern "C" { -#endif - -/* APIs */ -inv_error_t inv_enable_prog_no_motion(void); -inv_error_t inv_disable_prog_no_motion(void); - -/* internal use */ -int inv_get_prog_no_motion_enabled(void); -void inv_get_prog_no_motion_bias_changed(void); -int inv_get_prog_no_motion_state(void); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_PROG_NO_MOTION_H__ diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h index 2cf7a50..3c6a2c1 100644 --- a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h +++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -62,6 +62,7 @@ inv_error_t inv_stop_quat_accuracy_monitor(void); double get_compassNgravity(void);
double get_init_compassNgravity(void);
+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
#ifdef __cplusplus
}
diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h deleted file mode 100644 index 73eb363..0000000 --- a/libsensors_iio/software/core/mpl/sensor_moments.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_SENSOR_MOMENTS_H__ -#define MLDMP_SENSOR_MOMENTS_H__ - -#include "mltypes.h" - -enum moment_ord { - SECOND_ORD=0, - THIRD_ORD, - FOURTH_ORD, - MAX_ORD -}; - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_sm(void);
- inv_error_t inv_disable_sm(void);
- inv_error_t inv_sm_record_data(float sample, void *sensor);
- inv_error_t inv_sm_update_evt_act_state(int motion);
- void *inv_init_sm(enum moment_ord);
- float inv_sm_get_filtered_data(void *sensor); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_SENSOR_MOMENTS_H__ - diff --git a/libsensors_iio/software/core/mpl/shake.h b/libsensors_iio/software/core/mpl/shake.h new file mode 100644 index 0000000..67acb7b --- /dev/null +++ b/libsensors_iio/software/core/mpl/shake.h @@ -0,0 +1,94 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_SHAKE_H__
+#define INV_SHAKE_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+ #define STATE_ZERO 0
+ #define STATE_INIT_1 1
+ #define STATE_INIT_2 2
+ #define STATE_DETECT 3
+
+ struct t_shake_config_params {
+ long shake_time_min_ms;
+ long shake_time_max_ms;
+ long shake_time_min;
+ long shake_time_max;
+ unsigned char shake_time_set;
+ long shake_time_saved;
+ float shake_deriv_thr;
+ int zero_cross_thr;
+ float accel_delta_min;
+ float accel_delta_max;
+ unsigned char interp_enable;
+ };
+
+ struct t_shake_state_params {
+ unsigned char state;
+ float accel_peak_high;
+ float accel_peak_low;
+ float accel_range;
+ int num_zero_cross;
+ short curr_shake_time;
+ int deriv_major_change;
+ int deriv_major_sign;
+ float accel_buffer[200];
+ float delta_buffer[200];
+ };
+
+ struct t_shake_data_params {
+ float accel_prev;
+ float accel_curr;
+ float delta_prev;
+ float delta_curr;
+ float delta_prev_buffer;
+ };
+
+ struct t_shake_results {
+ //unsigned char shake_int;
+ int shake_number;
+ };
+
+ struct t_shake_cb {
+ void (*shake_callback)(struct t_shake_results *shake_results);
+ };
+
+
+ /* --------------------- */
+ /* - Function p-types. - */
+ /* --------------------- */
+ inv_error_t inv_enable_shake(void);
+ inv_error_t inv_disable_shake(void);
+ inv_error_t inv_init_shake(void);
+ inv_error_t inv_start_shake(void);
+ int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
+ void inv_config_shake_time_params(long sample_time_ms);
+ void inv_set_shake_accel_delta_min(float accel_g);
+ void inv_set_shake_accel_delta_max(float accel_g);
+ void inv_set_shake_zero_cross_thresh(int num_zero_cross);
+ void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
+ void inv_set_shake_time_min_ms(long time_ms);
+ void inv_set_shake_time_max_ms(long time_ms);
+ void inv_enable_shake_data_interpolation(unsigned char en);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_SHAKE__
\ No newline at end of file diff --git a/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h deleted file mode 100644 index c1eb47b..0000000 --- a/libsensors_iio/software/core/mpl/state_storage.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INV_STATE_STORAGE_H__ -#define INV_STATE_STORAGE_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_store_data(const void *data, size_t size, unsigned long module, - unsigned long version); -inv_error_t inv_load_data(void *data, size_t size, unsigned long module, - unsigned long version); - -#ifdef __cplusplus -} -#endif - -#endif // INV_STATE_STORAGE_H__ |