libsensors_iio/software/core/mllite/hal_outputs.c - platform/hardware/invensense - Git at Google

 /*
  $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 "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;
 }

 /** 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)
 {
     /* 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
     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)
 {
     /* 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
     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;
     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;
     }

     switch (use_sensor) {
     default:
     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;
     }

     /* 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;
 }

 /**
  * @}
  */
	/*
	$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 "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;
	}

	/** 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)
	{
	/* 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
	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)
	{
	/* 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
	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
	* {xsin(@f$\theta@f$/2), ysin(@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
	* {xsin(@f$\theta@f$/2), ysin(@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;
	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;
	}

	switch (use_sensor) {
	default:
	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;
	}

	/* 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;
	}

	/**
	* @}
	*/