17494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* 27494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $License: 37494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 47494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall See included License.txt for License information. 57494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $ 67494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 77494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 87494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 97494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @defgroup Data_Builder data_builder 107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Motion Library - Data Builder 117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Constructs and Creates the data for MPL 127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @{ 147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @file data_builder.c 157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Data Builder. 167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#undef MPL_LOG_NDEBUG 197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ 207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 218504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall#include <string.h> 228504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "ml_math_func.h" 247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "data_builder.h" 257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "mlmath.h" 267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "storage_manager.h" 277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "message_layer.h" 2833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "results_holder.h" 297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "log.h" 317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#undef MPL_LOG_TAG 327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define MPL_LOG_TAG "MPL" 337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralltypedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); 357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstruct process_t { 377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_process_cb_func func; 387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int priority; 397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int data_required; 407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall}; 417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstruct inv_db_save_t { 437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ 447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long compass_bias[3]; 457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ 467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long gyro_bias[3]; 477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /** Temperature when *gyro_bias was stored. */ 487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long gyro_temp; 497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ 507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long accel_bias[3]; 517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /** Temperature when accel bias was stored. */ 527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long accel_temp; 537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long gyro_temp_slope[3]; 5433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall /** Sensor Accuracy */ 5533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int gyro_accuracy; 5633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int accel_accuracy; 5733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int compass_accuracy; 587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall}; 597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstruct inv_data_builder_t { 617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int num_cb; 627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall struct process_t process[INV_MAX_DATA_CB]; 637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall struct inv_db_save_t save; 647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int compass_disturbance; 657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int debug_mode; 677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int last_mode; 687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall FILE *file; 697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall}; 717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic void inv_set_contiguous(void); 747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic struct inv_data_builder_t inv_data_builder; 767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic struct inv_sensor_cal_t sensors; 777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Change this key if the data being stored by this file changes */ 7933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#define INV_DB_SAVE_KEY 53395 807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Turn on data logging to allow playback of same scenario at a later time. 847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] file File to write to, must be open. 857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_turn_on_data_logging(FILE *file) 877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall MPL_LOGV("input data logging started\n"); 897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.file = file; 907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.debug_mode = RD_RECORD; 917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Turn off data logging to allow playback of same scenario at a later time. 947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* File passed to inv_turn_on_data_logging() must be closed after calling this. 957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_turn_off_data_logging() 977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall MPL_LOGV("input data logging stopped\n"); 997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.debug_mode = RD_NO_DEBUG; 1007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.file = NULL; 1017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This function receives the data that was stored in non-volatile memory between power off */ 1057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic inv_error_t inv_db_load_func(const unsigned char *data) 1067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); 10833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // copy in the saved accuracy in the actual sensors accuracy 10933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; 11033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; 11133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; 11233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // TODO 11333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (sensors.compass.accuracy == 3) { 11433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_set_compass_bias_found(1); 11533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 1167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 1177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This function returns the data to be stored in non-volatile memory between power off */ 1207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic inv_error_t inv_db_save_func(unsigned char *data) 1217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); 1237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 1247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Initialize the data builder 1277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_init_data_builder(void) 1297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* TODO: Hardcode temperature scale/offset here. */ 1317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memset(&inv_data_builder, 0, sizeof(inv_data_builder)); 1327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memset(&sensors, 0, sizeof(sensors)); 1337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return inv_register_load_store(inv_db_load_func, inv_db_save_func, 1347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sizeof(inv_data_builder.save), 1357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INV_DB_SAVE_KEY); 1367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Gyro sensitivity. 1397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return A scale factor to convert device units to degrees per second scaled by 2^16 1407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 1417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum rate * 2^15. 1427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_get_gyro_sensitivity() 1447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.gyro.sensitivity; 1467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Accel sensitivity. 1497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return A scale factor to convert device units to g's scaled by 2^16 1507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that g_s = device_units * sensitivity / 2^30. Typically 1517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum accel value in g's * 2^15. 1527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_get_accel_sensitivity(void) 1547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.accel.sensitivity; 1567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Compass sensitivity. 1597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return A scale factor to convert device units to micro Tesla scaled by 2^16 1607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that uT = device_units * sensitivity / 2^30. Typically 1617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum uT * 2^15. 1627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_get_compass_sensitivity(void) 1647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.compass.sensitivity; 1667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets orientation and sensitivity field for a sensor. 1697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] sensor Structure to apply settings to 1707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation Orientation description of how part is mounted. 1717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensitivity A Scale factor to convert from hardware units to 1727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* standard units (dps, uT, g). 1737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, 1757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int orientation, long sensitivity) 1767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensor->sensitivity = sensitivity; 1787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensor->orientation = orientation; 1797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 18033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 1817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the Orientation and Sensitivity of the gyro data. 1827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar defining the transformation from chip mounting 1837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to the body frame. The function inv_orientation_matrix_to_scalar() 1847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* can convert the transformation matrix to this scalar and describes the 1857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* scalar in further detail. 1867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 1877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 1887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum rate * 2^15. 1897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) 1917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 1937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 1947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_G_ORIENT; 1957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 1977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 1987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 2007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall set_sensor_orientation_and_scale(&sensors.gyro, orientation, 2017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensitivity); 2027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Gyro Sample rate in micro seconds. 2057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sample_rate_us Set Gyro Sample rate in us 2067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_gyro_sample_rate(long sample_rate_us) 2087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 2107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 2117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; 2127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 2137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 2147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 2167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.sample_rate_us = sample_rate_us; 2177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.sample_rate_ms = sample_rate_us / 1000; 2187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.gyro.bandwidth == 0) { 2197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); 2207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Accel Sample rate in micro seconds. 2247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sample_rate_us Set Accel Sample rate in us 2257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_accel_sample_rate(long sample_rate_us) 2277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 2297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 2307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; 2317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 2327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 2337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 2357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.sample_rate_us = sample_rate_us; 2367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.sample_rate_ms = sample_rate_us / 1000; 2377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.accel.bandwidth == 0) { 2387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); 2397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Compass Sample rate in micro seconds. 2437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. 2447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_compass_sample_rate(long sample_rate_us) 2467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 2487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 2497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; 2507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 2517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 2527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 2547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.sample_rate_us = sample_rate_us; 2557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.sample_rate_ms = sample_rate_us / 1000; 2567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.compass.bandwidth == 0) { 2577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); 2587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 26033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 26133ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 26233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 26333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *sample_rate_ms = sensors.gyro.sample_rate_ms; 26433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 26533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 26633ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_get_accel_sample_rate_ms(long *sample_rate_ms) 26733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 26833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *sample_rate_ms = sensors.accel.sample_rate_ms; 26933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 27033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 27133ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_get_compass_sample_rate_ms(long *sample_rate_ms) 27233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 27333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *sample_rate_ms = sensors.compass.sample_rate_ms; 27433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 27533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 2767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Quat Sample rate in micro seconds. 2777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sample_rate_us Set Quat Sample rate in us 2787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_quat_sample_rate(long sample_rate_us) 2807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 2827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 2837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; 2847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 2857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 2867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 2887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.sample_rate_us = sample_rate_us; 2897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.sample_rate_ms = sample_rate_us / 1000; 2907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Gyro Bandwidth in Hz 2937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bandwidth_hz Gyro bandwidth in Hz 2947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_gyro_bandwidth(int bandwidth_hz) 2967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.bandwidth = bandwidth_hz; 2987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Accel Bandwidth in Hz 3017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bandwidth_hz Gyro bandwidth in Hz 3027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_accel_bandwidth(int bandwidth_hz) 3047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.bandwidth = bandwidth_hz; 3067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set Compass Bandwidth in Hz 3097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bandwidth_hz Gyro bandwidth in Hz 3107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_compass_bandwidth(int bandwidth_hz) 3127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.bandwidth = bandwidth_hz; 3147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Helper function stating whether the compass is on or off. 3177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return TRUE if compass if on, 0 if compass if off 3187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_compass_on() 3207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; 3227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Helper function stating whether the gyro is on or off. 3257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return TRUE if gyro if on, 0 if gyro if off 3267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_gyro_on() 3287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; 3307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Helper function stating whether the acceleromter is on or off. 3337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return TRUE if accel if on, 0 if accel if off 3347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_accel_on() 3367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; 3387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Get last timestamp across all 3 sensors that are on. 3417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* This find out which timestamp has the largest value for sensors that are on. 3427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 3437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_time_t inv_get_last_timestamp() 3457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_time_t timestamp = 0; 3477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.accel.status & INV_SENSOR_ON) { 3487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall timestamp = sensors.accel.timestamp; 3497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.gyro.status & INV_SENSOR_ON) { 3517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp < sensors.gyro.timestamp) { 3527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall timestamp = sensors.gyro.timestamp; 3537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.compass.status & INV_SENSOR_ON) { 3567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp < sensors.compass.timestamp) { 3577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall timestamp = sensors.compass.timestamp; 3587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.temp.status & INV_SENSOR_ON) { 3617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp < sensors.temp.timestamp) 3627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall timestamp = sensors.temp.timestamp; 3637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return timestamp; 3657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the orientation and sensitivity of the gyro data. 3687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar defining the transformation from chip mounting 3697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to the body frame. The function inv_orientation_matrix_to_scalar() 3707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* can convert the transformation matrix to this scalar and describes the 3717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* scalar in further detail. 3727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensitivity A scale factor to convert device units to g's 3737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that g's = device_units * sensitivity / 2^30. Typically 3747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum g_value * 2^15. 3757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_accel_orientation_and_scale(int orientation, long sensitivity) 3777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 3797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 3807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_A_ORIENT; 3817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 3827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 3837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 3847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 3867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall set_sensor_orientation_and_scale(&sensors.accel, orientation, 3877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensitivity); 3887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the Orientation and Sensitivity of the gyro data. 3917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar defining the transformation from chip mounting 3927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to the body frame. The function inv_orientation_matrix_to_scalar() 3937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* can convert the transformation matrix to this scalar and describes the 3947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* scalar in further detail. 3957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensitivity A scale factor to convert device units to uT 3967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* such that uT = device_units * sensitivity / 2^30. Typically 3977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* it works out to be the maximum uT_value * 2^15. 3987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_compass_orientation_and_scale(int orientation, long sensitivity) 4007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 4027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 4037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_C_ORIENT; 4047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 4057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 4067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 4077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 4097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); 4107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_matrix_vector_mult(const long *A, const long *x, long *y) 4137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); 4157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); 4167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); 4177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Takes raw data stored in the sensor, removes bias, and converts it to 42033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* calibrated data in the body frame. Also store raw data for body frame. 4217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in,out] sensor structure to modify 4227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bias bias in the mounting frame, in hardware units scaled by 4237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* 2^16. Length 3. 4247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 4257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) 4267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long raw32[3]; 4287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Convert raw to calibrated 43033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[0] = (long)sensor->raw[0] << 15; 43133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[1] = (long)sensor->raw[1] << 15; 43233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[2] = (long)sensor->raw[2] << 15; 4337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4348504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); 4357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 43633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[0] -= bias[0] >> 1; 43733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[1] -= bias[1] >> 1; 43833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall raw32[2] -= bias[2] >> 1; 43933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 44033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); 4417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensor->status |= INV_CALIBRATED; 4437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Returns the current bias for the compass 4467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. 4477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* Length 3. 4487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 4497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_compass_bias(long *bias) 4507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias != NULL) { 4527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); 4537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_compass_bias(const long *bias, int accuracy) 4577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { 4597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); 4607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 4617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.accuracy = accuracy; 46333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.compass_accuracy = accuracy; 4647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); 4657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Set the state of a compass disturbance 4687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] dist 1=disturbance, 0=no disturbance 4697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 4707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_compass_disturbance(int dist) 4717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.compass_disturbance = dist; 4737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_compass_disturbance(void) { 4767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return inv_data_builder.compass_disturbance; 4777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the accel bias. 4797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 4807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 4817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 4827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_accel_bias(const long *bias, int accuracy) 4837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias) { 4857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { 4867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); 4877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 4887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.accuracy = accuracy; 49133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.accel_accuracy = accuracy; 4927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 4937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4958504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall/** Sets the accel accuracy. 4968504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 4978504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall*/ 4988504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrallvoid inv_set_accel_accuracy(int accuracy) 4998504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall{ 5008504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall sensors.accel.accuracy = accuracy; 5018504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall inv_data_builder.save.accel_accuracy = accuracy; 5028504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 5038504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall} 5048504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 5057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the accel bias with control over which axis. 5067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 5077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 5087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] mask Mask to select axis to apply bias set. 5097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 5107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) 5117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias) { 5137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (mask & 1){ 5147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.save.accel_bias[0] = bias[0]; 5157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (mask & 2){ 5177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.save.accel_bias[1] = bias[1]; 5187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (mask & 4){ 5207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.save.accel_bias[2] = bias[2]; 5217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 5247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.accuracy = accuracy; 52633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.accel_accuracy = accuracy; 5278504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 5287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Sets the gyro bias 5327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. 5337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* Length 3. 5347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. 5357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 5367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_set_gyro_bias(const long *bias, int accuracy) 5377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias != NULL) { 5397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { 5407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); 5417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); 5427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.accuracy = accuracy; 54533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.gyro_accuracy = accuracy; 54633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 5477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* TODO: What should we do if there's no temperature data? */ 5487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.temp.calibrated[0]) 5497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; 5507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else 5517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* Set to 27 deg C for now until we've got a better solution. */ 5527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.save.gyro_temp = 1769472L; 5537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); 5547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* TODO: Add this information to inv_sensor_cal_t */ 5577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 5587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Get the gyro biases and temperature record from MPL 5597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] bias 5607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Gyro bias in hardware units scaled by 2^16. 5617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * In chip mounting frame. 5627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Length 3. 5637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] temp 5647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Tempearature in degrees C. 5657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 5667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_gyro_bias(long *bias, long *temp) 5677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias != NULL) 5697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(bias, inv_data_builder.save.gyro_bias, 5707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sizeof(inv_data_builder.save.gyro_bias)); 5717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (temp != NULL) 5727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp[0] = inv_data_builder.save.gyro_temp; 5737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Get Accel Bias 5767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] bias Accel bias where 5777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] temp Temperature where 1 C = 2^16 5787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 5797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_accel_bias(long *bias, long *temp) 5807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (bias != NULL) 5827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(bias, inv_data_builder.save.accel_bias, 5837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sizeof(inv_data_builder.save.accel_bias)); 5847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (temp != NULL) 5857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp[0] = inv_data_builder.save.accel_temp; 5867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 5897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Record new accel data for use when inv_execute_on_data() is called 5907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] accel accel data. 5917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Length 3. 5927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Calibrated data is in m/s^2 scaled by 2^16 in body frame. 5937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Raw data is in device units in chip mounting frame. 5947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] status 5957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 5967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * being most accurate. 5977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * The upper bit INV_CALIBRATED, is set if the data was calibrated 5987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * outside MPL and it is not set if the data being passed is raw. 5997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Raw data should be in device units, typically in a 16-bit range. 6007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] timestamp 6017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Monotonic time stamp, for Android it's in nanoseconds. 6027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Returns INV_SUCCESS if successful or an error code if not. 6037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 6047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 6057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 6077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 6087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_ACCEL; 6097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 6107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); 6117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 6127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 6137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 6147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if ((status & INV_CALIBRATED) == 0) { 6167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.raw[0] = (short)accel[0]; 6177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.raw[1] = (short)accel[1]; 6187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.raw[2] = (short)accel[2]; 6197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status |= INV_RAW_DATA; 6207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 6217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 6227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.calibrated[0] = accel[0]; 6237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.calibrated[1] = accel[1]; 6247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.calibrated[2] = accel[2]; 6257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status |= INV_CALIBRATED; 6267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.accuracy = status & 3; 62733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.accel_accuracy = status & 3; 6287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 6297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; 6307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.timestamp_prev = sensors.accel.timestamp; 6317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.timestamp = timestamp; 6327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 6347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Record new gyro data and calls inv_execute_on_data() if previous 6377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* sample has not been processed. 6387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] gyro Data is in device units. Length 3. 6397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 6407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] executed Set to 1 if data processing was done. 6417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 6427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) 6447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 6467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 6477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_GYRO; 6487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 6497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); 6507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 6517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 6527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 6537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); 6557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 6567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.timestamp_prev = sensors.gyro.timestamp; 6577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.timestamp = timestamp; 6587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); 6597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 6617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Record new compass data for use when inv_execute_on_data() is called 6647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. 6657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* Length 3. 6667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. 6677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is 6687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* not set if the data being passed is raw. Raw data should be in device units, typically 6697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* in a 16-bit range. 6707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 6717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] executed Set to 1 if data processing was done. 6727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 6737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_build_compass(const long *compass, int status, 6757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_time_t timestamp) 6767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 6787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 6797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_COMPASS; 6807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 6817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); 6827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 6837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 6847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 6857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if ((status & INV_CALIBRATED) == 0) { 6877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.raw[0] = (short)compass[0]; 6887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.raw[1] = (short)compass[1]; 6897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.raw[2] = (short)compass[2]; 6907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 6917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status |= INV_RAW_DATA; 6927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 6937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.calibrated[0] = compass[0]; 6947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.calibrated[1] = compass[1]; 6957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.calibrated[2] = compass[2]; 6967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status |= INV_CALIBRATED; 6977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.accuracy = status & 3; 69833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_data_builder.save.compass_accuracy = status & 3; 6997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 7007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.timestamp_prev = sensors.compass.timestamp; 7017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.timestamp = timestamp; 7027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; 7037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 7057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Record new temperature data for use when inv_execute_on_data() is called. 7087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] temp Temperature data in q16 format. 7097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] timestamp Monotonic time stamp; for Android it's in 7107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * nanoseconds. 7117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 7127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 7137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_build_temp(const long temp, inv_time_t timestamp) 7147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 7167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 7177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_TEMPERATURE; 7187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 7197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); 7207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 7217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 7227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 7237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.calibrated[0] = temp; 7247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 7257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.timestamp_prev = sensors.temp.timestamp; 7267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.timestamp = timestamp; 7277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* TODO: Apply scale, remove offset. */ 7287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 7307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** quaternion data 7327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 7337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* Real part first. Length 4. 7347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] status number of axis, 16-bit or 32-bit 7357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] timestamp 7367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] timestamp Monotonic time stamp; for Android it's in 7377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* nanoseconds. 7387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] executed Set to 1 if data processing was done. 7397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 7407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 7417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) 7427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 7447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 7457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_QUAT; 7467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 7477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); 7487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 7497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 7507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 7517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); 7537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.timestamp = timestamp; 7547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 7557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.status |= (INV_BIAS_APPLIED & status); 7567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 7587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This should be called when the accel has been turned off. This is so 7617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* that we will know if the data is contiguous. 7627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 7637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_accel_was_turned_off() 7647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status = 0; 7667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This should be called when the compass has been turned off. This is so 7697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* that we will know if the data is contiguous. 7707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 7717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_compass_was_turned_off() 7727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status = 0; 7747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This should be called when the quaternion data from the DMP has been turned off. This is so 7777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* that we will know if the data is contiguous. 7787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 7797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_quaternion_sensor_was_turned_off(void) 7807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.status = 0; 7827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This should be called when the gyro has been turned off. This is so 7857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* that we will know if the data is contiguous. 7867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 7877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_gyro_was_turned_off() 7887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.status = 0; 7907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 7927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This should be called when the temperature sensor has been turned off. 7937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * This is so that we will know if the data is contiguous. 7947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 7957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_temperature_was_turned_off() 7967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 7977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.status = 0; 7987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 7997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Registers to receive a callback when there is new sensor data. 8017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @internal 8027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] func Function pointer to receive callback when there is new sensor data 8037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 8047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* numbers must be unique. 8057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 8067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 8077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* gyro data, INV_MAG_NEW = compass data. So passing in 8087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* INV_ACCEL_NEW | INV_MAG_NEW, a 8097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* callback would be generated if there was new magnetomer data OR new accel data. 8107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 8117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_register_data_cb( 8127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_error_t (*func)(struct inv_sensor_cal_t *data), 8137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int priority, int sensor_type) 8147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 8157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_error_t result = INV_SUCCESS; 8167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk, nn; 8177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Make sure we haven't registered this function already 8197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Or used the same priority 8207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 8217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if ((inv_data_builder.process[kk].func == func) || 8227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (inv_data_builder.process[kk].priority == priority)) { 8237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_INVALID_PARAMETER; //fixme give a warning 8247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Make sure we have not filled up our number of allowable callbacks 8287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { 8297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall kk = 0; 8307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.num_cb != 0) { 8317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // set kk to be where this new callback goes in the array 8327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall while ((kk < inv_data_builder.num_cb) && 8337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (inv_data_builder.process[kk].priority < priority)) { 8347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall kk++; 8357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (kk != inv_data_builder.num_cb) { 8377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // We need to move the others 8387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (nn = inv_data_builder.num_cb; nn > kk; --nn) { 8397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[nn] = 8407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[nn - 1]; 8417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Add new callback 8457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[kk].func = func; 8467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[kk].priority = priority; 8477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[kk].data_required = sensor_type; 8487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.num_cb++; 8497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 8507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall MPL_LOGE("Unable to add feature callback as too many were already registered\n"); 8517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = INV_ERROR_MEMORY_EXAUSTED; 8527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 8557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 8567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Unregisters the callback that happens when new sensor data is received. 8587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @internal 8597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] func Function pointer to receive callback when there is new sensor data 8607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 8617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* numbers must be unique. 8627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 8637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 8647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* gyro data, INV_MAG_NEW = compass data. So passing in 8657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* INV_ACCEL_NEW | INV_MAG_NEW, a 8667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* callback would be generated if there was new magnetomer data OR new accel data. 8677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 8687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_unregister_data_cb( 8697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_error_t (*func)(struct inv_sensor_cal_t *data)) 8707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 8717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk, nn; 8727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 8747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.process[kk].func == func) { 8757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Delete this callback 8767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { 8777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[nn - 1] = 8787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.process[nn]; 8797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_data_builder.num_cb--; 8817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 8827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 8847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; // We did not find the callback 8867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 8877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 8887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** After at least one of inv_build_gyro(), inv_build_accel(), or 8897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* inv_build_compass() has been called, this function should be called. 8907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* It will process the data it has received and update all the internal states 8917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* and features that have been turned on. 8927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 8937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 8947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_execute_on_data(void) 8957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 8967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_error_t result, first_error; 8977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk; 8987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int mode; 8997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef INV_PLAYBACK_DBG 9017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.debug_mode == RD_RECORD) { 9027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int type = PLAYBACK_DBG_TYPE_EXECUTE; 9037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall fwrite(&type, sizeof(type), 1, inv_data_builder.file); 9047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 9067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Determine what new data we have 9077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode = 0; 9087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.gyro.status & INV_NEW_DATA) 9097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode |= INV_GYRO_NEW; 9107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.accel.status & INV_NEW_DATA) 9117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode |= INV_ACCEL_NEW; 9127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.compass.status & INV_NEW_DATA) 9137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode |= INV_MAG_NEW; 9147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.temp.status & INV_NEW_DATA) 9157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode |= INV_TEMP_NEW; 9167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.quat.status & INV_QUAT_NEW) 9177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mode |= INV_QUAT_NEW; 9187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall first_error = INV_SUCCESS; 9207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 9227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (mode & inv_data_builder.process[kk].data_required) { 9237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = inv_data_builder.process[kk].func(&sensors); 9247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (result && !first_error) { 9257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall first_error = result; 9267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_set_contiguous(); 9317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return first_error; 9337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 9347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. 9367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* 9377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 9387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic void inv_set_contiguous(void) 9397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 9407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_time_t current_time = 0; 9417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.gyro.status & INV_NEW_DATA) { 9427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.status |= INV_CONTIGUOUS; 9437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall current_time = sensors.gyro.timestamp; 9447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.accel.status & INV_NEW_DATA) { 9467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status |= INV_CONTIGUOUS; 9477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall current_time = MAX(current_time, sensors.accel.timestamp); 9487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.compass.status & INV_NEW_DATA) { 9507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status |= INV_CONTIGUOUS; 9517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall current_time = MAX(current_time, sensors.compass.timestamp); 9527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.temp.status & INV_NEW_DATA) { 9547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.status |= INV_CONTIGUOUS; 9557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall current_time = MAX(current_time, sensors.temp.timestamp); 9567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sensors.quat.status & INV_NEW_DATA) { 9587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.status |= INV_CONTIGUOUS; 9597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall current_time = MAX(current_time, sensors.quat.timestamp); 9607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#if 0 9637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() 9647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * type functions. This is just in case that breaks down. We make sure 9657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * all the data is within 2 seconds of the newest piece of data*/ 9667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) 9677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_gyro_was_turned_off(); 9687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) 9697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_accel_was_turned_off(); 9707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) 9717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_compass_was_turned_off(); 9727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* TODO: Temperature might not need to be read this quickly. */ 9737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) 9747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_temperature_was_turned_off(); 9757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 9767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* clear bits */ 9787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.gyro.status &= ~INV_NEW_DATA; 9797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.accel.status &= ~INV_NEW_DATA; 9807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.compass.status &= ~INV_NEW_DATA; 9817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.temp.status &= ~INV_NEW_DATA; 9827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sensors.quat.status &= ~INV_NEW_DATA; 9837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 9847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 9857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Gets a whole set of accel data including data, accuracy and timestamp. 9867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] data Accel Data where 1g = 2^16 9877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 9887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] timestamp The timestamp of the data sample. 9897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 9907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 9917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 9927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (data != NULL) { 9937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); 9947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp != NULL) { 9967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *timestamp = sensors.accel.timestamp; 9977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 9987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (accuracy != NULL) { 9997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *accuracy = sensors.accel.accuracy; 10007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 10017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Gets a whole set of gyro data including data, accuracy and timestamp. 10047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] data Gyro Data where 1 dps = 2^16 10057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 10067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] timestamp The timestamp of the data sample. 10077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 10097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 10117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp != NULL) { 10127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *timestamp = sensors.gyro.timestamp; 10137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 10147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (accuracy != NULL) { 10157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *accuracy = sensors.gyro.accuracy; 10167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 10177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 101933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Gets a whole set of gyro raw data including data, accuracy and timestamp. 102033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @param[out] data Gyro Data where 1 dps = 2^16 102133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 102233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @param[out] timestamp The timestamp of the data sample. 102333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 102433ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 102533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 10268504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); 102733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (timestamp != NULL) { 102833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *timestamp = sensors.gyro.timestamp; 102933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 103033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (accuracy != NULL) { 103133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *accuracy = sensors.gyro.accuracy; 103233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 103333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 103433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 10357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Get's latest gyro data. 10367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. 10377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_gyro(long *gyro) 10397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 10417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Gets a whole set of compass data including data, accuracy and timestamp. 10447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] data Compass Data where 1 uT = 2^16 10457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 10467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] timestamp The timestamp of the data sample. 10477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 10497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); 10517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp != NULL) { 10527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *timestamp = sensors.compass.timestamp; 10537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 10547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (accuracy != NULL) { 10557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.compass_disturbance) 10567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *accuracy = 0; 10577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else 10587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *accuracy = sensors.compass.accuracy; 10597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 10607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Gets a whole set of temperature data including data, accuracy and timestamp. 10637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] data Temperature data where 1 degree C = 2^16 10647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] accuracy 0 to 3, where 3 is most accurate. 10657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] timestamp The timestamp of the data sample. 10667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 10677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) 10687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall data[0] = sensors.temp.calibrated[0]; 10707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (timestamp) 10717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *timestamp = sensors.temp.timestamp; 10727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (accuracy) 10737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *accuracy = sensors.temp.accuracy; 10747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Returns accuracy of gyro. 10777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. 10787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_gyro_accuracy(void) 10807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.gyro.accuracy; 10827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Returns accuracy of compass. 10857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. 10867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_mag_accuracy(void) 10887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (inv_data_builder.compass_disturbance) 10907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return 0; 10917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.compass.accuracy; 10927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 10937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 10947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Returns accuracy of accel. 10957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. 10967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 10977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallint inv_get_accel_accuracy(void) 10987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 10997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sensors.accel.accuracy; 11007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 11017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 11027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_get_gyro_orient(int *orient) 11037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 11047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *orient = sensors.gyro.orientation; 11057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return 0; 11067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 11077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 11087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_get_accel_orient(int *orient) 11097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 11107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *orient = sensors.accel.orientation; 11117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return 0; 11127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 11137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 11147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 11157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 11167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @} 11177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 1118