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