diff options
author | JP Abgrall <jpa@google.com> | 2012-10-03 20:16:57 -0700 |
---|---|---|
committer | JP Abgrall <jpa@google.com> | 2012-10-03 20:16:57 -0700 |
commit | 33ce91b37062fa63af192f5643de93f3beebe854 (patch) | |
tree | ba6a03c7954a32ce23b00c6d46cd3d524f4d7007 /libsensors_iio/software/core/mllite/ml_math_func.c | |
parent | 64ca18f95225d0a86f7ccfd1d21c23971b9f77ae (diff) | |
download | invensense-33ce91b37062fa63af192f5643de93f3beebe854.tar.gz |
MotionApps 5.1.1 release, with MA 5.1.0 for further merge review.android-sdk-support_r11android-cts-4.2_r2android-cts-4.2_r1android-4.2_r1android-4.2.2_r1.2android-4.2.2_r1.1android-4.2.2_r1android-4.2.1_r1.2android-4.2.1_r1.1android-4.2.1_r1tools_r22jb-mr1.1-releasejb-mr1.1-dev-plus-aospjb-mr1.1-devjb-mr1-releasejb-mr1-dev-plus-aospjb-mr1-dev
1. Removed all #ifdef in HAL's member APIs.
2. Added necessary comments as reference.
3. Made changes for coding style, optimization and so on per prior comments.
4. Now raw/calibrated gyroscope sensors could co-exist
Default sensor would be calibrated gyroscope sensor
for getDefaultSensor() call in Android.
* Correctly handle onPower()/masterEnable().
* Use the support functions for reading/writing sysfs.
1 line instead of 9 all over the place.
* Fix return code for {read,write}_sysfs_int():
was > 0 in case of failure instead of < 0.
Bug: 7211625
Change-Id: Ib49dab8ca0f48f45a2838de72f4f8ac011d0e68f
Diffstat (limited to 'libsensors_iio/software/core/mllite/ml_math_func.c')
-rw-r--r-- | libsensors_iio/software/core/mllite/ml_math_func.c | 51 |
1 files changed, 49 insertions, 2 deletions
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c index 86c4b41..d1fd9c4 100644 --- a/libsensors_iio/software/core/mllite/ml_math_func.c +++ b/libsensors_iio/software/core/mllite/ml_math_func.c @@ -648,13 +648,60 @@ void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity } /** find a norm for a vector -* @param[in] a vector [3x1] -* @param[out] output the norm of the input vector +* @param[in] x a vector [3x1] +* @return the normalize vector. */ double inv_vector_norm(const float *x) { return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); } + +void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { + int i; + // initial state to zero + pFilter->state[0] = 0; + pFilter->state[1] = 0; + + // set up coefficients + for (i=0; i<5; i++) { + pFilter->c[i] = pBiquadCoeff[i]; + } +} + +void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) +{ + float divider; + pFilter->input = input; + pFilter->output = input; + pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); + pFilter->state[1] = pFilter->state[0]; +} + +float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { + float stateZero; + + pFilter->input = input; + // calculate the new state; + stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] + - pFilter->c[3]*pFilter->state[1]; + + pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] + + pFilter->c[1]*pFilter->state[1]; + + // update the output and state + pFilter->output = pFilter->output * pFilter->c[4]; + pFilter->state[1] = pFilter->state[0]; + pFilter->state[0] = stateZero; + return pFilter->output; +} + +void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { + + cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; +} + /** * @} */ |