1c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 2c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $License: 3c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro See included License.txt for License information. 5c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $ 6c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 7c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 8c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 9c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @defgroup HAL_Outputs hal_outputs 10c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Motion Library - HAL Outputs 11c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Sets up common outputs for HAL 12c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 13c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @{ 14c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @file hal_outputs.c 15c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief HAL Outputs. 16c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 17c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 18c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#undef MPL_LOG_NDEBUG 19c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */ 20c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#undef MPL_LOG_TAG 21c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define MPL_LOG_TAG "MLLITE" 22c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro//#define MPL_LOG_9AXIS_DEBUG 1 23c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 24c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <string.h> 25c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 26c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "hal_outputs.h" 27c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "log.h" 28c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "ml_math_func.h" 29c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "mlmath.h" 30c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "start_manager.h" 31c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "data_builder.h" 32c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "results_holder.h" 33c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 34c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* commenting this define out will bypass the low pass filter noise reduction 35c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro filter for compass data. 36c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Disable this only for testing purpose (e.g. comparing the raw and calibrated 37c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro compass data, since the former is unfiltered and the latter is filtered, 38c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro leading to a small difference in the readings sample by sample). 39c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Android specifications require this filter to be enabled to have the Magnetic 40c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Field output's standard deviation fall below 0.5 uT. 41c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 42c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define CALIB_COMPASS_NOISE_REDUCTION 43c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 44c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct hal_output_t { 45c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int accuracy_mag; /**< Compass accuracy */ 46c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //int accuracy_gyro; /**< Gyro Accuracy */ 47c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //int accuracy_accel; /**< Accel Accuracy */ 48c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int accuracy_quat; /**< quat Accuracy */ 49c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 50c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t nav_timestamp; 51c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t gam_timestamp; 52c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //inv_time_t accel_timestamp; 53c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t mag_timestamp; 54c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long nav_quat[4]; 55c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int gyro_status; 56c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int accel_status; 57c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int compass_status; 58c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int nine_axis_status; 59c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int quat_status; 60c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_biquad_filter_t lp_filter[3]; 61c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float compass_float[3]; 62c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long linear_acceleration_sample_rate_us; 63c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long orientation_sample_rate_us; 64c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long rotation_vector_sample_rate_us; 65c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gravity_sample_rate_us; 66c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long orientation_6_axis_sample_rate_us; 67c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long orientation_geomagnetic_sample_rate_us; 68c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long rotation_vector_6_axis_sample_rate_us; 69c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long geomagnetic_rotation_vector_sample_rate_us; 70c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro}; 71c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 72c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic struct hal_output_t hal_out; 73c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 74c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_linear_acceleration_sample_rate(long sample_rate_us) 75c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 76c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.linear_acceleration_sample_rate_us = sample_rate_us; 77c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 78c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 79c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_orientation_sample_rate(long sample_rate_us) 80c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 81c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.orientation_sample_rate_us = sample_rate_us; 82c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 83c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 84c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_rotation_vector_sample_rate(long sample_rate_us) 85c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 86c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.rotation_vector_sample_rate_us = sample_rate_us; 87c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 88c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 89c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_gravity_sample_rate(long sample_rate_us) 90c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 91c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.gravity_sample_rate_us = sample_rate_us; 92c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 93c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 94c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_orientation_6_axis_sample_rate(long sample_rate_us) 95c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 96c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.orientation_6_axis_sample_rate_us = sample_rate_us; 97c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 98c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 99c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_orientation_geomagnetic_sample_rate(long sample_rate_us) 100c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 101c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us; 102c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 103c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 104c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_rotation_vector_6_axis_sample_rate(long sample_rate_us) 105c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 106c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.rotation_vector_6_axis_sample_rate_us = sample_rate_us; 107c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 108c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 109c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_geomagnetic_rotation_vector_sample_rate(long sample_rate_us) 110c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 111c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us; 112c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 113c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 114c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Acceleration (m/s^2) in body frame. 115c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it 116c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* should return a vector of magnitude near 9.81 m/s^2 117c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 118c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 119c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* inv_build_accel(). 120c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 121c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 122c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, 123c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 124c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 125c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 126c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. 127c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * So this 9.80665 / 2^16 */ 128c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define ACCEL_CONVERSION 0.000149637603759766f 129c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long accel[3]; 130c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(accel, accuracy, timestamp); 131c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = accel[0] * ACCEL_CONVERSION; 132c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = accel[1] * ACCEL_CONVERSION; 133c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = accel[2] * ACCEL_CONVERSION; 134c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.accel_status & INV_NEW_DATA) 135c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 1; 136c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 137c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 0; 138c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("accel values:%f %f %f -%d -%lld", values[0], values[1], 139c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2], status, *timestamp); 140c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 141c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 142c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 143c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Linear Acceleration (m/s^2) in Body Frame. 144c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show 145c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* accel biases while at rest. 146c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 147c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 148c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* inv_build_accel(). 149c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 150c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 151c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, 152c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 153c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 154c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gravity[3], accel[3]; 155c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp1; 156c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 157c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(accel, accuracy, ×tamp1); 158c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gravity(gravity); 159c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel[0] -= gravity[0] >> 14; 160c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel[1] -= gravity[1] >> 14; 161c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel[2] -= gravity[2] >> 14; 162c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = accel[0] * ACCEL_CONVERSION; 163c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = accel[1] * ACCEL_CONVERSION; 164c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = accel[2] * ACCEL_CONVERSION; 165c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 166c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp); 167c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 168c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 169c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gravity vector (m/s^2) in Body Frame. 170c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Gravity vector in body frame, length 3, (m/s^2) 171c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 172c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 173c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* inv_build_accel(). 174c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 175c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 176c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_gravity(float *values, int8_t *accuracy, 177c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 178c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 179c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gravity[3]; 180c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 181c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = (int8_t) hal_out.accuracy_quat; 182c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gravity(gravity); 183c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; 184c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; 185c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; 186c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 187c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp); 188c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 189c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 190c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. 191c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * So this is: pi / 2^16 / 180 */ 192c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define GYRO_CONVERSION 2.66316109007924e-007f 193c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 194c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gyroscope calibrated data (rad/s) in body frame. 195c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Rotation Rate in rad/sec. 196c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 197c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 198c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* inv_build_gyro(). 199c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 200c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 201c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, 202c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 203c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 204c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gyro[3]; 205c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 206c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 207c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gyro_set(gyro, accuracy, timestamp); 208c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 209c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = gyro[0] * GYRO_CONVERSION; 210c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = gyro[1] * GYRO_CONVERSION; 211c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = gyro[2] * GYRO_CONVERSION; 212c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.gyro_status & INV_NEW_DATA) 213c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 1; 214c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 215c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 0; 216c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 217c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 218c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 219c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gyroscope raw data (rad/s) in body frame. 220c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Rotation Rate in rad/sec. 221c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, 222c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* while 3 is most accurate. 223c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. Derived from the 224c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* timestamp sent to inv_build_gyro(). 225c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 226c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 227c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, 228c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 229c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 230c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gyro[3]; 231c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 232c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 233c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gyro_set_raw(gyro, accuracy, timestamp); 234c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = gyro[0] * GYRO_CONVERSION; 235c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = gyro[1] * GYRO_CONVERSION; 236c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = gyro[2] * GYRO_CONVERSION; 237c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.gyro_status & INV_NEW_DATA) 238c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 1; 239c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 240c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 0; 241c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 242c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 243c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 244c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 245c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* This corresponds to Sensor.TYPE_ROTATION_VECTOR. 246c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* The rotation vector represents the orientation of the device as a combination 247c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ 248c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* around an axis {x, y, z}. <br> 249c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* The three elements of the rotation vector are 250c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* {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 251c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is 252c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* equal to the direction of the axis of rotation. 253c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* 254c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* The three elements of the rotation vector are equal to the last three components of a unit quaternion 255c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* {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). 256c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* 257c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. 258c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* The reference coordinate system is defined as a direct orthonormal basis, where: 259c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* 260c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick 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). 261c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. 262c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* -Z points towards the sky and is perpendicular to the ground. 263c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values 264c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Length 5, 4th element being the w angle of the originating 4 265c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* elements quaternion and 5th element being the heading accuracy 266c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* at 95%. 267c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy is not defined 268c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp Timestamp. In (ns) for Android. 269c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 270c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 271c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, 272c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 273c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 274c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float quat_float[4]; 275c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = (int8_t) hal_out.accuracy_quat; 276c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_quaternion_float(quat_float); 277c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 278c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (quat_float[0] >= .0) { 279c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = quat_float[1]; 280c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = quat_float[2]; 281c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = quat_float[3]; 282c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = quat_float[0]; 283c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 284c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = -quat_float[1]; 285c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = -quat_float[2]; 286c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = -quat_float[3]; 287c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = -quat_float[0]; 288c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 289c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[4] = inv_get_heading_confidence_interval(); 290c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_9_axis_timestamp(hal_out.rotation_vector_sample_rate_us, timestamp); 291c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 292c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 293c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy, 294c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 295c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 296c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 297c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long accel[3]; 298c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float quat_6_axis[4]; 299c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp1; 300c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(accel, accuracy, ×tamp1); 301c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_6axis_quaternion_float(quat_6_axis, ×tamp1); 302c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 303c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (quat_6_axis[0] >= .0) { 304c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = quat_6_axis[1]; 305c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = quat_6_axis[2]; 306c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = quat_6_axis[3]; 307c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = quat_6_axis[0]; 308c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 309c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = -quat_6_axis[1]; 310c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = -quat_6_axis[2]; 311c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = -quat_6_axis[3]; 312c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = -quat_6_axis[0]; 313c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 314c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //This sensor does not report an estimated heading accuracy 315c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[4] = 0; 316c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.quat_status & INV_QUAT_3AXIS) 317c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 318c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = hal_out.quat_status & INV_NEW_DATA? 1 : 0; 319c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 320c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 321c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; 322c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 323c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("values:%f %f %f %f %f -%d -%lld", values[0], values[1], 324c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2], values[3], values[4], status, timestamp1); 325c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_gyro_accel_timestamp(hal_out.rotation_vector_6_axis_sample_rate_us, timestamp); 326c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 327c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 328c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 329c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* This corresponds to Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR. 330c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer 331c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* instead of using a gyroscope. 332c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Fourth element = estimated_accuracy in radians (heading confidence). 333c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Length 4. 334c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy is not defined. 335c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp in (ns) for Android. 336c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated, 0 otherwise. 337c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 338c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy, 339c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 340c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 341c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass[3]; 342c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float quat_geomagnetic[4]; 343c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 344c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp1; 345c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_compass_set(compass, accuracy, ×tamp1); 346c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_geomagnetic_quaternion_float(quat_geomagnetic, ×tamp1); 347c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 348c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (quat_geomagnetic[0] >= .0) { 349c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = quat_geomagnetic[1]; 350c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = quat_geomagnetic[2]; 351c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = quat_geomagnetic[3]; 352c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = quat_geomagnetic[0]; 353c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 354c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[0] = -quat_geomagnetic[1]; 355c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[1] = -quat_geomagnetic[2]; 356c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2] = -quat_geomagnetic[3]; 357c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[3] = -quat_geomagnetic[0]; 358c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 359c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[4] = inv_get_accel_compass_confidence_interval(); 360c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; 361c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("values:%f %f %f %f %f -%d", values[0], values[1], 362c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[2], values[3], values[4], status); 363c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 364c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_compass_accel_timestamp(hal_out.geomagnetic_rotation_vector_sample_rate_us, timestamp); 365c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 366c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 367c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Compass data (uT) in body frame. 368c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Compass data in (uT), length 3. May be calibrated by having 369c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* biases removed and sensitivity adjusted 370c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 371c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp Timestamp. In (ns) for Android. 372c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 373c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 374c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, 375c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 376c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 377c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 378c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 379c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 380c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * So this is: 1 / 2^16*/ 381c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro//#define COMPASS_CONVERSION 1.52587890625e-005f 382c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 383c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = hal_out.mag_timestamp; 384c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = (int8_t) hal_out.accuracy_mag; 385c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 386c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; i++) 387c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[i] = hal_out.compass_float[i]; 388c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.compass_status & INV_NEW_DATA) 389c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 1; 390c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 391c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 0; 392c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 393c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 394c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 395c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Compass raw data (uT) in body frame. 396c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Compass data in (uT), length 3. May be calibrated by having 397c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* biases removed and sensitivity adjusted 398c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 399c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp Timestamp. In (ns) for Android. 400c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 401c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 402c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy, 403c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 404c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 405c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long mag[3]; 406c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int status; 407c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 408c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 409c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_compass_set_raw(mag, accuracy, timestamp); 410c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 411c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 412c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * So this is: 1 / 2^16*/ 413c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define COMPASS_CONVERSION 1.52587890625e-005f 414c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 415c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; i++) { 416c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro values[i] = (float)mag[i] * COMPASS_CONVERSION; 417c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 418c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hal_out.compass_status & INV_NEW_DATA) 419c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 1; 420c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 421c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = 0; 422c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 423c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 424c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void inv_get_rotation_geomagnetic(float r[3][3]) 425c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 426c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long rot[9], quat_geo[4]; 427c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float conv = 1.f / (1L<<30); 428c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp; 429c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_geomagnetic_quaternion(quat_geo, ×tamp); 430c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_quaternion_to_rotation(quat_geo, rot); 431c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][0] = rot[0]*conv; 432c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][1] = rot[1]*conv; 433c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][2] = rot[2]*conv; 434c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][0] = rot[3]*conv; 435c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][1] = rot[4]*conv; 436c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][2] = rot[5]*conv; 437c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][0] = rot[6]*conv; 438c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][1] = rot[7]*conv; 439c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][2] = rot[8]*conv; 440c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 441c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void google_orientation_geomagnetic(float *g) 442c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 443c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float rad2deg = (float)(180.0 / M_PI); 444c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float R[3][3]; 445c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_rotation_geomagnetic(R); 446c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 447c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 448c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[2] = asinf ( R[2][0]) * rad2deg; 449c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (g[0] < 0) 450c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] += 360; 451c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 452c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 453c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void inv_get_rotation_6_axis(float r[3][3]) 454c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 455c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long rot[9], quat_6_axis[4]; 456c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float conv = 1.f / (1L<<30); 457c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp; 458c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 459c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_6axis_quaternion(quat_6_axis, ×tamp); 460c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_quaternion_to_rotation(quat_6_axis, rot); 461c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][0] = rot[0]*conv; 462c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][1] = rot[1]*conv; 463c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][2] = rot[2]*conv; 464c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][0] = rot[3]*conv; 465c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][1] = rot[4]*conv; 466c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][2] = rot[5]*conv; 467c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][0] = rot[6]*conv; 468c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][1] = rot[7]*conv; 469c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][2] = rot[8]*conv; 470c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 471c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 472c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void google_orientation_6_axis(float *g) 473c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 474c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float rad2deg = (float)(180.0 / M_PI); 475c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float R[3][3]; 476c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 477c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_rotation_6_axis(R); 478c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 479c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 480c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 481c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[2] = asinf ( R[2][0]) * rad2deg; 482c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (g[0] < 0) 483c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] += 360; 484c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 485c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 486c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void inv_get_rotation(float r[3][3]) 487c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 488c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long rot[9]; 489c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float conv = 1.f / (1L<<30); 490c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 491c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_quaternion_to_rotation(hal_out.nav_quat, rot); 492c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][0] = rot[0]*conv; 493c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][1] = rot[1]*conv; 494c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[0][2] = rot[2]*conv; 495c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][0] = rot[3]*conv; 496c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][1] = rot[4]*conv; 497c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[1][2] = rot[5]*conv; 498c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][0] = rot[6]*conv; 499c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][1] = rot[7]*conv; 500c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r[2][2] = rot[8]*conv; 501c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 502c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 503c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void google_orientation(float *g) 504c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 505c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float rad2deg = (float)(180.0 / M_PI); 506c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float R[3][3]; 507c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 508c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_rotation(R); 509c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 510c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 511c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 512c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[2] = asinf ( R[2][0]) * rad2deg; 513c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (g[0] < 0) 514c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g[0] += 360; 515c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 516c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 517c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. 518c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] values Length 3, Degrees.<br> 519c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* - values[0]: Azimuth, angle between the magnetic north direction 520c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> 521c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values 522c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* when the z-axis moves toward the y-axis.<br> 523c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive 524c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* values when the x-axis moves toward the z-axis.<br> 525c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* 526c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @note This definition is different from yaw, pitch and roll used in aviation 527c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* where the X axis is along the long side of the plane (tail to nose). 528c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() 529c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* in conjunction with remapCoordinateSystem() and getOrientation() to compute 530c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* these values instead. 531c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Important note: For historical reasons the roll angle is positive in the 532c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* clockwise direction (mathematically speaking, it should be positive in 533c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* the counter-clockwise direction). 534c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 535c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp The timestamp for this sensor. 536c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns 1 if the data was updated or 0 if it was not updated. 537c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 538c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_orientation(float *values, int8_t *accuracy, 539c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 540c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 541c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = (int8_t) hal_out.accuracy_quat; 542c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro google_orientation(values); 543c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 544c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_9_axis_timestamp(hal_out.orientation_sample_rate_us, timestamp); 545c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 546c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 547c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy, 548c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 549c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 550c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long accel[3]; 551c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp1; 552c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(accel, accuracy, ×tamp1); 553c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 554c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro google_orientation_6_axis(values); 555c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 556c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_gyro_accel_timestamp(hal_out.orientation_6_axis_sample_rate_us, timestamp); 557c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 558c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 559c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy, 560c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t * timestamp) 561c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 562c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass[3]; 563c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t timestamp1; 564c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_compass_set(compass, accuracy, ×tamp1); 565c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 566c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro google_orientation_geomagnetic(values); 567c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 568c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return inv_get_6_axis_compass_accel_timestamp(hal_out.orientation_geomagnetic_sample_rate_us, timestamp); 569c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 570c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 571c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Main callback to generate HAL outputs. Typically not called by library users. 572c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] sensor_cal Input variable to take sensor data whenever there is new 573c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* sensor data. 574c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 575c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 576c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) 577c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 578c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int use_sensor = 0; 579c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long sr = 1000; 580c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass[3]; 581c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int8_t accuracy; 582c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 583c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (void) sensor_cal; 584c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 585c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, 586c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro &hal_out.nav_timestamp); 587c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.gyro_status = sensor_cal->gyro.status; 588c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.accel_status = sensor_cal->accel.status; 589c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.compass_status = sensor_cal->compass.status; 590c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.quat_status = sensor_cal->quat.status; 591c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#if MPL_LOG_9AXIS_DEBUG 592c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("hal_out:g=%d", hal_out.gyro_status); 593c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 594c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Find the highest sample rate and tie generating 9-axis to that one. 595c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->gyro.status & INV_SENSOR_ON) { 596c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sr = sensor_cal->gyro.sample_rate_ms; 597c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = 0; 598c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 599c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { 600c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sr = sensor_cal->accel.sample_rate_ms; 601c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = 1; 602c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 603c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { 604c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sr = sensor_cal->compass.sample_rate_ms; 605c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = 2; 606c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 607c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { 608c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sr = sensor_cal->quat.sample_rate_ms; 609c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = 3; 610c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 611c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 612c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // If the timestamp did not change, remove the new data flag 613c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->gyro.timestamp_prev == sensor_cal->gyro.timestamp) { 614c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.gyro_status &= ~INV_NEW_DATA; 615c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 616c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->accel.timestamp_prev == sensor_cal->accel.timestamp) { 617c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.accel_status &= ~INV_NEW_DATA; 618c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 619c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->compass.timestamp_prev == sensor_cal->compass.timestamp) { 620c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.compass_status &= ~INV_NEW_DATA; 621c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 622c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->quat.timestamp_prev == sensor_cal->quat.timestamp) { 623c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.quat_status &= ~INV_NEW_DATA; 624c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 625c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 626c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Only output 9-axis if all 9 sensors are on. 627c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (sensor_cal->quat.status & INV_SENSOR_ON) { 628c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // If quaternion sensor is on, gyros are not required as quaternion already has that part 629c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 630c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = -1; 631c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 632c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 633c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 634c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro use_sensor = -1; 635c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 636c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 637c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#if MPL_LOG_9AXIS_DEBUG 638c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGI("use_sensor=%d", use_sensor); 639c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 640c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (use_sensor) { 641c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 0: 642c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; 643c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 644c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 645c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 1: 646c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; 647c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nav_timestamp = sensor_cal->accel.timestamp; 648c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 649c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 2: 650c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; 651c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nav_timestamp = sensor_cal->compass.timestamp; 652c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 653c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 3: 654c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; 655c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nav_timestamp = sensor_cal->quat.timestamp; 656c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 657c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 658c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.nine_axis_status = 0; // Don't output quaternion related info 659c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 660c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 661c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#if MPL_LOG_9AXIS_DEBUG 662c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGI("nav ts: %lld", hal_out.nav_timestamp); 663c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 664c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 665c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * So this is: 1 / 2^16*/ 666c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro #define COMPASS_CONVERSION 1.52587890625e-005f 667c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 668c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); 669c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.accuracy_mag = (int)accuracy; 670c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 671c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifndef CALIB_COMPASS_NOISE_REDUCTION 672c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; i++) { 673c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION; 674c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 675c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#else 676c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; i++) { 677c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == 678c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_NEW_DATA) { 679c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // set the state variables to match output with input 680c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_calc_state_to_match_output(&hal_out.lp_filter[i], 681c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (float)compass[i]); 682c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 683c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == 684c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (INV_NEW_DATA | INV_RAW_DATA)) { 685c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.compass_float[i] = 686c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_biquad_filter_process(&hal_out.lp_filter[i], 687c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (float)compass[i]) * 688c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro COMPASS_CONVERSION; 689c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA) { 690c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION; 691c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 692c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 693c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif // CALIB_COMPASS_NOISE_REDUCTION 694c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 695c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 696c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 697c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns off generation of HAL outputs. 698c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 699c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 700c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_stop_hal_outputs(void) 701c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 702c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 703c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_unregister_data_cb(inv_generate_hal_outputs); 704c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 705c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 706c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 707c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() 708c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). 709c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 710c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 711c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_start_hal_outputs(void) 712c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 713c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 714c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = 715c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_register_data_cb(inv_generate_hal_outputs, 716c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_PRIORITY_HAL_OUTPUTS, 717c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW | INV_QUAT_NEW | INV_PRESSURE_NEW); 718c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 719c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 720c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 721c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* file name: lowPassFilterCoeff_1_6.c */ 722c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarofloat compass_low_pass_filter_coeff[5] = 723c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; 724c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 725c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Initializes hal outputs class. This is called automatically by the 726c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* enable function. It may be called any time the feature is enabled, but 727c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* is typically not needed to be called by outside callers. 728c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 729c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 730c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_init_hal_outputs(void) 731c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 732c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 733c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memset(&hal_out, 0, sizeof(hal_out)); 734c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i=0; i<3; i++) { 735c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); 736c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 737c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 738c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 739c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 740c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 741c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns on creation and storage of HAL type results. 742c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 743c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 744c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_enable_hal_outputs(void) 745c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 746c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 747c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 748c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // don't need to check the result for inv_init_hal_outputs 749c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // since it's always INV_SUCCESS 750c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_init_hal_outputs(); 751c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 752c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_register_mpl_start_notification(inv_start_hal_outputs); 753c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 754c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 755c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 756c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns off creation and storage of HAL type results. 757c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 758c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_disable_hal_outputs(void) 759c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 760c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 761c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 762c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_stop_hal_outputs(); // Ignore error if we have already stopped this 763c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); 764c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 765c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 766c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 767c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 768c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @} 769c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 770c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 771c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 772c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 773