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 HAL_Outputs hal_outputs 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Motion Library - HAL Outputs 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Sets up common outputs for HAL 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @{ 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @file hal_outputs.c 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief HAL Outputs. 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#undef MPL_LOG_TAG 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MPL_LOG_TAG "MLLITE" 22cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro//#define MPL_LOG_9AXIS_DEBUG 1 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "hal_outputs.h" 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "log.h" 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_math_func.h" 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlmath.h" 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "start_manager.h" 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "data_builder.h" 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "results_holder.h" 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* commenting this define out will bypass the low pass filter noise reduction 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow filter for compass data. 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Disable this only for testing purpose (e.g. comparing the raw and calibrated 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow compass data, since the former is unfiltered and the latter is filtered, 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow leading to a small difference in the readings sample by sample). 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Android specifications require this filter to be enabled to have the Magnetic 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Field output's standard deviation fall below 0.5 uT. 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define CALIB_COMPASS_NOISE_REDUCTION 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct hal_output_t { 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int accuracy_mag; /**< Compass accuracy */ 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //int accuracy_gyro; /**< Gyro Accuracy */ 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //int accuracy_accel; /**< Accel Accuracy */ 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int accuracy_quat; /**< quat Accuracy */ 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t nav_timestamp; 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t gam_timestamp; 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //inv_time_t accel_timestamp; 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t mag_timestamp; 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long nav_quat[4]; 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int gyro_status; 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int accel_status; 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int compass_status; 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int nine_axis_status; 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int quat_status; 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_biquad_filter_t lp_filter[3]; 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float compass_float[3]; 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct hal_output_t hal_out; 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Acceleration (m/s^2) in body frame. 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* should return a vector of magnitude near 9.81 m/s^2 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* inv_build_accel(). 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * So this 9.80665 / 2^16 */ 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define ACCEL_CONVERSION 0.000149637603759766f 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long accel[3]; 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(accel, accuracy, timestamp); 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = accel[0] * ACCEL_CONVERSION; 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = accel[1] * ACCEL_CONVERSION; 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = accel[2] * ACCEL_CONVERSION; 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.accel_status & INV_NEW_DATA) 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 90cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGV("accel values:%f %f %f -%d -%lld", values[0], values[1], 91cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro values[2], status, *timestamp); 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Linear Acceleration (m/s^2) in Body Frame. 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* accel biases while at rest. 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* inv_build_accel(). 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 10249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gravity[3], accel[3]; 107cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int status; 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(accel, accuracy, timestamp); 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gravity(gravity); 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel[0] -= gravity[0] >> 14; 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel[1] -= gravity[1] >> 14; 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel[2] -= gravity[2] >> 14; 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = accel[0] * ACCEL_CONVERSION; 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = accel[1] * ACCEL_CONVERSION; 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = accel[2] * ACCEL_CONVERSION; 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 118cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (hal_out.accel_status & INV_NEW_DATA) 119cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro status = 1; 120cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro else 121cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro status = 0; 122cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return status; 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gravity vector (m/s^2) in Body Frame. 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Gravity vector in body frame, length 3, (m/s^2) 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* inv_build_accel(). 13049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 13149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 13249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_gravity(float *values, int8_t *accuracy, 13349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 13449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 13549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gravity[3]; 13649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 13749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 13849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = (int8_t) hal_out.accuracy_quat; 13949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = hal_out.nav_timestamp; 14049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gravity(gravity); 14149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; 14249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; 14349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; 144cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (hal_out.accel_status & INV_NEW_DATA) 14549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 14649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 14749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 14949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. 15249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * So this is: pi / 2^16 / 180 */ 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GYRO_CONVERSION 2.66316109007924e-007f 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gyroscope calibrated data (rad/s) in body frame. 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Rotation Rate in rad/sec. 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 15849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* inv_build_gyro(). 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 16149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 16449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 16549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gyro[3]; 16649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 16749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 16849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gyro_set(gyro, accuracy, timestamp); 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = gyro[0] * GYRO_CONVERSION; 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = gyro[1] * GYRO_CONVERSION; 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = gyro[2] * GYRO_CONVERSION; 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.gyro_status & INV_NEW_DATA) 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gyroscope raw data (rad/s) in body frame. 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Rotation Rate in rad/sec. 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* while 3 is most accurate. 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. Derived from the 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* timestamp sent to inv_build_gyro(). 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gyro[3]; 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 19349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gyro_set_raw(gyro, accuracy, timestamp); 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = gyro[0] * GYRO_CONVERSION; 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = gyro[1] * GYRO_CONVERSION; 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = gyro[2] * GYRO_CONVERSION; 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.gyro_status & INV_NEW_DATA) 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* This corresponds to Sensor.TYPE_ROTATION_VECTOR. 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The rotation vector represents the orientation of the device as a combination 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* around an axis {x, y, z}. <br> 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The three elements of the rotation vector are 21149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation 21249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is 21349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* equal to the direction of the axis of rotation. 21449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 21549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The three elements of the rotation vector are equal to the last three components of a unit quaternion 21649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2). 21749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 21849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. 21949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The reference coordinate system is defined as a direct orthonormal basis, where: 220cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* 221cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). 222cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. 223cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* -Z points towards the sky and is perpendicular to the ground. 224cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* @param[out] values 225cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* Length 5, 4th element being the w angle of the originating 4 226cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* elements quaternion and 5th element being the heading accuracy 227cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* at 95%. 22849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy is not defined 22949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp Timestamp. In (ns) for Android. 23049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 23149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 23249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, 23349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 23449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 23549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = (int8_t) hal_out.accuracy_quat; 23649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = hal_out.nav_timestamp; 23749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.nav_quat[0] >= 0) { 23949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 24049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 24149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 24249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 24349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 24449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 24549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 24649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 24749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 24849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 24949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[4] = inv_get_heading_confidence_interval(); 25049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return hal_out.nine_axis_status; 25149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 25249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 25349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy, 25449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 25549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 25649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 25749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long accel[3], quat_6_axis[4]; 25849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(accel, accuracy, timestamp); 259cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_6axis_quaternion(quat_6_axis, timestamp); 26049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 26149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (quat_6_axis[0] >= 0) { 26249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = quat_6_axis[1] * INV_TWO_POWER_NEG_30; 26349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = quat_6_axis[2] * INV_TWO_POWER_NEG_30; 26449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = quat_6_axis[3] * INV_TWO_POWER_NEG_30; 26549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = quat_6_axis[0] * INV_TWO_POWER_NEG_30; 26649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 26749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = -quat_6_axis[1] * INV_TWO_POWER_NEG_30; 26849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = -quat_6_axis[2] * INV_TWO_POWER_NEG_30; 26949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = -quat_6_axis[3] * INV_TWO_POWER_NEG_30; 27049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = -quat_6_axis[0] * INV_TWO_POWER_NEG_30; 27149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 27249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //This sensor does not report an estimated heading accuracy 27349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[4] = 0; 274cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (hal_out.quat_status & INV_QUAT_3AXIS) 275cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro { 276cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro status = hal_out.quat_status & INV_NEW_DATA? 1 : 0; 277cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 278cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro else { 279cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; 280cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 28149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("values:%f %f %f %f %f -%d -%lld", values[0], values[1], 28249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2], values[3], values[4], status, *timestamp); 28349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 28449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 28549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 28749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* This corresponds to Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR. 28849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer 28949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* instead of using a gyroscope. 29049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Fourth element = estimated_accuracy in radians (heading confidence). 29149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Length 4. 29249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy is not defined. 29349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp in (ns) for Android. 29449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated, 0 otherwise. 29549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 29649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy, 29749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 29849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 29949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass[3], quat_geomagnetic[4]; 30049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 30149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_set(compass, accuracy, timestamp); 30249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_geomagnetic_quaternion(quat_geomagnetic, timestamp); 30349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (quat_geomagnetic[0] >= 0) { 30449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = quat_geomagnetic[1] * INV_TWO_POWER_NEG_30; 30549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = quat_geomagnetic[2] * INV_TWO_POWER_NEG_30; 30649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = quat_geomagnetic[3] * INV_TWO_POWER_NEG_30; 30749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = quat_geomagnetic[0] * INV_TWO_POWER_NEG_30; 30849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 30949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[0] = -quat_geomagnetic[1] * INV_TWO_POWER_NEG_30; 31049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[1] = -quat_geomagnetic[2] * INV_TWO_POWER_NEG_30; 31149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2] = -quat_geomagnetic[3] * INV_TWO_POWER_NEG_30; 31249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[3] = -quat_geomagnetic[0] * INV_TWO_POWER_NEG_30; 31349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 31449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[4] = inv_get_accel_compass_confidence_interval(); 31549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; 31649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPL_LOGV("values:%f %f %f %f %f -%d", values[0], values[1], 31749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[2], values[3], values[4], status); 31849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (status); 31949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 32049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Compass data (uT) in body frame. 32249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Compass data in (uT), length 3. May be calibrated by having 32349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* biases removed and sensitivity adjusted 32449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 32549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp Timestamp. In (ns) for Android. 32649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 32749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 32849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, 32949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 33049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 33149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 33249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 33349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 33449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * So this is: 1 / 2^16*/ 33549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow//#define COMPASS_CONVERSION 1.52587890625e-005f 33649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 33749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = hal_out.mag_timestamp; 33849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = (int8_t) hal_out.accuracy_mag; 33949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; i++) 34149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[i] = hal_out.compass_float[i]; 34249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.compass_status & INV_NEW_DATA) 34349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 34449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 34549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 34649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 34749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 34849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Compass raw data (uT) in body frame. 35049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Compass data in (uT), length 3. May be calibrated by having 35149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* biases removed and sensitivity adjusted 35249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 35349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp Timestamp. In (ns) for Android. 35449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 35549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 35649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy, 35749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 35849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 35949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long mag[3]; 36049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 36149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 36249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 36349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_set_raw(mag, accuracy, timestamp); 36449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 36549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 36649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * So this is: 1 / 2^16*/ 36749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define COMPASS_CONVERSION 1.52587890625e-005f 36849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 36949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; i++) { 37049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow values[i] = (float)mag[i] * COMPASS_CONVERSION; 37149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 37249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hal_out.compass_status & INV_NEW_DATA) 37349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 1; 37449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 37549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = 0; 37649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return status; 37749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 37849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void inv_get_rotation_geomagnetic(float r[3][3]) 37949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 38049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long rot[9], quat_geo[4]; 38149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float conv = 1.f / (1L<<30); 38249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t timestamp; 38349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_geomagnetic_quaternion(quat_geo, ×tamp); 38449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_quaternion_to_rotation(quat_geo, rot); 38549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][0] = rot[0]*conv; 38649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][1] = rot[1]*conv; 38749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][2] = rot[2]*conv; 38849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][0] = rot[3]*conv; 38949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][1] = rot[4]*conv; 39049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][2] = rot[5]*conv; 39149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][0] = rot[6]*conv; 39249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][1] = rot[7]*conv; 39349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][2] = rot[8]*conv; 39449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 39549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void google_orientation_geomagnetic(float *g) 39649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 39749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float rad2deg = (float)(180.0 / M_PI); 39849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float R[3][3]; 39949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_rotation_geomagnetic(R); 40049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 40149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 40249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[2] = asinf ( R[2][0]) * rad2deg; 40349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (g[0] < 0) 40449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] += 360; 40549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 40649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void inv_get_rotation_6_axis(float r[3][3]) 40849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 40949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long rot[9], quat_6_axis[4]; 41049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float conv = 1.f / (1L<<30); 411cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_time_t timestamp; 41249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 413cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_6axis_quaternion(quat_6_axis, ×tamp); 41449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_quaternion_to_rotation(quat_6_axis, rot); 41549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][0] = rot[0]*conv; 41649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][1] = rot[1]*conv; 41749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][2] = rot[2]*conv; 41849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][0] = rot[3]*conv; 41949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][1] = rot[4]*conv; 42049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][2] = rot[5]*conv; 42149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][0] = rot[6]*conv; 42249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][1] = rot[7]*conv; 42349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][2] = rot[8]*conv; 42449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 42549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 42649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void google_orientation_6_axis(float *g) 42749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 42849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float rad2deg = (float)(180.0 / M_PI); 42949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float R[3][3]; 43049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 43149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_rotation_6_axis(R); 43249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 43349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 43449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 43549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[2] = asinf ( R[2][0]) * rad2deg; 43649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (g[0] < 0) 43749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] += 360; 43849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 43949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 44049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void inv_get_rotation(float r[3][3]) 44149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 44249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long rot[9]; 44349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float conv = 1.f / (1L<<30); 44449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 44549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_quaternion_to_rotation(hal_out.nav_quat, rot); 44649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][0] = rot[0]*conv; 44749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][1] = rot[1]*conv; 44849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[0][2] = rot[2]*conv; 44949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][0] = rot[3]*conv; 45049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][1] = rot[4]*conv; 45149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[1][2] = rot[5]*conv; 45249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][0] = rot[6]*conv; 45349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][1] = rot[7]*conv; 45449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow r[2][2] = rot[8]*conv; 45549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 45649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 45749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic void google_orientation(float *g) 45849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 45949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float rad2deg = (float)(180.0 / M_PI); 46049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float R[3][3]; 46149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 46249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_rotation(R); 46349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 46449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 46549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 46649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[2] = asinf ( R[2][0]) * rad2deg; 46749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (g[0] < 0) 46849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow g[0] += 360; 46949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 47049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 47149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. 47249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] values Length 3, Degrees.<br> 47349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* - values[0]: Azimuth, angle between the magnetic north direction 47449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> 47549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values 47649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* when the z-axis moves toward the y-axis.<br> 47749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive 47849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* values when the x-axis moves toward the z-axis.<br> 47949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 48049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @note This definition is different from yaw, pitch and roll used in aviation 48149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* where the X axis is along the long side of the plane (tail to nose). 48249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() 48349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* in conjunction with remapCoordinateSystem() and getOrientation() to compute 48449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* these values instead. 48549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Important note: For historical reasons the roll angle is positive in the 48649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* clockwise direction (mathematically speaking, it should be positive in 48749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* the counter-clockwise direction). 48849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 48949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp The timestamp for this sensor. 49049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns 1 if the data was updated or 0 if it was not updated. 49149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 49249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_orientation(float *values, int8_t *accuracy, 49349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 49449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 49549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = (int8_t) hal_out.accuracy_quat; 49649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = hal_out.nav_timestamp; 49749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 49849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow google_orientation(values); 49949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return hal_out.nine_axis_status; 50149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 50249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy, 50449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 50549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 50649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long accel[3]; 50749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(accel, accuracy, timestamp); 50849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.gam_timestamp = hal_out.nav_timestamp; 51049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = hal_out.gam_timestamp; 51149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow google_orientation_6_axis(values); 51349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return hal_out.accel_status; 51549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 51649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy, 51849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t * timestamp) 51949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 52049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass[3], quat_geomagnetic[4]; 52149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_set(compass, accuracy, timestamp); 52249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_geomagnetic_quaternion(quat_geomagnetic, timestamp); 52349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 52449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow google_orientation_geomagnetic(values); 52549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return hal_out.accel_status & hal_out.compass_status; 52649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 52749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Main callback to generate HAL outputs. Typically not called by library users. 52849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensor_cal Input variable to take sensor data whenever there is new 52949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* sensor data. 53049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 53149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 53249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) 53349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 53449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int use_sensor = 0; 53549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long sr = 1000; 53649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass[3]; 53749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int8_t accuracy; 53849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 53949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (void) sensor_cal; 54049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, 54249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &hal_out.nav_timestamp); 54349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.gyro_status = sensor_cal->gyro.status; 54449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.accel_status = sensor_cal->accel.status; 54549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.compass_status = sensor_cal->compass.status; 54649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.quat_status = sensor_cal->quat.status; 547cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#if MPL_LOG_9AXIS_DEBUG 548cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGV("hal_out:g=%d", hal_out.gyro_status); 549cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 55049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Find the highest sample rate and tie generating 9-axis to that one. 55149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensor_cal->gyro.status & INV_SENSOR_ON) { 55249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sr = sensor_cal->gyro.sample_rate_ms; 55349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = 0; 55449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { 55649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sr = sensor_cal->accel.sample_rate_ms; 55749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = 1; 55849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { 56049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sr = sensor_cal->compass.sample_rate_ms; 56149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = 2; 56249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { 56449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sr = sensor_cal->quat.sample_rate_ms; 56549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = 3; 56649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Only output 9-axis if all 9 sensors are on. 56949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensor_cal->quat.status & INV_SENSOR_ON) { 57049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // If quaternion sensor is on, gyros are not required as quaternion already has that part 57149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 57249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = -1; 57349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 57449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 57549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 57649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow use_sensor = -1; 57749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 57849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 579cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#if MPL_LOG_9AXIS_DEBUG 580cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGI("use_sensor=%d", use_sensor); 581cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 58249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow switch (use_sensor) { 58349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case 0: 58449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; 58549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 58649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 58749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case 1: 58849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; 58949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nav_timestamp = sensor_cal->accel.timestamp; 59049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 59149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case 2: 59249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; 59349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nav_timestamp = sensor_cal->compass.timestamp; 59449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 59549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case 3: 59649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; 59749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nav_timestamp = sensor_cal->quat.timestamp; 59849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 59949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow default: 60049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.nine_axis_status = 0; // Don't output quaternion related info 60149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 60249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 603cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#if MPL_LOG_9AXIS_DEBUG 604cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro MPL_LOGI("nav ts: %lld", hal_out.nav_timestamp); 605cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 60649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 60749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * So this is: 1 / 2^16*/ 60849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow #define COMPASS_CONVERSION 1.52587890625e-005f 60949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 61049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); 61149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.accuracy_mag = (int)accuracy; 61249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 61349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef CALIB_COMPASS_NOISE_REDUCTION 61449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; i++) { 61549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION; 61649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 61749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 61849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; i++) { 61949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == 62049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INV_NEW_DATA) { 62149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set the state variables to match output with input 62249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_calc_state_to_match_output(&hal_out.lp_filter[i], 62349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (float)compass[i]); 62449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 62549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == 62649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (INV_NEW_DATA | INV_RAW_DATA)) { 62749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.compass_float[i] = 62849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_biquad_filter_process(&hal_out.lp_filter[i], 62949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (float)compass[i]) * 63049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow COMPASS_CONVERSION; 63149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA) { 63249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION; 63349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 63449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 63549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif // CALIB_COMPASS_NOISE_REDUCTION 63649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 63749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 63849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 63949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turns off generation of HAL outputs. 64049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 64149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 64249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_stop_hal_outputs(void) 64349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 64449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 64549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_unregister_data_cb(inv_generate_hal_outputs); 64649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 64749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 64849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 64949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() 65049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). 65149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 65249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 65349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_start_hal_outputs(void) 65449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 65549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 65649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = 65749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_register_data_cb(inv_generate_hal_outputs, 65849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INV_PRIORITY_HAL_OUTPUTS, 659cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW | INV_QUAT_NEW | INV_PRESSURE_NEW); 66049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 66149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 66249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* file name: lowPassFilterCoeff_1_6.c */ 66449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat compass_low_pass_filter_coeff[5] = 66549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; 66649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Initializes hal outputs class. This is called automatically by the 66849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* enable function. It may be called any time the feature is enabled, but 66949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* is typically not needed to be called by outside callers. 67049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 67149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 67249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_init_hal_outputs(void) 67349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 67449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 67549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&hal_out, 0, sizeof(hal_out)); 67649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 67749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); 67849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 67949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 68149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 68249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turns on creation and storage of HAL type results. 68449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 68549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 68649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_enable_hal_outputs(void) 68749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 68849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 68949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // don't need to check the result for inv_init_hal_outputs 69149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // since it's always INV_SUCCESS 69249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_init_hal_outputs(); 69349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_register_mpl_start_notification(inv_start_hal_outputs); 69549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 69649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 69749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turns off creation and storage of HAL type results. 69949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 70049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_disable_hal_outputs(void) 70149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 70249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 70349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 70449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_stop_hal_outputs(); // Ignore error if we have already stopped this 70549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); 70649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 70749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 70849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 70949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 71049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @} 71149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 71249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 71349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 71449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 715