diff options
Diffstat (limited to 'libsensors_iio/software/core/mllite/linux/ml_stored_data.c')
-rw-r--r-- | libsensors_iio/software/core/mllite/linux/ml_stored_data.c | 353 |
1 files changed, 353 insertions, 0 deletions
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c new file mode 100644 index 0000000..c5cf2e6 --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -0,0 +1,353 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_STORED_DATA + * + * @{ + * @file ml_stored_data.c + * @brief functions for reading and writing stored data sets. + * Typically, these functions process stored calibration data. + */ + +#include <stdio.h> + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + + +#include "ml_stored_data.h" +#include "storage_manager.h" +#include "log.h" +#include "mlos.h" + +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define DEFAULT_KEY 29681 + +#define STORECAL_LOG MPL_LOGI +#define LOADCAL_LOG MPL_LOGI + +inv_error_t inv_read_cal(unsigned char *cal, size_t len) +{ + FILE *fp; + int bytesRead; + inv_error_t result = INV_SUCCESS; + + 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); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + else { + MPL_LOGI("Bytes read = %d", bytesRead); + } + +read_cal_end: + fclose(fp); + return result; +} + +inv_error_t inv_write_cal(unsigned char *cal, size_t len) +{ + FILE *fp; + int bytesWritten; + inv_error_t result = INV_SUCCESS; + + if (len <= 0) { + MPL_LOGE("Nothing to write"); + return INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("cal data size to write = %d", len); + } + fp = fopen(MLCAL_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(cal, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("Bytes written = %d", bytesWritten); + } + fclose(fp); + return result; +} + +/** + * @brief Loads a type 0 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * This calibration data is produced internally by the MPL and its + * size is 2777 bytes (header and checksum included). + * Calibration format type 1 is currently used for ITG3500 + * + * @pre inv_init_storage_manager() + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) +{ + inv_error_t result; + + LOADCAL_LOG("Entering inv_load_cal_V0\n"); + + /*if (len != expLen) { + MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + }*/ + + result = inv_load_mpl_states(calData, len); + return result; +} + +/** + * @brief Loads a type 1 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 36 bytes (header and checksum included). + * Calibration format type 1 is produced by the MPU Self Test and + * substitutes the type 0 : inv_load_cal_V0(). + * + * @pre + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) +{ + return INV_SUCCESS; +} + +/** + * @brief Loads a set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * + * @pre + * + * + * @param calData + * A pointer to an array of bytes to be parsed. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal(unsigned char *calData) +{ + int calType = 0; + int len = 0; + //int ptr; + //uint32_t chk = 0; + //uint32_t cmp_chk = 0; + + /*load_func_t loaders[] = { + inv_load_cal_V0, + inv_load_cal_V1, + }; + */ + + inv_load_cal_V0(calData, len); + + /* read the header (type and len) + len is the total record length including header and checksum */ + len = 0; + len += 16777216L * ((int)calData[0]); + len += 65536L * ((int)calData[1]); + len += 256 * ((int)calData[2]); + len += (int)calData[3]; + + calType = ((int)calData[4]) * 256 + ((int)calData[5]); + if (calType > 5) { + MPL_LOGE("Unsupported calibration file format %d. " + "Valid types 0..5\n", calType); + return INV_ERROR_INVALID_PARAMETER; + } + + /* call the proper method to read in the data */ + //return loaders[calType] (calData, len); + return 0; +} + +/** + * @brief Stores a set of calibration data. + * It generates a binary data set containing calibration data. + * The binary data set is intended to be stored into a file. + * + * @pre inv_dmp_open() + * + * @param calData + * A pointer to an array of bytes to be stored. + * @param length + * The amount of bytes available in the array. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_store_cal(unsigned char *calData, size_t length) +{ + inv_error_t res = 0; + size_t size; + + STORECAL_LOG("Entering inv_store_cal\n"); + + inv_get_mpl_state_size(&size); + + MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); + + /* store data */ + res = inv_save_mpl_states(calData, size); + if(res != 0) + { + MPL_LOGE("inv_save_mpl_states() failed"); + } + + STORECAL_LOG("Exiting inv_store_cal\n"); + return INV_SUCCESS; +} + +/** + * @brief Load a calibration file. + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_load_calibration(void) +{ + unsigned char *calData; + 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; + } + + 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); + if(result != INV_SUCCESS) { + MPL_LOGE("Could not load cal file - " + "aborting\n"); + } + + result = inv_load_mpl_states(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + size_t length; + + result = inv_get_mpl_state_size(&length); + 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; + } + else { + MPL_LOGI("mpl state size = %d", length); + } + + result = inv_save_mpl_states(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not save mpl states - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + else { + MPL_LOGE("calData from inv_save_mpl_states, size=%d", + strlen((char *)calData)); + } + + result = inv_write_cal(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not store calibrated data on file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @} + */ |