133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/* 233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall $License: 333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall See included License.txt for License information. 533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall $ 633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall */ 733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** 933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @defgroup HAL_Outputs hal_outputs 1033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @brief Motion Library - HAL Outputs 1133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * Sets up common outputs for HAL 1233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * 1333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @{ 148504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall * @file hal_outputs.c 1533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @brief HAL Outputs. 1633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall */ 178504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 188504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall#include <string.h> 198504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 2033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "hal_outputs.h" 2133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "log.h" 2233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "ml_math_func.h" 2333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "mlmath.h" 2433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "start_manager.h" 2533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "data_builder.h" 2633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#include "results_holder.h" 2733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 2833ce91b37062fa63af192f5643de93f3beebe854JP Abgrallstruct hal_output_t { 2933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int accuracy_mag; /**< Compass accuracy */ 3033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall// int accuracy_gyro; /**< Gyro Accuracy */ 3133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall// int accuracy_accel; /**< Accel Accuracy */ 3233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int accuracy_quat; /**< quat Accuracy */ 3333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 3433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t nav_timestamp; 3533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t gam_timestamp; 3633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall// inv_time_t accel_timestamp; 3733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t mag_timestamp; 3833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long nav_quat[4]; 3933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int gyro_status; 4033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int accel_status; 4133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int compass_status; 4233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int nine_axis_status; 4333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_biquad_filter_t lp_filter[3]; 4433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float compass_float[3]; 4533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall}; 4633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 4733ce91b37062fa63af192f5643de93f3beebe854JP Abgrallstatic struct hal_output_t hal_out; 4833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 4933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Acceleration (m/s^2) in body frame. 5033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it 5133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* should return a vector of magnitude near 9.81 m/s^2 5233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 5333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 5433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* inv_build_accel(). 5533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 5633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 5733ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, 5833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 5933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 6033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int status; 6133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. 6233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * So this 9.80665 / 2^16 */ 6333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall#define ACCEL_CONVERSION 0.000149637603759766f 6433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long accel[3]; 6533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_accel_set(accel, accuracy, timestamp); 6633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = accel[0] * ACCEL_CONVERSION; 6733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = accel[1] * ACCEL_CONVERSION; 6833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = accel[2] * ACCEL_CONVERSION; 6933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (hal_out.accel_status & INV_NEW_DATA) 7033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 1; 7133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall else 7233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 0; 7333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return status; 7433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 7533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 7633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Linear Acceleration (m/s^2) in Body Frame. 7733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show 7833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* accel biases while at rest. 7933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 8033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 8133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* inv_build_accel(). 8233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 8333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 8433ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, 8533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 8633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 8733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long gravity[3], accel[3]; 8833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 8933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_accel_set(accel, accuracy, timestamp); 9033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_gravity(gravity); 9133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall accel[0] -= gravity[0] >> 14; 9233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall accel[1] -= gravity[1] >> 14; 9333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall accel[2] -= gravity[2] >> 14; 9433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = accel[0] * ACCEL_CONVERSION; 9533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = accel[1] * ACCEL_CONVERSION; 9633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = accel[2] * ACCEL_CONVERSION; 9733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 9833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return hal_out.nine_axis_status; 9933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 10033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 10133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Gravity vector (m/s^2) in Body Frame. 10233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Gravity vector in body frame, length 3, (m/s^2) 10333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 10433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 10533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* inv_build_accel(). 10633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 10733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 10833ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_gravity(float *values, int8_t *accuracy, 10933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 11033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 11133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long gravity[3]; 11233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int status; 11333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 11433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *accuracy = (int8_t) hal_out.accuracy_quat; 11533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *timestamp = hal_out.nav_timestamp; 11633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_gravity(gravity); 11733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; 11833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; 11933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; 12033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) 12133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 1; 12233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall else 12333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 0; 12433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return status; 12533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 12633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 1278504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. 1288504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall * So this is: pi / 2^16 / 180 */ 1298504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall#define GYRO_CONVERSION 2.66316109007924e-007f 1308504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 13133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Gyroscope calibrated data (rad/s) in body frame. 13233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Rotation Rate in rad/sec. 13333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 13433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 13533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* inv_build_gyro(). 13633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 13733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 13833ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, 13933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 14033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 14133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long gyro[3]; 14233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int status; 14333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 14433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_gyro_set(gyro, accuracy, timestamp); 14533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = gyro[0] * GYRO_CONVERSION; 14633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = gyro[1] * GYRO_CONVERSION; 14733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = gyro[2] * GYRO_CONVERSION; 14833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (hal_out.gyro_status & INV_NEW_DATA) 14933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 1; 15033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall else 15133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 0; 15233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return status; 15333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 15433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 15533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Gyroscope raw data (rad/s) in body frame. 15633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Rotation Rate in rad/sec. 15733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 15833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 15933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* inv_build_gyro(). 16033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 16133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 16233ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, 16333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 16433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 16533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long gyro[3]; 16633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int status; 16733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 16833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_gyro_set_raw(gyro, accuracy, timestamp); 16933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = gyro[0] * GYRO_CONVERSION; 17033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = gyro[1] * GYRO_CONVERSION; 17133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = gyro[2] * GYRO_CONVERSION; 17233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (hal_out.gyro_status & INV_NEW_DATA) 17333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 1; 17433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall else 17533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 0; 17633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return status; 17733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 17833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 17933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** 18033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* This corresponds to Sensor.TYPE_ROTATION_VECTOR. 18133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* The rotation vector represents the orientation of the device as a combination 18233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ 18333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* around an axis {x, y, z}. <br> 18433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* The three elements of the rotation vector are 18533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* {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 18633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is 18733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* equal to the direction of the axis of rotation. 18833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* 18933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* The three elements of the rotation vector are equal to the last three components of a unit quaternion 19033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* {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). 19133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* 19233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. 19333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* The reference coordinate system is defined as a direct orthonormal basis, where: 19433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 19533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall -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). 19633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. 19733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall -Z points towards the sky and is perpendicular to the ground. 19833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Length 4. 19933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 20033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp Timestamp. In (ns) for Android. 20133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 20233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 20333ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, 20433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 20533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 20633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *accuracy = (int8_t) hal_out.accuracy_quat; 20733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *timestamp = hal_out.nav_timestamp; 20833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 20933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (hal_out.nav_quat[0] >= 0) { 21033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 21133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 21233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 21333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 21433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } else { 21533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 21633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 21733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 21833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 21933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 22033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[4] = inv_get_heading_confidence_interval(); 22133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 22233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return hal_out.nine_axis_status; 22333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 22433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 22533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 22633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Compass data (uT) in body frame. 22733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Compass data in (uT), length 3. May be calibrated by having 22833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* biases removed and sensitivity adjusted 22933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 23033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp Timestamp. In (ns) for Android. 23133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 23233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 23333ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, 23433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 23533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 23633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int status; 23733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 23833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * So this is: 1 / 2^16*/ 23933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall//#define COMPASS_CONVERSION 1.52587890625e-005f 24033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int i; 24133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 24233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *timestamp = hal_out.mag_timestamp; 24333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *accuracy = (int8_t) hal_out.accuracy_mag; 24433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 24533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall for (i=0; i<3; i++) { 24633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall values[i] = hal_out.compass_float[i]; 24733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 24833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (hal_out.compass_status & INV_NEW_DATA) 24933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 1; 25033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall else 25133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall status = 0; 25233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return status; 25333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 25433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 25533ce91b37062fa63af192f5643de93f3beebe854JP Abgrallstatic void inv_get_rotation(float r[3][3]) 25633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 25733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long rot[9]; 25833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float conv = 1.f / (1L<<30); 25933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 26033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_quaternion_to_rotation(hal_out.nav_quat, rot); 26133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[0][0] = rot[0]*conv; 26233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[0][1] = rot[1]*conv; 26333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[0][2] = rot[2]*conv; 26433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[1][0] = rot[3]*conv; 26533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[1][1] = rot[4]*conv; 26633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[1][2] = rot[5]*conv; 26733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[2][0] = rot[6]*conv; 26833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[2][1] = rot[7]*conv; 26933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall r[2][2] = rot[8]*conv; 27033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 27133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 27233ce91b37062fa63af192f5643de93f3beebe854JP Abgrallstatic void google_orientation(float *g) 27333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 27433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float rad2deg = (float)(180.0 / M_PI); 27533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float R[3][3]; 27633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 27733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_rotation(R); 27833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 27933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 28033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 28133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall g[2] = asinf ( R[2][0]) * rad2deg; 28233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (g[0] < 0) 28333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall g[0] += 360; 28433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 28533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 28633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 28733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. 28833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] values Length 3, Degrees.<br> 28933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* - values[0]: Azimuth, angle between the magnetic north direction 29033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> 29133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values 29233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* when the z-axis moves toward the y-axis.<br> 29333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive 29433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* values when the x-axis moves toward the z-axis.<br> 29533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* 29633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @note This definition is different from yaw, pitch and roll used in aviation 29733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* where the X axis is along the long side of the plane (tail to nose). 29833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() 29933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* in conjunction with remapCoordinateSystem() and getOrientation() to compute 30033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* these values instead. 30133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* Important note: For historical reasons the roll angle is positive in the 30233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* clockwise direction (mathematically speaking, it should be positive in 30333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* the counter-clockwise direction). 30433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 30533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[out] timestamp The timestamp for this sensor. 30633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns 1 if the data was updated or 0 if it was not updated. 30733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 30833ce91b37062fa63af192f5643de93f3beebe854JP Abgrallint inv_get_sensor_type_orientation(float *values, int8_t *accuracy, 30933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_time_t * timestamp) 31033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 31133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *accuracy = (int8_t) hal_out.accuracy_quat; 31233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall *timestamp = hal_out.nav_timestamp; 31333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 31433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall google_orientation(values); 31533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 31633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return hal_out.nine_axis_status; 31733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 31833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 31933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Main callback to generate HAL outputs. Typically not called by library users. 32033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[in] sensor_cal Input variable to take sensor data whenever there is new 32133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* sensor data. 32233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 32333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 32433ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) 32533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 32633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int use_sensor = 0; 32733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long sr = 1000; 32833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall long compass[3]; 32933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int8_t accuracy; 33033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int i; 33133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall (void) sensor_cal; 33233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 33333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, 33433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall &hal_out.nav_timestamp); 33533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.gyro_status = sensor_cal->gyro.status; 33633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.accel_status = sensor_cal->accel.status; 33733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.compass_status = sensor_cal->compass.status; 33833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 33933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // Find the highest sample rate and tie generating 9-axis to that one. 34033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if (sensor_cal->gyro.status & INV_SENSOR_ON) { 34133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sr = sensor_cal->gyro.sample_rate_ms; 34233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall use_sensor = 0; 34333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 34433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { 34533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sr = sensor_cal->accel.sample_rate_ms; 34633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall use_sensor = 1; 34733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 34833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { 34933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sr = sensor_cal->compass.sample_rate_ms; 35033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall use_sensor = 2; 35133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 35233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { 35333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall sr = sensor_cal->quat.sample_rate_ms; 35433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall use_sensor = 3; 35533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 35633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 3578504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall // Only output 9-axis if all 9 sensors are on. 3588504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall if (sensor_cal->quat.status & INV_SENSOR_ON) { 3598504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall // If quaternion sensor is on, gyros are not required as quaternion already has that part 3608504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 3618504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall use_sensor = -1; 3628504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall } 3638504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall } else { 3648504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 3658504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall use_sensor = -1; 3668504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall } 3678504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall } 3688504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall 36933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall switch (use_sensor) { 37033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall case 0: 37133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; 37233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 37333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall break; 37433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall case 1: 37533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; 37633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nav_timestamp = sensor_cal->accel.timestamp; 37733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall break; 37833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall case 2: 37933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; 38033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nav_timestamp = sensor_cal->compass.timestamp; 38133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall break; 38233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall case 3: 38333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; 38433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.nav_timestamp = sensor_cal->quat.timestamp; 38533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall break; 3868504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall default: 3878504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall hal_out.nine_axis_status = 0; // Don't output quaternion related info 3888504ee554e5ca7014b3160b1cbeb4506e231338bJP Abgrall break; 38933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 39033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 39133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 39233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * So this is: 1 / 2^16*/ 39333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall #define COMPASS_CONVERSION 1.52587890625e-005f 39433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 39533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); 39633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.accuracy_mag = (int ) accuracy; 39733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 39833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall for (i=0; i<3; i++) { 39933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == 40033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall INV_NEW_DATA ) { 40133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // set the state variables to match output with input 40233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]); 40333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 40433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 40533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == 40633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall (INV_NEW_DATA | INV_RAW_DATA) ) { 40733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 40833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i], 40933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall (float ) compass[i]) * COMPASS_CONVERSION; 41033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 41133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) { 41233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION; 41333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 41433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 41533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 41633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return INV_SUCCESS; 41733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 41833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 41933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Turns off generation of HAL outputs. 42033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 42133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall */ 42233ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_stop_hal_outputs(void) 42333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 42433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_error_t result; 42533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall result = inv_unregister_data_cb(inv_generate_hal_outputs); 42633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return result; 42733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 42833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 42933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() 43033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). 43133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 43233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 43333ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_start_hal_outputs(void) 43433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 43533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_error_t result; 43633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall result = 43733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_register_data_cb(inv_generate_hal_outputs, 43833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall INV_PRIORITY_HAL_OUTPUTS, 43933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 44033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return result; 44133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 44233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/* file name: lowPassFilterCoeff_1_6.c */ 44333ce91b37062fa63af192f5643de93f3beebe854JP Abgrallfloat compass_low_pass_filter_coeff[5] = 44433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; 44533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 44633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Initializes hal outputs class. This is called automatically by the 44733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* enable function. It may be called any time the feature is enabled, but 44833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* is typically not needed to be called by outside callers. 44933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 45033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 45133ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_init_hal_outputs(void) 45233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 45333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int i; 45433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall memset(&hal_out, 0, sizeof(hal_out)); 45533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall for (i=0; i<3; i++) { 45633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); 45733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 45833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 45933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return INV_SUCCESS; 46033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 46133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 46233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Turns on creation and storage of HAL type results. 46333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 46433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 46533ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_enable_hal_outputs(void) 46633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 46733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_error_t result; 46833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 46933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // don't need to check the result for inv_init_hal_outputs 47033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // since it's always INV_SUCCESS 47133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_init_hal_outputs(); 47233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 47333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall result = inv_register_mpl_start_notification(inv_start_hal_outputs); 47433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return result; 47533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 47633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 47733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** Turns off creation and storage of HAL type results. 47833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall*/ 47933ce91b37062fa63af192f5643de93f3beebe854JP Abgrallinv_error_t inv_disable_hal_outputs(void) 48033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 48133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_error_t result; 48233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 48333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall inv_stop_hal_outputs(); // Ignore error if we have already stopped this 48433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); 48533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return result; 48633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 48733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 48833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall/** 48933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall * @} 49033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall */ 491