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