149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $License: 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow See included License.txt for License information. 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $ 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @defgroup Data_Builder data_builder 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Motion Library - Data Builder 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Constructs and Creates the data for MPL 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @{ 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @file data_builder.c 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Data Builder. 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#undef MPL_LOG_NDEBUG 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */ 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_math_func.h" 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "data_builder.h" 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlmath.h" 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "storage_manager.h" 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "message_layer.h" 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "results_holder.h" 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "log.h" 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#undef MPL_LOG_TAG 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MPL_LOG_TAG "MLLITE" 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowtypedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct process_t { 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_process_cb_func func; 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int priority; 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int data_required; 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct inv_data_builder_t { 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int num_cb; 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct process_t process[INV_MAX_DATA_CB]; 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct inv_db_save_t save; 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct inv_db_save_mpl_t save_mpl; 47cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro struct inv_db_save_accel_mpl_t save_accel_mpl; 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int compass_disturbance; 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int debug_mode; 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int last_mode; 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *file; 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void inv_set_contiguous(void); 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct inv_data_builder_t inv_data_builder; 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct inv_sensor_cal_t sensors; 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turn on data logging to allow playback of same scenario at a later time. 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] file File to write to, must be open. 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_turn_on_data_logging(FILE *file) 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("input data logging started\n"); 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.file = file; 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.debug_mode = RD_RECORD; 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turn off data logging to allow playback of same scenario at a later time. 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* File passed to inv_turn_on_data_logging() must be closed after calling this. 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_turn_off_data_logging() 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("input data logging stopped\n"); 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.debug_mode = RD_NO_DEBUG; 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.file = NULL; 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets last value of raw compass data. 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] raw Raw compass data in mounting frame in hardware units. Length 3. 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_raw_compass(short *raw) 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw)); 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function receives the data that was stored in non-volatile memory between power off */ 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic inv_error_t inv_db_load_func(const unsigned char *data) 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // copy in the saved accuracy in the actual sensors accuracy 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // TODO 102cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (sensors.accel.accuracy == 3) { 103cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_accel_bias_found(1); 104cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.compass.accuracy == 3) { 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_bias_found(1); 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function returns the data to be stored in non-volatile memory between power off */ 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic inv_error_t inv_db_save_func(unsigned char *data) 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function receives the data for mpl that was stored in non-volatile memory between power off */ 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic inv_error_t inv_db_load_mpl_func(const unsigned char *data) 12049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(&inv_data_builder.save_mpl, data, sizeof(inv_data_builder.save_mpl)); 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function returns the data for mpl to be stored in non-volatile memory between power off */ 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic inv_error_t inv_db_save_mpl_func(unsigned char *data) 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, &inv_data_builder.save_mpl, sizeof(inv_data_builder.save_mpl)); 13049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 13149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 13249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 133cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/** This function receives the data for mpl that was stored in non-volatile memory between power off */ 134cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarostatic inv_error_t inv_db_load_accel_mpl_func(const unsigned char *data) 135cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 136cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(&inv_data_builder.save_accel_mpl, data, sizeof(inv_data_builder.save_accel_mpl)); 137cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 138cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return INV_SUCCESS; 139cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 140cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 141cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/** This function returns the data for mpl to be stored in non-volatile memory between power off */ 142cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarostatic inv_error_t inv_db_save_accel_mpl_func(unsigned char *data) 143cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 144cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(data, &inv_data_builder.save_accel_mpl, sizeof(inv_data_builder.save_accel_mpl)); 145cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return INV_SUCCESS; 146cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 147cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Initialize the data builder 14949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_init_data_builder(void) 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 15249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: Hardcode temperature scale/offset here. */ 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&inv_data_builder, 0, sizeof(inv_data_builder)); 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&sensors, 0, sizeof(sensors)); 15549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable the soft iron transform process 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_reset_compass_soft_iron_matrix(); 15849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ((inv_register_load_store(inv_db_load_func, inv_db_save_func, 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof(inv_data_builder.save), 16149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INV_DB_SAVE_KEY)) 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (inv_register_load_store(inv_db_load_mpl_func, inv_db_save_mpl_func, 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof(inv_data_builder.save_mpl), 164cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro INV_DB_SAVE_MPL_KEY)) 165cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (inv_register_load_store(inv_db_load_accel_mpl_func, inv_db_save_accel_mpl_func, 166cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sizeof(inv_data_builder.save_accel_mpl), 167cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro INV_DB_SAVE_ACCEL_MPL_KEY)) ); 16849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gyro sensitivity. 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return A scale factor to convert device units to degrees per second scaled by 2^16 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum rate * 2^15. 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_get_gyro_sensitivity(void) 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.gyro.sensitivity; 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Accel sensitivity. 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return A scale factor to convert device units to g's scaled by 2^16 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that g_s = device_units * sensitivity / 2^30. Typically 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum accel value in g's * 2^15. 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_get_accel_sensitivity(void) 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.accel.sensitivity; 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Compass sensitivity. 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return A scale factor to convert device units to micro Tesla scaled by 2^16 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that uT = device_units * sensitivity / 2^30. Typically 19349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum uT * 2^15. 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_get_compass_sensitivity(void) 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.compass.sensitivity; 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets orientation and sensitivity field for a sensor. 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] sensor Structure to apply settings to 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation Orientation description of how part is mounted. 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensitivity A Scale factor to convert from hardware units to 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* standard units (dps, uT, g). 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int orientation, long sensitivity) 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int error = 0; 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 21149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!sensitivity) { 21249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Sensitivity can't be zero 21349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensitivity = 1L<<16; 21449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGE("\n\nCritical error! Sensitivity is zero.\n\n"); 21549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 21649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 21749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensor->sensitivity = sensitivity; 21849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Make sure we don't describe some impossible orientation 21949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((orientation & 3) == 3) { 22049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow error = 1; 22149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 22249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((orientation & 0x18) == 0x18) { 22349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow error = 1; 22449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 22549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((orientation & 0xc0) == 0xc0) { 22649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow error = 1; 22749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 22849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (error) { 22949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orientation = 0x88; // Identity 23049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGE("\n\nCritical error! Impossible mounting orientation given. Using Identity instead\n\n"); 23149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 23249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensor->orientation = orientation; 23349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 23449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the Orientation and Sensitivity of the gyro data. 23649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar defining the transformation from chip mounting 23749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to the body frame. The function inv_orientation_matrix_to_scalar() 23849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* can convert the transformation matrix to this scalar and describes the 23949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* scalar in further detail. 24049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 24149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 24249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum rate * 2^15. 24349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 24449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) 24549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 24649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 24749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 24849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_G_ORIENT; 24949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 25049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 25149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 25249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 25349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 25449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow set_sensor_orientation_and_scale(&sensors.gyro, orientation, 25549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensitivity); 25649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 25749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 25849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Gyro Sample rate in micro seconds. 25949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sample_rate_us Set Gyro Sample rate in us 26049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 26149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_gyro_sample_rate(long sample_rate_us) 26249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 26349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 26449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 26549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; 26649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 26749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 26849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 26949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 27049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.sample_rate_us = sample_rate_us; 27149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.sample_rate_ms = sample_rate_us / 1000; 27249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.gyro.bandwidth == 0) { 27349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); 27449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 27549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 27649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 27749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Accel Sample rate in micro seconds. 27849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sample_rate_us Set Accel Sample rate in us 27949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 28049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_sample_rate(long sample_rate_us) 28149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 28249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 28349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 28449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; 28549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 28649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 28749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 28849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 28949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.sample_rate_us = sample_rate_us; 29049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.sample_rate_ms = sample_rate_us / 1000; 29149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.accel.bandwidth == 0) { 29249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); 29349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 29449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 29549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 29649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Compass Sample rate in micro seconds. 29749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. 29849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 29949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_sample_rate(long sample_rate_us) 30049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 30149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 30249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 30349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; 30449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 30549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 30649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 30749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 30849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.sample_rate_us = sample_rate_us; 30949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.sample_rate_ms = sample_rate_us / 1000; 31049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.compass.bandwidth == 0) { 31149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); 31249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 31349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 31449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 31549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 31649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 31749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *sample_rate_ms = sensors.gyro.sample_rate_ms; 31849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 31949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_accel_sample_rate_ms(long *sample_rate_ms) 32149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 32249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *sample_rate_ms = sensors.accel.sample_rate_ms; 32349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 32449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_sample_rate_ms(long *sample_rate_ms) 32649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 32749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *sample_rate_ms = sensors.compass.sample_rate_ms; 32849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 32949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 33049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Quat Sample rate in micro seconds. 33149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sample_rate_us Set Quat Sample rate in us 33249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 33349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_quat_sample_rate(long sample_rate_us) 33449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 33549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 33649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 33749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; 33849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 33949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 34049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 34149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 34249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.sample_rate_us = sample_rate_us; 34349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.sample_rate_ms = sample_rate_us / 1000; 34449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 34549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Gyro Bandwidth in Hz 34749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bandwidth_hz Gyro bandwidth in Hz 34849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 34949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_gyro_bandwidth(int bandwidth_hz) 35049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 35149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.bandwidth = bandwidth_hz; 35249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 35349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 35449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Accel Bandwidth in Hz 35549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bandwidth_hz Gyro bandwidth in Hz 35649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 35749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_bandwidth(int bandwidth_hz) 35849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 35949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.bandwidth = bandwidth_hz; 36049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 36149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 36249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set Compass Bandwidth in Hz 36349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bandwidth_hz Gyro bandwidth in Hz 36449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 36549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_bandwidth(int bandwidth_hz) 36649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 36749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.bandwidth = bandwidth_hz; 36849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 36949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 37049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Helper function stating whether the compass is on or off. 37149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return TRUE if compass if on, 0 if compass if off 37249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 37349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_compass_on() 37449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 37549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; 37649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 37749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 37849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Helper function stating whether the gyro is on or off. 37949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return TRUE if gyro if on, 0 if gyro if off 38049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 38149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_gyro_on() 38249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 38349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; 38449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 38549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 38649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Helper function stating whether the acceleromter is on or off. 38749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return TRUE if accel if on, 0 if accel if off 38849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 38949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_accel_on() 39049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 39149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; 39249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 39349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get last timestamp across all 3 sensors that are on. 39549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* This find out which timestamp has the largest value for sensors that are on. 39649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 39749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 39849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_time_t inv_get_last_timestamp() 39949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 40049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t timestamp = 0; 40149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.accel.status & INV_SENSOR_ON) { 40249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timestamp = sensors.accel.timestamp; 40349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 40449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.gyro.status & INV_SENSOR_ON) { 40549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp < sensors.gyro.timestamp) { 40649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timestamp = sensors.gyro.timestamp; 40749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 408cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGV("g ts: %lld", timestamp); 40949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.compass.status & INV_SENSOR_ON) { 41149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp < sensors.compass.timestamp) { 41249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timestamp = sensors.compass.timestamp; 41349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.temp.status & INV_SENSOR_ON) { 41649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp < sensors.temp.timestamp) 41749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timestamp = sensors.temp.timestamp; 41849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.quat.status & INV_SENSOR_ON) { 42049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp < sensors.quat.timestamp) 42149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timestamp = sensors.quat.timestamp; 422cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGV("q ts: %lld", timestamp); 42349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 424cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 42549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return timestamp; 42649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 42749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 42849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the orientation and sensitivity of the gyro data. 42949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar defining the transformation from chip mounting 43049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to the body frame. The function inv_orientation_matrix_to_scalar() 43149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* can convert the transformation matrix to this scalar and describes the 43249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* scalar in further detail. 43349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensitivity A scale factor to convert device units to g's 43449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that g's = device_units * sensitivity / 2^30. Typically 43549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum g_value * 2^15. 43649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 43749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_orientation_and_scale(int orientation, long sensitivity) 43849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 43949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 44049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 44149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_A_ORIENT; 44249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 44349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 44449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 44549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 44649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 44749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow set_sensor_orientation_and_scale(&sensors.accel, orientation, 44849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensitivity); 44949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 45049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 45149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the Orientation and Sensitivity of the gyro data. 45249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar defining the transformation from chip mounting 45349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to the body frame. The function inv_orientation_matrix_to_scalar() 45449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* can convert the transformation matrix to this scalar and describes the 45549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* scalar in further detail. 45649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensitivity A scale factor to convert device units to uT 45749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* such that uT = device_units * sensitivity / 2^30. Typically 45849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* it works out to be the maximum uT_value * 2^15. 45949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 46049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_orientation_and_scale(int orientation, long sensitivity) 46149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 46249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 46349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 46449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_C_ORIENT; 46549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 46649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 46749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 46849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 46949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 47049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); 47149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 47249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 47349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_matrix_vector_mult(const long *A, const long *x, long *y) 47449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 47549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); 47649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); 47749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); 47849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 47949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 48049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Takes raw data stored in the sensor, removes bias, and converts it to 48149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* calibrated data in the body frame. Also store raw data for body frame. 48249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in,out] sensor structure to modify 48349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bias bias in the mounting frame, in hardware units scaled by 48449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 2^16. Length 3. 48549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 48649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) 48749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 48849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long raw32[3]; 48949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 49049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Convert raw to calibrated 49149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[0] = (long)sensor->raw[0] << 15; 49249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[1] = (long)sensor->raw[1] << 15; 49349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[2] = (long)sensor->raw[2] << 15; 49449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 49549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); 49649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 49749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[0] -= bias[0] >> 1; 49849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[1] -= bias[1] >> 1; 49949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw32[2] -= bias[2] >> 1; 50049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); 50249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensor->status |= INV_CALIBRATED; 50449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 50549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns the current bias for the compass 50749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. 50849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Length 3. 50949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 51049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_bias(long *bias) 51149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 51249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias != NULL) { 51349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); 51449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 51549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 51649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the compass bias 51849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bias Length 3, in body frame, in hardware units scaled by 2^16 to allow fractional bit correction. 51949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] accuracy Accuracy of compass data, where 3=most accurate, and 0=least accurate. 52049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 52149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_bias(const long *bias, int accuracy) 52249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 52349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { 52449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); 52549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 52649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 52749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.accuracy = accuracy; 52849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.compass_accuracy = accuracy; 52949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); 53049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 53149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set the state of a compass disturbance 53349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] dist 1=disturbance, 0=no disturbance 53449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 53549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_disturbance(int dist) 53649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 53749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.compass_disturbance = dist; 53849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 53949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_compass_disturbance(void) { 54149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return inv_data_builder.compass_disturbance; 54249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 543cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 544cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/** 545cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Sets the factory accel bias 546cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * @param[in] bias 547cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Accel bias in hardware units (+/- 2 gee full scale assumed) 548cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * scaled by 2^16. In chip mounting frame. Length of 3. 549cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro */ 550cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarovoid inv_set_accel_bias(const long *bias) 55149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 552cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (!bias) 553cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 554cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 555cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (memcmp(inv_data_builder.save.factory_accel_bias, bias, 556cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sizeof(inv_data_builder.save.factory_accel_bias))) { 557cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(inv_data_builder.save.factory_accel_bias, bias, 558cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sizeof(inv_data_builder.save.factory_accel_bias)); 55949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 560cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_message(INV_MSG_NEW_FAB_EVENT, INV_MSG_NEW_FAB_EVENT, 0); 56149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 56249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the accel accuracy. 56449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 56549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 56649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_accuracy(int accuracy) 56749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 56849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.accuracy = accuracy; 56949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.accel_accuracy = accuracy; 57049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 57149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the accel bias with control over which axis. 57349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 57449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 57549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] mask Mask to select axis to apply bias set. 57649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 57749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) 57849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 57949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias) { 58049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mask & 1){ 581cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_data_builder.save_accel_mpl.accel_bias[0] = bias[0]; 58249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 58349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mask & 2){ 584cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_data_builder.save_accel_mpl.accel_bias[1] = bias[1]; 58549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 58649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mask & 4){ 587cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_data_builder.save_accel_mpl.accel_bias[2] = bias[2]; 58849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 58949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 590cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); 59149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 592cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_accel_accuracy(accuracy); 59349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 59449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 59549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/** 5974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Sets the factory gyro bias 5984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * @param[in] bias 5994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Gyro bias in hardware units (+/- 2000 dps full scale assumed) 6004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * scaled by 2^16. In chip mounting frame. Length of 3. 6014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow */ 60249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_gyro_bias(const long *bias) 60349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 6044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!bias) 6054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return; 6064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (memcmp(inv_data_builder.save.factory_gyro_bias, bias, 6084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sizeof(inv_data_builder.save.factory_gyro_bias))) { 6094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memcpy(inv_data_builder.save.factory_gyro_bias, bias, 6104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sizeof(inv_data_builder.save.factory_gyro_bias)); 61149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 6124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_message(INV_MSG_NEW_FGB_EVENT, INV_MSG_NEW_FGB_EVENT, 0); 61349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 61449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/** 6164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Sets the mpl gyro bias 6174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * @param[in] bias 6184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Gyro bias in hardware units scaled by 2^16 (+/- 2000 dps full 6194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * scale assumed). In chip mounting frame. Length 3. 6204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * @param[in] accuracy 6214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Accuracy of bias. 0 = least accurate, 3 = most accurate. 6224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow */ 62349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_mpl_gyro_bias(const long *bias, int accuracy) 62449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 62549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias != NULL) { 6264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (memcmp(inv_data_builder.save_mpl.gyro_bias, bias, 6274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sizeof(inv_data_builder.save_mpl.gyro_bias))) { 6284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memcpy(inv_data_builder.save_mpl.gyro_bias, bias, 6294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sizeof(inv_data_builder.save_mpl.gyro_bias)); 6304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_apply_calibration(&sensors.gyro, 6314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_data_builder.save_mpl.gyro_bias); 63249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 63349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 63449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.accuracy = accuracy; 63549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.gyro_accuracy = accuracy; 63649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 63749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: What should we do if there's no temperature data? */ 63849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.temp.calibrated[0]) 63949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; 64049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 64149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Set to 27 deg C for now until we've got a better solution. */ 64249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.gyro_temp = 27L << 16; 64349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); 64449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 64549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: this flag works around the synchronization problem seen with using 64649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow the user-exposed message layer to signal the temperature compensation 64749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow module that gyro biases were set. 64849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow A better, cleaner method is certainly needed. */ 64949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.gyro_bias_tc_set = true; 65049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 65149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 65249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 65349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @internal 65449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Return whether gyro biases were set to signal the temperature 65549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * compensation algorithm that it can collect a data point to build 65649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * the temperature slope while in no motion state. 65749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * The flag clear automatically after is read. 65849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return true if the flag was set, indicating gyro biases were set. 65949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * false if the flag was not set. 66049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 66149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_gyro_bias_tc_set(void) 66249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 66349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int flag = (inv_data_builder.save.gyro_bias_tc_set == true); 66449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.gyro_bias_tc_set = false; 66549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return flag; 66649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 66749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 67049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Get the mpl gyro biases 67149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] bias 6724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Gyro calibrated bias. 67349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Length 3. 67449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 67549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_mpl_gyro_bias(long *bias, long *temp) 67649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 67749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias != NULL) 67849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(bias, inv_data_builder.save_mpl.gyro_bias, 67949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof(inv_data_builder.save_mpl.gyro_bias)); 68049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (temp != NULL) 68249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp[0] = inv_data_builder.save.gyro_temp; 68349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 68449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/** Gyro Bias in the form used by the DMP. 6864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow* @param[out] bias Gyro Bias in the form used by the DMP. It is scaled appropriately 6874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow* and is in the body frame as needed. If this bias is applied in the DMP 6884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow* then any quaternion must have the flag INV_BIAS_APPLIED set if it is a 6894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow* 3-axis quaternion, or INV_QUAT_6AXIS if it is a 6-axis quaternion 6904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow*/ 6914a28f9c897c46c42a255823f7e307169a828a025Rosa Chowvoid inv_get_gyro_bias_dmp_units(long *bias) 6924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 6934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (bias == NULL) 6944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return; 695cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_convert_to_body_with_scale(sensors.gyro.orientation, 46850825L, 696cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_data_builder.save_mpl.gyro_bias, bias); 6974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 6984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 69949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* TODO: Add this information to inv_sensor_cal_t */ 70049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 70149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Get the gyro biases and temperature record from MPL 70249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] bias 70349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Gyro bias in hardware units scaled by 2^16. 70449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * In chip mounting frame. 70549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Length 3. 70649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 70749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_gyro_bias(long *bias) 70849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 70949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias != NULL) 71049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(bias, inv_data_builder.save.factory_gyro_bias, 71149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof(inv_data_builder.save.factory_gyro_bias)); 71249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 71349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 714cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/** 715cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Get factory accel bias mask 716cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * @param[in] bias 717cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Accel bias mask 718cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * 1 is set, 0 is not set, Length 3 = x,y,z. 719cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro */ 720cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint inv_get_factory_accel_bias_mask() 721cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 722cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long bias[3]; 723cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int bias_mask = 0; 724cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_accel_bias(bias); 725cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (bias != NULL) { 726cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int i; 727cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro for(i = 0; i < 3; i++) { 728cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(bias[i] != 0) { 729cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro bias_mask |= 1 << i; 730cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 731cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro bias_mask &= ~ (1 << i); 732cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 733cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 734cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 735cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return bias_mask; 736cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 737cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 738cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/** 739cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Get accel bias from MPL 740cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * @param[in] bias 741cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Accel bias in hardware units scaled by 2^16. 742cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * In chp mounting frame. 743cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro * Length 3. 744cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro */ 745cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarovoid inv_get_accel_bias(long *bias) 746cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 747cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (bias != NULL) 748cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(bias, inv_data_builder.save.factory_accel_bias, 749cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sizeof(inv_data_builder.save.factory_accel_bias)); 750cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 751cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 75249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get Accel Bias 75349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] bias Accel bias 75449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] temp Temperature where 1 C = 2^16 75549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 756cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarovoid inv_get_mpl_accel_bias(long *bias, long *temp) 75749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 75849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (bias != NULL) 759cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(bias, inv_data_builder.save_accel_mpl.accel_bias, 760cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sizeof(inv_data_builder.save_accel_mpl.accel_bias)); 76149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (temp != NULL) 76249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp[0] = inv_data_builder.save.accel_temp; 76349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 76449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 76549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 76649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Record new accel data for use when inv_execute_on_data() is called 7674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * @param[in] accel 7684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * accel data, length 3. 7694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Calibrated data is in m/s^2 scaled by 2^16 in body frame. 7704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Raw data is in device units in chip mounting frame. 77149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] status 7724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 7734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * being most accurate. 7744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * The upper bit INV_CALIBRATED, is set if the data was calibrated 7754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * outside MPL and it is not set if the data being passed is raw. 7764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Raw data should be in device units, typically in a 16-bit range. 77749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] timestamp 7784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow * Monotonic time stamp, for Android it's in nanoseconds. 77949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 78049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 78149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 78249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 78349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 78449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 78549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_ACCEL; 78649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 78749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); 78849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 78949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 79049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 79149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 79249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((status & INV_CALIBRATED) == 0) { 79349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.raw[0] = (short)accel[0]; 79449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.raw[1] = (short)accel[1]; 79549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.raw[2] = (short)accel[2]; 79649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status |= INV_RAW_DATA; 797cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); 79849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 79949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.calibrated[0] = accel[0]; 80049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.calibrated[1] = accel[1]; 80149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.calibrated[2] = accel[2]; 80249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status |= INV_CALIBRATED; 80349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.accuracy = status & 3; 80449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.accel_accuracy = status & 3; 80549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 80649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; 80749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.timestamp_prev = sensors.accel.timestamp; 80849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.timestamp = timestamp; 80949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 81049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 81149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 81249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 81349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Record new gyro data and calls inv_execute_on_data() if previous 81449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* sample has not been processed. 81549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] gyro Data is in device units. Length 3. 81649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 81749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] executed Set to 1 if data processing was done. 81849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 81949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 82049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) 82149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 82249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 82349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 82449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_GYRO; 82549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 82649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); 82749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 82849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 82949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 83049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 83149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); 83249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 83349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.timestamp_prev = sensors.gyro.timestamp; 83449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.timestamp = timestamp; 83549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias); 83649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 83749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 83849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 83949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 84049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Record new compass data for use when inv_execute_on_data() is called 84149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. 84249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Length 3. 84349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. 84449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is 84549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* not set if the data being passed is raw. Raw data should be in device units, typically 84649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* in a 16-bit range. 84749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 84849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] executed Set to 1 if data processing was done. 84949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 85049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 85149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_build_compass(const long *compass, int status, 85249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t timestamp) 85349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 85449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 85549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 85649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_COMPASS; 85749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 85849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); 85949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 86049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 86149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 86249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 86349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((status & INV_CALIBRATED) == 0) { 86449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long data[3]; 86549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_soft_iron_input_data(compass); 86649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_soft_iron_output_data(data); 86749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.raw[0] = (short)data[0]; 86849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.raw[1] = (short)data[1]; 86949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.raw[2] = (short)data[2]; 87049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 87149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status |= INV_RAW_DATA; 87249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 87349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.calibrated[0] = compass[0]; 87449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.calibrated[1] = compass[1]; 87549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.calibrated[2] = compass[2]; 87649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status |= INV_CALIBRATED; 87749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.accuracy = status & 3; 87849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.save.compass_accuracy = status & 3; 87949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 88049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.timestamp_prev = sensors.compass.timestamp; 88149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.timestamp = timestamp; 88249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; 88349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 88449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 88549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 88649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 88749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Record new temperature data for use when inv_execute_on_data() is called. 88849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] temp Temperature data in q16 format. 88949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] timestamp Monotonic time stamp; for Android it's in 89049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * nanoseconds. 89149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 89249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 89349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_build_temp(const long temp, inv_time_t timestamp) 89449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 89549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 89649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 89749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_TEMPERATURE; 89849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 89949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); 90049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 90149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 90249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 90349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.calibrated[0] = temp; 90449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 90549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.timestamp_prev = sensors.temp.timestamp; 90649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.timestamp = timestamp; 90749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: Apply scale, remove offset. */ 90849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 90949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 91049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 91149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** quaternion data 91249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 91349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Real part first. Length 4. 91449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] status number of axis, 16-bit or 32-bit 915cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* set INV_QUAT_3AXIS if input quaternion has only 3 elements (no scalar). 916cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* inv_compute_scalar_part() assumes 32-bit data. If using 16-bit quaternion, 917cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* shift 16 bits first before calling this function. 91849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp 91949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp Monotonic time stamp; for Android it's in 92049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* nanoseconds. 92149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] executed Set to 1 if data processing was done. 92249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 92349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 92449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) 92549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 92649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 92749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 92849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_QUAT; 92949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 93049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); 93149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 93249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 93349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 93449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 93549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Android version DMP does not have scalar part */ 93649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (status & INV_QUAT_3AXIS) { 93749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long resultQuat[4]; 93849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("3q: %ld,%ld,%ld\n", quat[0], quat[1], quat[2]); 93949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_compute_scalar_part(quat, resultQuat); 94049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("4q: %ld,%ld,%ld,%ld\n", resultQuat[0], resultQuat[1], resultQuat[2], resultQuat[3]); 94149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw)); 94249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 94349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); 94449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 94549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.timestamp = timestamp; 94649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 947cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors.quat.status |= (INV_QUAT_3AXIS & status); 94849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status |= (INV_BIAS_APPLIED & status); 949cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors.quat.status |= (INV_QUAT_6AXIS & status); 95049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status |= (INV_QUAT_9AXIS & status); 95149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status); 952cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGV("quat.status: %d", sensors.quat.status); 95349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) { 95449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Set quaternion 95549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_store_gaming_quaternion(quat, timestamp); 95649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 95749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.quat.status & INV_QUAT_9AXIS) { 95849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long identity[4] = {(1L<<30), 0, 0, 0}; 95949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_correction(identity, timestamp); 96049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 96149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 96249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 96349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 964cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroinv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp) 965cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 966cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors.pressure.status |= INV_NEW_DATA; 967cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return INV_SUCCESS; 968cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 969cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 97049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This should be called when the accel has been turned off. This is so 97149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* that we will know if the data is contiguous. 97249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 97349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_accel_was_turned_off() 97449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 97549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 97649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 97749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; 97849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 97949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 98049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 98149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status = 0; 98249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 98349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 98449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This should be called when the compass has been turned off. This is so 98549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* that we will know if the data is contiguous. 98649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 98749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_compass_was_turned_off() 98849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 98949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 99049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 99149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; 99249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 99349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 99449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 99549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status = 0; 99649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 99749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 99849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This should be called when the quaternion data from the DMP has been turned off. This is so 99949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* that we will know if the data is contiguous. 100049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 100149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_quaternion_sensor_was_turned_off(void) 100249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 100349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 100449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 100549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_QUAT_OFF; 100649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 100749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 100849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 100949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status = 0; 101049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 101149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 101249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This should be called when the gyro has been turned off. This is so 101349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* that we will know if the data is contiguous. 101449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 101549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_gyro_was_turned_off() 101649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 101749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 101849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 101949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_GYRO_OFF; 102049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 102149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 102249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 102349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.status = 0; 102449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 102549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 102649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This should be called when the temperature sensor has been turned off. 102749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * This is so that we will know if the data is contiguous. 102849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 102949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_temperature_was_turned_off() 103049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 103149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.status = 0; 103249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 103349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 103449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Registers to receive a callback when there is new sensor data. 103549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @internal 103649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] func Function pointer to receive callback when there is new sensor data 103749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 103849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* numbers must be unique. 103949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 104049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 104149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* gyro data, INV_MAG_NEW = compass data. So passing in 104249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* INV_ACCEL_NEW | INV_MAG_NEW, a 104349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* callback would be generated if there was new magnetomer data OR new accel data. 104449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 104549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_register_data_cb( 104649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t (*func)(struct inv_sensor_cal_t *data), 104749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int priority, int sensor_type) 104849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 104949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result = INV_SUCCESS; 105049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk, nn; 105149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 105249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Make sure we haven't registered this function already 105349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Or used the same priority 105449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 105549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((inv_data_builder.process[kk].func == func) || 105649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (inv_data_builder.process[kk].priority == priority)) { 105749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; //fixme give a warning 105849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 105949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 106049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 106149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Make sure we have not filled up our number of allowable callbacks 106249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { 106349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow kk = 0; 106449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.num_cb != 0) { 106549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set kk to be where this new callback goes in the array 106649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow while ((kk < inv_data_builder.num_cb) && 106749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (inv_data_builder.process[kk].priority < priority)) { 106849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow kk++; 106949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 107049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (kk != inv_data_builder.num_cb) { 107149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // We need to move the others 107249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (nn = inv_data_builder.num_cb; nn > kk; --nn) { 107349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[nn] = 107449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[nn - 1]; 107549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 107649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 107749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 107849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Add new callback 107949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[kk].func = func; 108049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[kk].priority = priority; 108149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[kk].data_required = sensor_type; 108249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.num_cb++; 108349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 108449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGE("Unable to add feature callback as too many were already registered\n"); 108549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = INV_ERROR_MEMORY_EXAUSTED; 108649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 108749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 108849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 108949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 109049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 109149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Unregisters the callback that happens when new sensor data is received. 109249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @internal 109349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] func Function pointer to receive callback when there is new sensor data 109449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 109549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* numbers must be unique. 109649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 109749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 109849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* gyro data, INV_MAG_NEW = compass data. So passing in 109949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* INV_ACCEL_NEW | INV_MAG_NEW, a 110049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* callback would be generated if there was new magnetomer data OR new accel data. 110149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 110249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_unregister_data_cb( 110349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t (*func)(struct inv_sensor_cal_t *data)) 110449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 110549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk, nn; 110649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 110749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 110849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.process[kk].func == func) { 110949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Delete this callback 111049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { 111149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[nn - 1] = 111249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.process[nn]; 111349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 111449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_data_builder.num_cb--; 111549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 111649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 111749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 111849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 111949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; // We did not find the callback 112049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 112149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 112249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** After at least one of inv_build_gyro(), inv_build_accel(), or 112349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* inv_build_compass() has been called, this function should be called. 112449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* It will process the data it has received and update all the internal states 112549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* and features that have been turned on. 112649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 112749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 112849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_execute_on_data(void) 112949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 113049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result, first_error; 113149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk; 113249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int mode; 113349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 113449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 113549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.debug_mode == RD_RECORD) { 113649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = PLAYBACK_DBG_TYPE_EXECUTE; 113749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fwrite(&type, sizeof(type), 1, inv_data_builder.file); 113849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 113949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 114049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Determine what new data we have 114149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode = 0; 114249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.gyro.status & INV_NEW_DATA) 114349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode |= INV_GYRO_NEW; 114449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.accel.status & INV_NEW_DATA) 114549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode |= INV_ACCEL_NEW; 114649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.compass.status & INV_NEW_DATA) 114749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode |= INV_MAG_NEW; 114849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.temp.status & INV_NEW_DATA) 114949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode |= INV_TEMP_NEW; 115049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.quat.status & INV_QUAT_NEW) 115149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mode |= INV_QUAT_NEW; 1152cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (sensors.pressure.status & INV_NEW_DATA) 1153cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mode |= INV_PRESSURE_NEW; 115449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 115549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow first_error = INV_SUCCESS; 115649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 115749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 115849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mode & inv_data_builder.process[kk].data_required) { 115949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_data_builder.process[kk].func(&sensors); 116049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result && !first_error) { 116149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow first_error = result; 116249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 116349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 116449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 116549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 116649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_contiguous(); 116749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 116849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return first_error; 116949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 117049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 117149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. 117249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 117349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 117449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void inv_set_contiguous(void) 117549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 117649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t current_time = 0; 117749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.gyro.status & INV_NEW_DATA) { 117849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.status |= INV_CONTIGUOUS; 117949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow current_time = sensors.gyro.timestamp; 118049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.accel.status & INV_NEW_DATA) { 118249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status |= INV_CONTIGUOUS; 118349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow current_time = MAX(current_time, sensors.accel.timestamp); 118449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.compass.status & INV_NEW_DATA) { 118649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status |= INV_CONTIGUOUS; 118749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow current_time = MAX(current_time, sensors.compass.timestamp); 118849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.temp.status & INV_NEW_DATA) { 119049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.status |= INV_CONTIGUOUS; 119149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow current_time = MAX(current_time, sensors.temp.timestamp); 119249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 119349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.quat.status & INV_NEW_DATA) { 119449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status |= INV_CONTIGUOUS; 119549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow current_time = MAX(current_time, sensors.quat.timestamp); 119649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 119749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 119849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#if 0 119949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() 120049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * type functions. This is just in case that breaks down. We make sure 120149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * all the data is within 2 seconds of the newest piece of data*/ 120249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) 120349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_gyro_was_turned_off(); 120449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) 120549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_accel_was_turned_off(); 120649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) 120749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_compass_was_turned_off(); 120849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: Temperature might not need to be read this quickly. */ 120949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) 121049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_temperature_was_turned_off(); 121149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 121249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 121349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* clear bits */ 121449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.gyro.status &= ~INV_NEW_DATA; 121549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.accel.status &= ~INV_NEW_DATA; 121649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.compass.status &= ~INV_NEW_DATA; 121749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.temp.status &= ~INV_NEW_DATA; 121849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.quat.status &= ~INV_NEW_DATA; 1219cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors.pressure.status &= ~INV_NEW_DATA; 122049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 122149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 122249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of accel data including data, accuracy and timestamp. 122349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Accel Data where 1g = 2^16 122449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 122549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 122649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 122749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 122849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 122949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL) { 123049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); 123149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 123249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp != NULL) { 123349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.accel.timestamp; 123449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 123549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accuracy != NULL) { 123649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = sensors.accel.accuracy; 123749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 123849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 123949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 124049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of gyro data including data, accuracy and timestamp. 124149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Gyro Data where 1 dps = 2^16 124249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 124349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 124449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 124549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 124649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 124749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 124849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp != NULL) { 124949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.gyro.timestamp; 125049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 125149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accuracy != NULL) { 125249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = sensors.gyro.accuracy; 125349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 125449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 125549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 125649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of gyro raw data including data, accuracy and timestamp. 125749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Gyro Data where 1 dps = 2^16 125849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 125949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 126049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 126149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 126249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 126349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); 126449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp != NULL) { 126549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.gyro.timestamp; 126649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 126749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accuracy != NULL) { 126849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = 0; 126949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 127049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 127149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 127249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get's latest gyro data. 127349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. 127449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 127549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_gyro(long *gyro) 127649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 127749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 127849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 127949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 128049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of compass data including data, accuracy and timestamp. 128149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Compass Data where 1 uT = 2^16 128249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 128349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 128449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 128549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 128649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 128749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); 128849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp != NULL) { 128949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.compass.timestamp; 129049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 129149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accuracy != NULL) { 129249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.compass_disturbance) 129349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = 0; 129449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 129549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = sensors.compass.accuracy; 129649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 129749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 129849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 129949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of compass raw data including data, accuracy and timestamp. 130049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Compass Data where 1 uT = 2^16 130149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 130249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 130349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 130449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 130549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 130649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled)); 130749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp != NULL) { 130849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.compass.timestamp; 130949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 131049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //per Michele, since data is raw, accuracy should = 0 131149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = 0; 131249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 131349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 131449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets a whole set of temperature data including data, accuracy and timestamp. 131549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Temperature data where 1 degree C = 2^16 131649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy 0 to 3, where 3 is most accurate. 131749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp The timestamp of the data sample. 131849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 131949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) 132049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 132149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = sensors.temp.calibrated[0]; 132249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timestamp) 132349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = sensors.temp.timestamp; 132449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accuracy) 132549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = sensors.temp.accuracy; 132649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 132749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 132849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns accuracy of gyro. 132949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. 133049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 133149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_gyro_accuracy(void) 133249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 133349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.gyro.accuracy; 133449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 133549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 133649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns accuracy of compass. 133749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. 133849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 133949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_mag_accuracy(void) 134049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 134149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_data_builder.compass_disturbance) 134249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 134349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.compass.accuracy; 134449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 134549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 134649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns accuracy of accel. 134749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. 134849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 134949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_accel_accuracy(void) 135049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 135149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sensors.accel.accuracy; 135249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 135349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 135449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_gyro_orient(int *orient) 135549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 135649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *orient = sensors.gyro.orientation; 135749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 135849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 135949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 136049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_accel_orient(int *orient) 136149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 136249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *orient = sensors.accel.orientation; 136349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 136449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 136549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 136649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/*======================================================================*/ 136749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* compass soft iron module */ 136849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/*======================================================================*/ 136949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 137049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. 137149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] the pointer of the 3x3 matrix in Q30 format 137249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 137349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_soft_iron_matrix_d(long *matrix) { 137449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 137549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<9; i++) { 137649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow matrix[i] = sensors.soft_iron.matrix_d[i]; 137749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 137849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 137949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 138049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. 138149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] the pointer of the 3x3 matrix in Q30 format 138249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 138349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_soft_iron_matrix_d(long *matrix) { 138449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 138549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<9; i++) { 138649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set the floating point matrix 138749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_d[i] = matrix[i]; 138849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // convert to Q30 format 138949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]); 139049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 139149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 139249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets the 3x3 compass transform matrix in 32 bit floating point format. 139349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] the pointer of the 3x3 matrix in floating point format 139449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 139549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_soft_iron_matrix_f(float *matrix) { 139649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 139749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<9; i++) { 139849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow matrix[i] = sensors.soft_iron.matrix_f[i]; 139949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 140049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 140149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the 3x3 compass transform matrix in 32 bit floating point format. 140249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] the pointer of the 3x3 matrix in floating point format 140349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 140449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_soft_iron_matrix_f(float *matrix) { 140549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 140649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<9; i++) { 140749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set the floating point matrix 140849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_f[i] = matrix[i]; 140949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // convert to Q30 format 141049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG); 141149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 141249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 141349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 141449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine gets the fixed point Q30 compass data after the soft iron transformation. 141549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] the pointer of the 3x1 vector compass data in MPL format 141649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 141749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_soft_iron_output_data(long *data) { 141849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 141949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 142049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[i] = sensors.soft_iron.trans[i]; 142149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 142249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 142349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine gets the fixed point Q30 compass data before the soft iron transformation. 142449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] the pointer of the 3x1 vector compass data in MPL format 142549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 142649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_soft_iron_input_data(long *data) { 142749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 142849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 142949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[i] = sensors.soft_iron.raw[i]; 143049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 143149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 143249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine sets the compass raw data for the soft iron transformation. 143349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[int] the pointer of the 3x1 vector compass raw data in MPL format 143449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 143549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_soft_iron_input_data(const long *data) { 143649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 143749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 143849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.raw[i] = data[i]; 143949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 144049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors.soft_iron.enable == 1) { 144149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans); 144249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 144349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 144449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.trans[i] = data[i]; 144549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 144649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 144749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 144849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 144949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine resets the the soft iron transformation to unity matrix and 145049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * disable the soft iron transformation process by default. 145149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 145249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_reset_compass_soft_iron_matrix(void) { 145349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 145449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<9; i++) { 145549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_f[i] = 0.0f; 145649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 145749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 145849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d)); 145949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 146049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 146149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set the floating point matrix 146249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_f[i*4] = 1.0; 146349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set the fixed point matrix 146449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG; 146549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 146649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 146749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_disable_compass_soft_iron_matrix(); 146849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 146949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 147049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine enables the the soft iron transformation process. 147149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 147249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_enable_compass_soft_iron_matrix(void) { 147349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.enable = 1; 147449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 147549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 147649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This subroutine disables the the soft iron transformation process. 147749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 147849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_disable_compass_soft_iron_matrix(void) { 147949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors.soft_iron.enable = 0; 148049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 148149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 148249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 148349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @} 148449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 1485