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#include <string.h> 19 20#include "hal_outputs.h" 21#include "log.h" 22#include "ml_math_func.h" 23#include "mlmath.h" 24#include "start_manager.h" 25#include "data_builder.h" 26#include "results_holder.h" 27 28struct hal_output_t { 29 int accuracy_mag; /**< Compass accuracy */ 30// int accuracy_gyro; /**< Gyro Accuracy */ 31// int accuracy_accel; /**< Accel Accuracy */ 32 int accuracy_quat; /**< quat Accuracy */ 33 34 inv_time_t nav_timestamp; 35 inv_time_t gam_timestamp; 36// inv_time_t accel_timestamp; 37 inv_time_t mag_timestamp; 38 long nav_quat[4]; 39 int gyro_status; 40 int accel_status; 41 int compass_status; 42 int nine_axis_status; 43 inv_biquad_filter_t lp_filter[3]; 44 float compass_float[3]; 45}; 46 47static struct hal_output_t hal_out; 48 49/** Acceleration (m/s^2) in body frame. 50* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it 51* should return a vector of magnitude near 9.81 m/s^2 52* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 53* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 54* inv_build_accel(). 55* @return Returns 1 if the data was updated or 0 if it was not updated. 56*/ 57int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, 58 inv_time_t * timestamp) 59{ 60 int status; 61 /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. 62 * So this 9.80665 / 2^16 */ 63#define ACCEL_CONVERSION 0.000149637603759766f 64 long accel[3]; 65 inv_get_accel_set(accel, accuracy, timestamp); 66 values[0] = accel[0] * ACCEL_CONVERSION; 67 values[1] = accel[1] * ACCEL_CONVERSION; 68 values[2] = accel[2] * ACCEL_CONVERSION; 69 if (hal_out.accel_status & INV_NEW_DATA) 70 status = 1; 71 else 72 status = 0; 73 return status; 74} 75 76/** Linear Acceleration (m/s^2) in Body Frame. 77* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show 78* accel biases while at rest. 79* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 80* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 81* inv_build_accel(). 82* @return Returns 1 if the data was updated or 0 if it was not updated. 83*/ 84int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, 85 inv_time_t * timestamp) 86{ 87 long gravity[3], accel[3]; 88 89 inv_get_accel_set(accel, accuracy, timestamp); 90 inv_get_gravity(gravity); 91 accel[0] -= gravity[0] >> 14; 92 accel[1] -= gravity[1] >> 14; 93 accel[2] -= gravity[2] >> 14; 94 values[0] = accel[0] * ACCEL_CONVERSION; 95 values[1] = accel[1] * ACCEL_CONVERSION; 96 values[2] = accel[2] * ACCEL_CONVERSION; 97 98 return hal_out.nine_axis_status; 99} 100 101/** Gravity vector (m/s^2) in Body Frame. 102* @param[out] values Gravity vector in body frame, length 3, (m/s^2) 103* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 104* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 105* inv_build_accel(). 106* @return Returns 1 if the data was updated or 0 if it was not updated. 107*/ 108int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, 109 inv_time_t * timestamp) 110{ 111 long gravity[3]; 112 int status; 113 114 *accuracy = (int8_t) hal_out.accuracy_quat; 115 *timestamp = hal_out.nav_timestamp; 116 inv_get_gravity(gravity); 117 values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; 118 values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; 119 values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; 120 if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) 121 status = 1; 122 else 123 status = 0; 124 return status; 125} 126 127/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. 128 * So this is: pi / 2^16 / 180 */ 129#define GYRO_CONVERSION 2.66316109007924e-007f 130 131/** Gyroscope calibrated data (rad/s) in body frame. 132* @param[out] values Rotation Rate in rad/sec. 133* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 134* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to 135* inv_build_gyro(). 136* @return Returns 1 if the data was updated or 0 if it was not updated. 137*/ 138int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, 139 inv_time_t * timestamp) 140{ 141 long gyro[3]; 142 int status; 143 144 inv_get_gyro_set(gyro, accuracy, timestamp); 145 values[0] = gyro[0] * GYRO_CONVERSION; 146 values[1] = gyro[1] * GYRO_CONVERSION; 147 values[2] = gyro[2] * GYRO_CONVERSION; 148 if (hal_out.gyro_status & INV_NEW_DATA) 149 status = 1; 150 else 151 status = 0; 152 return status; 153} 154 155/** Gyroscope raw 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_raw(float *values, int8_t *accuracy, 163 inv_time_t * timestamp) 164{ 165 long gyro[3]; 166 int status; 167 168 inv_get_gyro_set_raw(gyro, accuracy, timestamp); 169 values[0] = gyro[0] * GYRO_CONVERSION; 170 values[1] = gyro[1] * GYRO_CONVERSION; 171 values[2] = gyro[2] * GYRO_CONVERSION; 172 if (hal_out.gyro_status & INV_NEW_DATA) 173 status = 1; 174 else 175 status = 0; 176 return status; 177} 178 179/** 180* This corresponds to Sensor.TYPE_ROTATION_VECTOR. 181* The rotation vector represents the orientation of the device as a combination 182* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ 183* around an axis {x, y, z}. <br> 184* The three elements of the rotation vector are 185* {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 186* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is 187* equal to the direction of the axis of rotation. 188* 189* The three elements of the rotation vector are equal to the last three components of a unit quaternion 190* {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). 191* 192* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. 193* The reference coordinate system is defined as a direct orthonormal basis, where: 194 195 -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). 196 -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. 197 -Z points towards the sky and is perpendicular to the ground. 198* @param[out] values Length 4. 199* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 200* @param[out] timestamp Timestamp. In (ns) for Android. 201* @return Returns 1 if the data was updated or 0 if it was not updated. 202*/ 203int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, 204 inv_time_t * timestamp) 205{ 206 *accuracy = (int8_t) hal_out.accuracy_quat; 207 *timestamp = hal_out.nav_timestamp; 208 209 if (hal_out.nav_quat[0] >= 0) { 210 values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 211 values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 212 values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 213 values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 214 } else { 215 values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; 216 values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; 217 values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; 218 values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; 219 } 220 values[4] = inv_get_heading_confidence_interval(); 221 222 return hal_out.nine_axis_status; 223} 224 225 226/** Compass data (uT) in body frame. 227* @param[out] values Compass data in (uT), length 3. May be calibrated by having 228* biases removed and sensitivity adjusted 229* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 230* @param[out] timestamp Timestamp. In (ns) for Android. 231* @return Returns 1 if the data was updated or 0 if it was not updated. 232*/ 233int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, 234 inv_time_t * timestamp) 235{ 236 int status; 237 /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 238 * So this is: 1 / 2^16*/ 239//#define COMPASS_CONVERSION 1.52587890625e-005f 240 int i; 241 242 *timestamp = hal_out.mag_timestamp; 243 *accuracy = (int8_t) hal_out.accuracy_mag; 244 245 for (i=0; i<3; i++) { 246 values[i] = hal_out.compass_float[i]; 247 } 248 if (hal_out.compass_status & INV_NEW_DATA) 249 status = 1; 250 else 251 status = 0; 252 return status; 253} 254 255static void inv_get_rotation(float r[3][3]) 256{ 257 long rot[9]; 258 float conv = 1.f / (1L<<30); 259 260 inv_quaternion_to_rotation(hal_out.nav_quat, rot); 261 r[0][0] = rot[0]*conv; 262 r[0][1] = rot[1]*conv; 263 r[0][2] = rot[2]*conv; 264 r[1][0] = rot[3]*conv; 265 r[1][1] = rot[4]*conv; 266 r[1][2] = rot[5]*conv; 267 r[2][0] = rot[6]*conv; 268 r[2][1] = rot[7]*conv; 269 r[2][2] = rot[8]*conv; 270} 271 272static void google_orientation(float *g) 273{ 274 float rad2deg = (float)(180.0 / M_PI); 275 float R[3][3]; 276 277 inv_get_rotation(R); 278 279 g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 280 g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 281 g[2] = asinf ( R[2][0]) * rad2deg; 282 if (g[0] < 0) 283 g[0] += 360; 284} 285 286 287/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. 288* @param[out] values Length 3, Degrees.<br> 289* - values[0]: Azimuth, angle between the magnetic north direction 290* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> 291* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values 292* when the z-axis moves toward the y-axis.<br> 293* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive 294* values when the x-axis moves toward the z-axis.<br> 295* 296* @note This definition is different from yaw, pitch and roll used in aviation 297* where the X axis is along the long side of the plane (tail to nose). 298* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() 299* in conjunction with remapCoordinateSystem() and getOrientation() to compute 300* these values instead. 301* Important note: For historical reasons the roll angle is positive in the 302* clockwise direction (mathematically speaking, it should be positive in 303* the counter-clockwise direction). 304* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. 305* @param[out] timestamp The timestamp for this sensor. 306* @return Returns 1 if the data was updated or 0 if it was not updated. 307*/ 308int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, 309 inv_time_t * timestamp) 310{ 311 *accuracy = (int8_t) hal_out.accuracy_quat; 312 *timestamp = hal_out.nav_timestamp; 313 314 google_orientation(values); 315 316 return hal_out.nine_axis_status; 317} 318 319/** Main callback to generate HAL outputs. Typically not called by library users. 320* @param[in] sensor_cal Input variable to take sensor data whenever there is new 321* sensor data. 322* @return Returns INV_SUCCESS if successful or an error code if not. 323*/ 324inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) 325{ 326 int use_sensor = 0; 327 long sr = 1000; 328 long compass[3]; 329 int8_t accuracy; 330 int i; 331 (void) sensor_cal; 332 333 inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, 334 &hal_out.nav_timestamp); 335 hal_out.gyro_status = sensor_cal->gyro.status; 336 hal_out.accel_status = sensor_cal->accel.status; 337 hal_out.compass_status = sensor_cal->compass.status; 338 339 // Find the highest sample rate and tie generating 9-axis to that one. 340 if (sensor_cal->gyro.status & INV_SENSOR_ON) { 341 sr = sensor_cal->gyro.sample_rate_ms; 342 use_sensor = 0; 343 } 344 if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { 345 sr = sensor_cal->accel.sample_rate_ms; 346 use_sensor = 1; 347 } 348 if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { 349 sr = sensor_cal->compass.sample_rate_ms; 350 use_sensor = 2; 351 } 352 if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { 353 sr = sensor_cal->quat.sample_rate_ms; 354 use_sensor = 3; 355 } 356 357 // Only output 9-axis if all 9 sensors are on. 358 if (sensor_cal->quat.status & INV_SENSOR_ON) { 359 // If quaternion sensor is on, gyros are not required as quaternion already has that part 360 if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 361 use_sensor = -1; 362 } 363 } else { 364 if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { 365 use_sensor = -1; 366 } 367 } 368 369 switch (use_sensor) { 370 case 0: 371 hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; 372 hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 373 break; 374 case 1: 375 hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; 376 hal_out.nav_timestamp = sensor_cal->accel.timestamp; 377 break; 378 case 2: 379 hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; 380 hal_out.nav_timestamp = sensor_cal->compass.timestamp; 381 break; 382 case 3: 383 hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; 384 hal_out.nav_timestamp = sensor_cal->quat.timestamp; 385 break; 386 default: 387 hal_out.nine_axis_status = 0; // Don't output quaternion related info 388 break; 389 } 390 391 /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. 392 * So this is: 1 / 2^16*/ 393 #define COMPASS_CONVERSION 1.52587890625e-005f 394 395 inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); 396 hal_out.accuracy_mag = (int ) accuracy; 397 398 for (i=0; i<3; i++) { 399 if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == 400 INV_NEW_DATA ) { 401 // set the state variables to match output with input 402 inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]); 403 } 404 405 if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == 406 (INV_NEW_DATA | INV_RAW_DATA) ) { 407 408 hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i], 409 (float ) compass[i]) * COMPASS_CONVERSION; 410 411 } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) { 412 hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION; 413 } 414 415 } 416 return INV_SUCCESS; 417} 418 419/** Turns off generation of HAL outputs. 420* @return Returns INV_SUCCESS if successful or an error code if not. 421 */ 422inv_error_t inv_stop_hal_outputs(void) 423{ 424 inv_error_t result; 425 result = inv_unregister_data_cb(inv_generate_hal_outputs); 426 return result; 427} 428 429/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() 430* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). 431* @return Returns INV_SUCCESS if successful or an error code if not. 432*/ 433inv_error_t inv_start_hal_outputs(void) 434{ 435 inv_error_t result; 436 result = 437 inv_register_data_cb(inv_generate_hal_outputs, 438 INV_PRIORITY_HAL_OUTPUTS, 439 INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 440 return result; 441} 442/* file name: lowPassFilterCoeff_1_6.c */ 443float compass_low_pass_filter_coeff[5] = 444{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; 445 446/** Initializes hal outputs class. This is called automatically by the 447* enable function. It may be called any time the feature is enabled, but 448* is typically not needed to be called by outside callers. 449* @return Returns INV_SUCCESS if successful or an error code if not. 450*/ 451inv_error_t inv_init_hal_outputs(void) 452{ 453 int i; 454 memset(&hal_out, 0, sizeof(hal_out)); 455 for (i=0; i<3; i++) { 456 inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); 457 } 458 459 return INV_SUCCESS; 460} 461 462/** Turns on creation and storage of HAL type results. 463* @return Returns INV_SUCCESS if successful or an error code if not. 464*/ 465inv_error_t inv_enable_hal_outputs(void) 466{ 467 inv_error_t result; 468 469 // don't need to check the result for inv_init_hal_outputs 470 // since it's always INV_SUCCESS 471 inv_init_hal_outputs(); 472 473 result = inv_register_mpl_start_notification(inv_start_hal_outputs); 474 return result; 475} 476 477/** Turns off creation and storage of HAL type results. 478*/ 479inv_error_t inv_disable_hal_outputs(void) 480{ 481 inv_error_t result; 482 483 inv_stop_hal_outputs(); // Ignore error if we have already stopped this 484 result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); 485 return result; 486} 487 488/** 489 * @} 490 */ 491