| /* |
| $License: |
| Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. |
| See included License.txt for License information. |
| $ |
| */ |
| |
| /** |
| * @defgroup HAL_Outputs hal_outputs |
| * @brief Motion Library - HAL Outputs |
| * Sets up common outputs for HAL |
| * |
| * @{ |
| * @file hal_outputs.c |
| * @brief HAL Outputs. |
| */ |
| |
| #include <string.h> |
| |
| #include "hal_outputs.h" |
| #include "log.h" |
| #include "ml_math_func.h" |
| #include "mlmath.h" |
| #include "start_manager.h" |
| #include "data_builder.h" |
| #include "results_holder.h" |
| |
| struct hal_output_t { |
| int accuracy_mag; /**< Compass accuracy */ |
| // int accuracy_gyro; /**< Gyro Accuracy */ |
| // int accuracy_accel; /**< Accel Accuracy */ |
| int accuracy_quat; /**< quat Accuracy */ |
| |
| inv_time_t nav_timestamp; |
| inv_time_t gam_timestamp; |
| // inv_time_t accel_timestamp; |
| inv_time_t mag_timestamp; |
| long nav_quat[4]; |
| int gyro_status; |
| int accel_status; |
| int compass_status; |
| int nine_axis_status; |
| inv_biquad_filter_t lp_filter[3]; |
| float compass_float[3]; |
| }; |
| |
| static struct hal_output_t hal_out; |
| |
| /** Acceleration (m/s^2) in body frame. |
| * @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it |
| * should return a vector of magnitude near 9.81 m/s^2 |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to |
| * inv_build_accel(). |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| int status; |
| /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. |
| * So this 9.80665 / 2^16 */ |
| #define ACCEL_CONVERSION 0.000149637603759766f |
| long accel[3]; |
| inv_get_accel_set(accel, accuracy, timestamp); |
| values[0] = accel[0] * ACCEL_CONVERSION; |
| values[1] = accel[1] * ACCEL_CONVERSION; |
| values[2] = accel[2] * ACCEL_CONVERSION; |
| if (hal_out.accel_status & INV_NEW_DATA) |
| status = 1; |
| else |
| status = 0; |
| return status; |
| } |
| |
| /** Linear Acceleration (m/s^2) in Body Frame. |
| * @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show |
| * accel biases while at rest. |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to |
| * inv_build_accel(). |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| long gravity[3], accel[3]; |
| |
| inv_get_accel_set(accel, accuracy, timestamp); |
| inv_get_gravity(gravity); |
| accel[0] -= gravity[0] >> 14; |
| accel[1] -= gravity[1] >> 14; |
| accel[2] -= gravity[2] >> 14; |
| values[0] = accel[0] * ACCEL_CONVERSION; |
| values[1] = accel[1] * ACCEL_CONVERSION; |
| values[2] = accel[2] * ACCEL_CONVERSION; |
| |
| return hal_out.nine_axis_status; |
| } |
| |
| /** Gravity vector (m/s^2) in Body Frame. |
| * @param[out] values Gravity vector in body frame, length 3, (m/s^2) |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to |
| * inv_build_accel(). |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| long gravity[3]; |
| int status; |
| |
| *accuracy = (int8_t) hal_out.accuracy_quat; |
| *timestamp = hal_out.nav_timestamp; |
| inv_get_gravity(gravity); |
| values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; |
| values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; |
| values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; |
| if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) |
| status = 1; |
| else |
| status = 0; |
| return status; |
| } |
| |
| /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. |
| * So this is: pi / 2^16 / 180 */ |
| #define GYRO_CONVERSION 2.66316109007924e-007f |
| |
| /** Gyroscope calibrated data (rad/s) in body frame. |
| * @param[out] values Rotation Rate in rad/sec. |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to |
| * inv_build_gyro(). |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| long gyro[3]; |
| int status; |
| |
| inv_get_gyro_set(gyro, accuracy, timestamp); |
| values[0] = gyro[0] * GYRO_CONVERSION; |
| values[1] = gyro[1] * GYRO_CONVERSION; |
| values[2] = gyro[2] * GYRO_CONVERSION; |
| if (hal_out.gyro_status & INV_NEW_DATA) |
| status = 1; |
| else |
| status = 0; |
| return status; |
| } |
| |
| /** Gyroscope raw data (rad/s) in body frame. |
| * @param[out] values Rotation Rate in rad/sec. |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to |
| * inv_build_gyro(). |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| long gyro[3]; |
| int status; |
| |
| inv_get_gyro_set_raw(gyro, accuracy, timestamp); |
| values[0] = gyro[0] * GYRO_CONVERSION; |
| values[1] = gyro[1] * GYRO_CONVERSION; |
| values[2] = gyro[2] * GYRO_CONVERSION; |
| if (hal_out.gyro_status & INV_NEW_DATA) |
| status = 1; |
| else |
| status = 0; |
| return status; |
| } |
| |
| /** |
| * This corresponds to Sensor.TYPE_ROTATION_VECTOR. |
| * The rotation vector represents the orientation of the device as a combination |
| * of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ |
| * around an axis {x, y, z}. <br> |
| * The three elements of the rotation vector are |
| * {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 |
| * vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is |
| * equal to the direction of the axis of rotation. |
| * |
| * The three elements of the rotation vector are equal to the last three components of a unit quaternion |
| * {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). |
| * |
| * Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. |
| * The reference coordinate system is defined as a direct orthonormal basis, where: |
| |
| -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). |
| -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. |
| -Z points towards the sky and is perpendicular to the ground. |
| * @param[out] values Length 4. |
| * @param[out] accuracy Accuracy 0 to 3, 3 = most accurate |
| * @param[out] timestamp Timestamp. In (ns) for Android. |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| *accuracy = (int8_t) hal_out.accuracy_quat; |
| *timestamp = hal_out.nav_timestamp; |
| |
| if (hal_out.nav_quat[0] >= 0) { |
| values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; |
| values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; |
| values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; |
| values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; |
| } else { |
| values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; |
| values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; |
| values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; |
| values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; |
| } |
| values[4] = inv_get_heading_confidence_interval(); |
| |
| return hal_out.nine_axis_status; |
| } |
| |
| |
| /** Compass data (uT) in body frame. |
| * @param[out] values Compass data in (uT), length 3. May be calibrated by having |
| * biases removed and sensitivity adjusted |
| * @param[out] accuracy Accuracy 0 to 3, 3 = most accurate |
| * @param[out] timestamp Timestamp. In (ns) for Android. |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| int status; |
| /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. |
| * So this is: 1 / 2^16*/ |
| //#define COMPASS_CONVERSION 1.52587890625e-005f |
| int i; |
| |
| *timestamp = hal_out.mag_timestamp; |
| *accuracy = (int8_t) hal_out.accuracy_mag; |
| |
| for (i=0; i<3; i++) { |
| values[i] = hal_out.compass_float[i]; |
| } |
| if (hal_out.compass_status & INV_NEW_DATA) |
| status = 1; |
| else |
| status = 0; |
| hal_out.compass_status = 0; |
| return status; |
| } |
| |
| static void inv_get_rotation(float r[3][3]) |
| { |
| long rot[9]; |
| float conv = 1.f / (1L<<30); |
| |
| inv_quaternion_to_rotation(hal_out.nav_quat, rot); |
| r[0][0] = rot[0]*conv; |
| r[0][1] = rot[1]*conv; |
| r[0][2] = rot[2]*conv; |
| r[1][0] = rot[3]*conv; |
| r[1][1] = rot[4]*conv; |
| r[1][2] = rot[5]*conv; |
| r[2][0] = rot[6]*conv; |
| r[2][1] = rot[7]*conv; |
| r[2][2] = rot[8]*conv; |
| } |
| |
| static void google_orientation(float *g) |
| { |
| float rad2deg = (float)(180.0 / M_PI); |
| float R[3][3]; |
| |
| inv_get_rotation(R); |
| |
| g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; |
| g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; |
| g[2] = asinf ( R[2][0]) * rad2deg; |
| if (g[0] < 0) |
| g[0] += 360; |
| } |
| |
| |
| /** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. |
| * @param[out] values Length 3, Degrees.<br> |
| * - values[0]: Azimuth, angle between the magnetic north direction |
| * and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> |
| * - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values |
| * when the z-axis moves toward the y-axis.<br> |
| * - values[2]: Roll, rotation around y-axis (-90 to 90), with positive |
| * values when the x-axis moves toward the z-axis.<br> |
| * |
| * @note This definition is different from yaw, pitch and roll used in aviation |
| * where the X axis is along the long side of the plane (tail to nose). |
| * Note: This sensor type exists for legacy reasons, please use getRotationMatrix() |
| * in conjunction with remapCoordinateSystem() and getOrientation() to compute |
| * these values instead. |
| * Important note: For historical reasons the roll angle is positive in the |
| * clockwise direction (mathematically speaking, it should be positive in |
| * the counter-clockwise direction). |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. |
| * @return Returns 1 if the data was updated or 0 if it was not updated. |
| */ |
| int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, |
| inv_time_t * timestamp) |
| { |
| *accuracy = (int8_t) hal_out.accuracy_quat; |
| *timestamp = hal_out.nav_timestamp; |
| |
| google_orientation(values); |
| |
| return hal_out.nine_axis_status; |
| } |
| |
| /** Main callback to generate HAL outputs. Typically not called by library users. |
| * @param[in] sensor_cal Input variable to take sensor data whenever there is new |
| * sensor data. |
| * @return Returns INV_SUCCESS if successful or an error code if not. |
| */ |
| inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) |
| { |
| int use_sensor = 0; |
| long sr = 1000; |
| long compass[3]; |
| int8_t accuracy; |
| int i; |
| (void) sensor_cal; |
| |
| inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, |
| &hal_out.nav_timestamp); |
| hal_out.gyro_status = sensor_cal->gyro.status; |
| hal_out.accel_status = sensor_cal->accel.status; |
| hal_out.compass_status = sensor_cal->compass.status; |
| |
| // Find the highest sample rate and tie generating 9-axis to that one. |
| if (sensor_cal->gyro.status & INV_SENSOR_ON) { |
| sr = sensor_cal->gyro.sample_rate_ms; |
| use_sensor = 0; |
| } |
| if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { |
| sr = sensor_cal->accel.sample_rate_ms; |
| use_sensor = 1; |
| } |
| if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { |
| sr = sensor_cal->compass.sample_rate_ms; |
| use_sensor = 2; |
| } |
| if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { |
| sr = sensor_cal->quat.sample_rate_ms; |
| use_sensor = 3; |
| } |
| |
| // Only output 9-axis if all 9 sensors are on. |
| if (sensor_cal->quat.status & INV_SENSOR_ON) { |
| // If quaternion sensor is on, gyros are not required as quaternion already has that part |
| if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { |
| use_sensor = -1; |
| } |
| } else { |
| if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { |
| use_sensor = -1; |
| } |
| } |
| |
| switch (use_sensor) { |
| case 0: |
| hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; |
| hal_out.nav_timestamp = sensor_cal->gyro.timestamp; |
| break; |
| case 1: |
| hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; |
| hal_out.nav_timestamp = sensor_cal->accel.timestamp; |
| break; |
| case 2: |
| hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; |
| hal_out.nav_timestamp = sensor_cal->compass.timestamp; |
| break; |
| case 3: |
| hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; |
| hal_out.nav_timestamp = sensor_cal->quat.timestamp; |
| break; |
| default: |
| hal_out.nine_axis_status = 0; // Don't output quaternion related info |
| break; |
| } |
| |
| /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. |
| * So this is: 1 / 2^16*/ |
| #define COMPASS_CONVERSION 1.52587890625e-005f |
| |
| inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); |
| hal_out.accuracy_mag = (int ) accuracy; |
| |
| for (i=0; i<3; i++) { |
| if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == |
| INV_NEW_DATA ) { |
| // set the state variables to match output with input |
| inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]); |
| } |
| |
| if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == |
| (INV_NEW_DATA | INV_RAW_DATA) ) { |
| |
| hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i], |
| (float ) compass[i]) * COMPASS_CONVERSION; |
| |
| } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) { |
| hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION; |
| } |
| |
| } |
| return INV_SUCCESS; |
| } |
| |
| /** Turns off generation of HAL outputs. |
| * @return Returns INV_SUCCESS if successful or an error code if not. |
| */ |
| inv_error_t inv_stop_hal_outputs(void) |
| { |
| inv_error_t result; |
| result = inv_unregister_data_cb(inv_generate_hal_outputs); |
| return result; |
| } |
| |
| /** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() |
| * to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). |
| * @return Returns INV_SUCCESS if successful or an error code if not. |
| */ |
| inv_error_t inv_start_hal_outputs(void) |
| { |
| inv_error_t result; |
| result = |
| inv_register_data_cb(inv_generate_hal_outputs, |
| INV_PRIORITY_HAL_OUTPUTS, |
| INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); |
| return result; |
| } |
| /* file name: lowPassFilterCoeff_1_6.c */ |
| float compass_low_pass_filter_coeff[5] = |
| {+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; |
| |
| /** Initializes hal outputs class. This is called automatically by the |
| * enable function. It may be called any time the feature is enabled, but |
| * is typically not needed to be called by outside callers. |
| * @return Returns INV_SUCCESS if successful or an error code if not. |
| */ |
| inv_error_t inv_init_hal_outputs(void) |
| { |
| int i; |
| memset(&hal_out, 0, sizeof(hal_out)); |
| for (i=0; i<3; i++) { |
| inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| /** Turns on creation and storage of HAL type results. |
| * @return Returns INV_SUCCESS if successful or an error code if not. |
| */ |
| inv_error_t inv_enable_hal_outputs(void) |
| { |
| inv_error_t result; |
| |
| // don't need to check the result for inv_init_hal_outputs |
| // since it's always INV_SUCCESS |
| inv_init_hal_outputs(); |
| |
| result = inv_register_mpl_start_notification(inv_start_hal_outputs); |
| return result; |
| } |
| |
| /** Turns off creation and storage of HAL type results. |
| */ |
| inv_error_t inv_disable_hal_outputs(void) |
| { |
| inv_error_t result; |
| |
| inv_stop_hal_outputs(); // Ignore error if we have already stopped this |
| result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); |
| return result; |
| } |
| |
| /** |
| * @} |
| */ |