diff options
Diffstat (limited to 'libsensors_iio/software/core/mllite/hal_outputs.c')
| -rw-r--r-- | libsensors_iio/software/core/mllite/hal_outputs.c | 425 |
1 files changed, 425 insertions, 0 deletions
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c new file mode 100644 index 0000000..1cd3b81 --- /dev/null +++ b/libsensors_iio/software/core/mllite/hal_outputs.c @@ -0,0 +1,425 @@ +/* + $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 */ + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; + inv_time_t accel_timestamp; + long nav_quat[4]; + int gyro_status; + int accel_status; + int compass_status; + int nine_axis_status; +}; + +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 = hal_out.accuracy_mag; + *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 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; +} + +/** +* 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 = hal_out.accuracy_mag; + *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 + long compass[3]; + inv_get_compass_set(compass, accuracy, timestamp); + values[0] = compass[0] * COMPASS_CONVERSION; + values[1] = compass[1] * COMPASS_CONVERSION; + values[2] = compass[2] * COMPASS_CONVERSION; + if (hal_out.compass_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + + +/** 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) +{ + long t1, t2, t3; + long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; + + *accuracy = hal_out.accuracy_mag; + *timestamp = hal_out.nav_timestamp; + + q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]); + q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]); + q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]); + q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]); + q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]); + q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]); + q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]); + q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]); + q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]); + q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]); + + /* X component of the Ybody axis in World frame */ + t1 = q12 - q03; + + /* Y component of the Ybody axis in World frame */ + t2 = q22 + q00 - (1L << 30); + values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; + if (values[0] < 0) + values[0] += 360; + + /* Z component of the Ybody axis in World frame */ + t3 = q23 + q01; + values[1] = + -atan2f((float) t3, + sqrtf((float) t1 * t1 + + (float) t2 * t2)) * 180.f / (float) M_PI; + /* Z component of the Zbody axis in World frame */ + t2 = q33 + q00 - (1L << 30); + if (t2 < 0) { + if (values[1] >= 0) + values[1] = 180.f - values[1]; + else + values[1] = -180.f - values[1]; + } + + /* X component of the Xbody axis in World frame */ + t1 = q11 + q00 - (1L << 30); + /* Y component of the Xbody axis in World frame */ + t2 = q12 + q03; + /* Z component of the Xbody axis in World frame */ + t3 = q13 - q02; + //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI; + + values[2] = + -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * + 180.f / (float) M_PI - 90); + if (values[2] >= 90) + values[2] = 180 - values[2]; + + if (values[2] < -90) + values[2] = -180 - values[2]; + + 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; + (void) sensor_cal; + inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag, + &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; + } + + 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; +} + +/** 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) +{ + memset(&hal_out, 0, sizeof(hal_out)); + 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; +} + +/** + * @} + */ |