149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $License: 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow See included License.txt for License information. 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $ 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/******************************************************************************* 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * $Id:$ 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ******************************************************************************/ 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @defgroup ML_MATH_FUNC ml_math_func 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Motion Library - Math Functions 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Common math functions the Motion Library 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @{ 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @file ml_math_func.c 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Math Functions. 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlmath.h" 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_math_func.h" 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlinclude.h" 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Does the cross product of compass by gravity, then converts that 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * to the world frame using the quaternion, then computes the angle that 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * is made. 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] compass Compass Vector (Body Frame), length 3 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] grav Gravity Vector (Body Frame), length 3 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] quat Quaternion, Length 4 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Angle Cross Product makes after quaternion rotation. 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_compass_angle(const long *compass, const long *grav, const float *quat) 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float cgcross[4], q1[4], q2[4], qi[4]; 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float angW; 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Compass cross Gravity 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[0] = 0.f; 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Now convert cross product into world frame 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_multf(quat, cgcross, q1); 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_invertf(quat, qi); 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_multf(q1, qi, q2); 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Protect against atan2 of 0,0 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((q2[2] == 0.f) && (q2[1] == 0.f)) 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0.f; 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // This is the unfiltered heading correction 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow angW = -atan2f(q2[2], q2[1]); 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return angW; 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief The gyro data magnitude squared : 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] gyro Gyro data scaled with 1 dps = 2^16 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return the computed magnitude squared output of the gyroscope. 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowunsigned long inv_get_gyro_sum_of_sqr(const long *gyro) 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned long gmag = 0; 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long temp; 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk; 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk = 0; kk < 3; ++kk) { 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gmag += temp * temp; 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return gmag; 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a multiply and shift by 29. These are good functions to write in assembly on 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * with devices with small memory where you want to get rid of the long long which some 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * assemblers don't handle well 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] a 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] b 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return ((long long)a*b)>>29 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_q29_mult(long a, long b) 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef UMPL_ELIMINATE_64BIT 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)((float)a * b / (1L << 29)); 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long long temp; 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp = (long long)a * b; 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)(temp >> 29); 10249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a multiply and shift by 30. These are good functions to write in assembly on 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * with devices with small memory where you want to get rid of the long long which some 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * assemblers don't handle well 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] a 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] b 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return ((long long)a*b)>>30 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_q30_mult(long a, long b) 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef UMPL_ELIMINATE_64BIT 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)((float)a * b / (1L << 30)); 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 12049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long long temp; 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp = (long long)a * b; 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)(temp >> 30); 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef UMPL_ELIMINATE_64BIT 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_q30_div(long a, long b) 13049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 13149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long long temp; 13249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 13349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp = (((long long)a) << 30) / b; 13449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)temp; 13549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 13649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 13749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 13849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 13949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a multiply and shift by shift. These are good functions to write 14049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * in assembly on with devices with small memory where you want to get rid of 14149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * the long long which some assemblers don't handle well 14249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] a First multicand 14349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] b Second multicand 14449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] shift Shift amount after multiplying 14549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return ((long long)a*b)<<shift 14649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 14749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef UMPL_ELIMINATE_64BIT 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_q_shift_mult(long a, long b, int shift) 14949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long result; 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = (long)(((long long)a * b) >> shift); 15249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 15549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a fixed point quaternion multiply. 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled 15849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to 2^30 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to 2^30 16149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] qProd Product after quaternion multiply. Length 4. 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 1.0 scaled to 2^30. 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 16449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_mult(const long *q1, const long *q2, long *qProd) 16549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 16649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 16749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - 16849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a fixed point quaternion addition. 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] q1 First Quaternion term, length 4. 1.0 scaled 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to 2^30 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to 2^30 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] qSum Sum after quaternion summation. Length 4. 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 1.0 scaled to 2^30. 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_add(long *q1, long *q2, long *qSum) 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[0] = q1[0] + q2[0]; 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[1] = q1[1] + q2[1]; 19349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[2] = q1[2] + q2[2]; 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[3] = q1[3] + q2[3]; 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_vector_normalize(long *vec, int length) 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double normSF = 0; 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int ii; 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (ii = 0; ii < length; ii++) { 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF += 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (normSF > 0) { 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = 1 / sqrt(normSF); 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (ii = 0; ii < length; ii++) { 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow vec[ii] = (int)((double)vec[ii] * normSF); 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 21149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 21249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow vec[0] = 1073741824L; 21349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (ii = 1; ii < length; ii++) { 21449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow vec[ii] = 0; 21549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 21649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 21749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 21849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 21949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_normalize(long *q) 22049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 22149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 22249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_vector_normalize(q, 4); 22349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 22449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 22549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_invert(const long *q, long *qInverted) 22649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 22749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 22849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[0] = q[0]; 22949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[1] = -q[1]; 23049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[2] = -q[2]; 23149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[3] = -q[3]; 23249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 23349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowdouble quaternion_to_rotation_angle(const long *quat) { 23549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double quat0 = (double )quat[0] / 1073741824; 23649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (quat0 > 1.0f) { 23749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow quat0 = 1.0; 23849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (quat0 < -1.0f) { 23949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow quat0 = -1.0; 24049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 24149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 24249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return acos(quat0)*2*180/M_PI; 24349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 24449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 24549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Rotates a 3-element vector by Rotation defined by Q 24649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 24749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_rotate(const long *q, const long *in, long *out) 24849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 24949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long q_temp1[4], q_temp2[4]; 25049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long in4[4], out4[4]; 25149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 25249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Fixme optimize 25349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow in4[0] = 0; 25449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(&in4[1], in, 3 * sizeof(long)); 25549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_mult(q, in4, q_temp1); 25649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_invert(q, q_temp2); 25749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_mult(q_temp1, q_temp2, out4); 25849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(out, &out4[1], 3 * sizeof(long)); 25949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 26049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 26149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_multf(const float *q1, const float *q2, float *qProd) 26249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 26349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 26449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[0] = 26549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); 26649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[1] = 26749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); 26849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[2] = 26949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); 27049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qProd[3] = 27149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); 27249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 27349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 27449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_addf(const float *q1, const float *q2, float *qSum) 27549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 27649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 27749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[0] = q1[0] + q2[0]; 27849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[1] = q1[1] + q2[1]; 27949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[2] = q1[2] + q2[2]; 28049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qSum[3] = q1[3] + q2[3]; 28149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 28249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_normalizef(float *q) 28449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 28549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 28649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float normSF = 0; 28749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float xHalf = 0; 28849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 28949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (normSF < 2) { 29049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xHalf = 0.5f * normSF; 29149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = normSF * (1.5f - xHalf * normSF * normSF); 29249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = normSF * (1.5f - xHalf * normSF * normSF); 29349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = normSF * (1.5f - xHalf * normSF * normSF); 29449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = normSF * (1.5f - xHalf * normSF * normSF); 29549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[0] *= normSF; 29649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[1] *= normSF; 29749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[2] *= normSF; 29849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[3] *= normSF; 29949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 30049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[0] = 1.0; 30149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[1] = 0.0; 30249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[2] = 0.0; 30349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[3] = 0.0; 30449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 30549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 30649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 30749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 30849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Performs a length 4 vector normalization with a square root. 30949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. 31049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 31149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_norm4(float *q) 31249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 31349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float mag; 31449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 31549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mag) { 31649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[0] /= mag; 31749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[1] /= mag; 31849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[2] /= mag; 31949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[3] /= mag; 32049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 32149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[0] = 1.f; 32249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[1] = 0.f; 32349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[2] = 0.f; 32449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow q[3] = 0.f; 32549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 32649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 32749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_q_invertf(const float *q, float *qInverted) 32949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 33049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INVENSENSE_FUNC_START; 33149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[0] = q[0]; 33249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[1] = -q[1]; 33349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[2] = -q[2]; 33449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow qInverted[3] = -q[3]; 33549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 33649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 33749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 33849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Converts a quaternion to a rotation matrix. 33949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] quat 4-element quaternion in fixed point. One is 2^30. 34049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] rot Rotation matrix in fixed point. One is 2^30. The 34149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * First 3 elements of the rotation matrix, represent 34249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * the first row of the matrix. Rotation matrix multiplied 34349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * by a 3 element column vector transform a vector from Body 34449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * to World. 34549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 34649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_quaternion_to_rotation(const long *quat, long *rot) 34749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 34849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[0] = 34949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], 35049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow quat[0]) - 35149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1073741824L; 35249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[1] = 35349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); 35449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[2] = 35549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); 35649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[3] = 35749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); 35849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[4] = 35949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], 36049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow quat[0]) - 36149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1073741824L; 36249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[5] = 36349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); 36449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[6] = 36549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); 36649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[7] = 36749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); 36849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[8] = 36949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], 37049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow quat[0]) - 37149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1073741824L; 37249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 37349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 37449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 37549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Converts a quaternion to a rotation vector. A rotation vector is 37649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * a method to represent a 4-element quaternion vector in 3-elements. 37749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * To get the quaternion from the 3-elements, The last 3-elements of 37849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * the quaternion will be the given rotation vector. The first element 37949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * of the quaternion will be the positive value that will be required 38049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. 38149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] quat 4-element quaternion in fixed point. One is 2^30. 38249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] rot Rotation vector in fixed point. One is 2^30. 38349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 38449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_quaternion_to_rotation_vector(const long *quat, long *rot) 38549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 38649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[0] = quat[1]; 38749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[1] = quat[2]; 38849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[2] = quat[3]; 38949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (quat[0] < 0.0) { 39149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[0] = -rot[0]; 39249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[1] = -rot[1]; 39349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rot[2] = -rot[2]; 39449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 39549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 39649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts a 32-bit long to a big endian byte stream */ 39849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowunsigned char *inv_int32_to_big8(long x, unsigned char *big8) 39949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 40049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[0] = (unsigned char)((x >> 24) & 0xff); 40149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[1] = (unsigned char)((x >> 16) & 0xff); 40249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[2] = (unsigned char)((x >> 8) & 0xff); 40349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[3] = (unsigned char)(x & 0xff); 40449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return big8; 40549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 40649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts a big endian byte stream into a 32-bit long */ 40849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_big8_to_int32(const unsigned char *big8) 40949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 41049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long x; 41149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) 41249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | ((long)big8[3]); 41349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return x; 41449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 41549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 41649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts a big endian byte stream into a 16-bit integer (short) */ 41749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowshort inv_big8_to_int16(const unsigned char *big8) 41849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 41949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short x; 42049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow x = ((short)big8[0] << 8) | ((short)big8[1]); 42149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return x; 42249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 42349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 42449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts a little endian byte stream into a 16-bit integer (short) */ 42549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowshort inv_little8_to_int16(const unsigned char *little8) 42649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 42749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short x; 42849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow x = ((short)little8[1] << 8) | ((short)little8[0]); 42949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return x; 43049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 43149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 43249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts a 16-bit short to a big endian byte stream */ 43349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowunsigned char *inv_int16_to_big8(short x, unsigned char *big8) 43449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 43549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[0] = (unsigned char)((x >> 8) & 0xff); 43649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow big8[1] = (unsigned char)(x & 0xff); 43749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return big8; 43849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 43949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 44049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) 44149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 44249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int k, l, i, j; 44349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0, k = 0; i < *n; i++, k++) { 44449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (j = 0, l = 0; j < *n; j++, l++) { 44549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i == x) 44649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow i++; 44749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (j == y) 44849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow j++; 44949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *(b + 6 * k + l) = *(a + 6 * i + j); 45049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 45149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 45249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *n = *n - 1; 45349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 45449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 45549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) 45649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 45749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int k, l, i, j; 45849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0, k = 0; i < *n; i++, k++) { 45949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (j = 0, l = 0; j < *n; j++, l++) { 46049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i == x) 46149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow i++; 46249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (j == y) 46349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow j++; 46449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *(b + 6 * k + l) = *(a + 6 * i + j); 46549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 46649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 46749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *n = *n - 1; 46849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 46949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 47049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_matrix_det(float *p, int *n) 47149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 47249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float d[6][6], sum = 0; 47349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, j, m; 47449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow m = *n; 47549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (*n == 2) 47649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (*p ** (p + 7) - *(p + 1) ** (p + 6)); 47749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0, j = 0; j < m; j++) { 47849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *n = m; 47949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_matrix_det_inc(p, &d[0][0], n, i, j); 48049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sum = 48149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sum + *(p + 6 * i + j) * SIGNM(i + 48249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow j) * 48349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_matrix_det(&d[0][0], n); 48449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 48549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 48649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (sum); 48749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 48849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 48949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowdouble inv_matrix_detd(double *p, int *n) 49049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 49149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double d[6][6], sum = 0; 49249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, j, m; 49349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow m = *n; 49449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (*n == 2) 49549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (*p ** (p + 7) - *(p + 1) ** (p + 6)); 49649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0, j = 0; j < m; j++) { 49749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *n = m; 49849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_matrix_det_incd(p, &d[0][0], n, i, j); 49949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sum = 50049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sum + *(p + 6 * i + j) * SIGNM(i + 50149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow j) * 50249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_matrix_detd(&d[0][0], n); 50349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 50449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (sum); 50649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 50749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Wraps angle from (-M_PI,M_PI] 50949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] ang Angle in radians to wrap 51049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Wrapped angle from (-M_PI,M_PI] 51149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 51249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_wrap_angle(float ang) 51349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 51449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (ang > M_PI) 51549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ang - 2 * (float)M_PI; 51649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (ang <= -(float)M_PI) 51749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ang + 2 * (float)M_PI; 51849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 51949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ang; 52049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 52149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 52249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Finds the minimum angle difference ang1-ang2 such that difference 52349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * is between [-M_PI,M_PI] 52449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] ang1 52549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] ang2 52649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return angle difference ang1-ang2 52749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 52849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_angle_diff(float ang1, float ang2) 52949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 53049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float d; 53149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ang1 = inv_wrap_angle(ang1); 53249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ang2 = inv_wrap_angle(ang2); 53349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow d = ang1 - ang2; 53449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (d > M_PI) 53549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow d -= 2 * (float)M_PI; 53649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (d < -(float)M_PI) 53749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow d += 2 * (float)M_PI; 53849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return d; 53949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 54049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** bernstein hash, derived from public domain source */ 54249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowuint32_t inv_checksum(const unsigned char *str, int len) 54349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 54449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t hash = 5381; 54549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, c; 54649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < len; i++) { 54849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow c = *(str + i); 549cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ 55049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 55249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return hash; 55349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 55449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 55549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic unsigned short inv_row_2_scale(const signed char *row) 55649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 55749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short b; 55849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 55949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (row[0] > 0) 56049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 0; 56149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (row[0] < 0) 56249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 4; 56349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (row[1] > 0) 56449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 1; 56549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (row[1] < 0) 56649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 5; 56749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (row[2] > 0) 56849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 2; 56949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (row[2] < 0) 57049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow b = 6; 57149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 572cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro b = 7; // error 57349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return b; 57449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 57549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. 57849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] mtx Orientation matrix to convert to a scalar. 57949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the 58049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent 58149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* the column the one is on for the second row with bit number 5 being the sign. 58249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The next 2 bits (6 and 7) represent the column the one is on for the third row with 58349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* bit number 8 being the sign. In binary the identity matrix would therefor be: 58449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 010_001_000 or 0x88 in hex. 58549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 58649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowunsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) 58749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 58849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 58949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short scalar; 59049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 59149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 59249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow XYZ 010_001_000 Identity Matrix 59349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow XZY 001_010_000 59449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow YXZ 010_000_001 59549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow YZX 000_010_001 59649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ZXY 001_000_010 59749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ZYX 000_001_010 59849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 59949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 60049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow scalar = inv_row_2_scale(mtx); 60149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow scalar |= inv_row_2_scale(mtx + 3) << 3; 60249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow scalar |= inv_row_2_scale(mtx + 6) << 6; 60349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 60449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return scalar; 60549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 60649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 60749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Uses the scalar orientation value to convert from chip frame to body frame 60849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar that represent how to go from chip to body frame 60949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] input Input vector, length 3 61049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] output Output vector, length 3 61149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 61249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_convert_to_body(unsigned short orientation, const long *input, long *output) 61349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 61449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); 61549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); 61649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); 61749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 61849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 61949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Uses the scalar orientation value to convert from body frame to chip frame 62049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar that represent how to go from chip to body frame 62149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] input Input vector, length 3 62249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] output Output vector, length 3 62349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 62449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_convert_to_chip(unsigned short orientation, const long *input, long *output) 62549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 62649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); 62749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); 62849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); 62949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 63049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 63149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 63249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Uses the scalar orientation value to convert from chip frame to body frame and 63349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* apply appropriate scaling. 63449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] orientation A scalar that represent how to go from chip to body frame 63549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sensitivity Sensitivity scale 63649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] input Input vector, length 3 63749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] output Output vector, length 3 63849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 639cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarovoid inv_convert_to_body_with_scale(unsigned short orientation, 640cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long sensitivity, 641cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro const long *input, long *output) 64249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 64349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[0] = inv_q30_mult(input[orientation & 0x03] * 64449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SIGNSET(orientation & 0x004), sensitivity); 64549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * 64649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SIGNSET(orientation & 0x020), sensitivity); 64749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * 64849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SIGNSET(orientation & 0x100), sensitivity); 64949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 65049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 65149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** find a norm for a vector 65249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] a vector [3x1] 65349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] output the norm of the input vector 65449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 65549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowdouble inv_vector_norm(const float *x) 65649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 65749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); 65849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 65949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { 66149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i; 66249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // initial state to zero 66349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[0] = 0; 66449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[1] = 0; 66549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set up coefficients 66749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<5; i++) { 66849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->c[i] = pBiquadCoeff[i]; 66949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 67049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 67149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 67249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) 67349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 67449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->input = input; 67549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->output = input; 67649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); 67749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[1] = pFilter->state[0]; 67849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 67949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { 68149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float stateZero; 68249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->input = input; 68449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // calculate the new state; 68549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] 68649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow - pFilter->c[3]*pFilter->state[1]; 68749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] 68949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow + pFilter->c[1]*pFilter->state[1]; 69049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // update the output and state 69249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->output = pFilter->output * pFilter->c[4]; 69349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[1] = pFilter->state[0]; 69449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pFilter->state[0] = stateZero; 69549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return pFilter->output; 69649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 69749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { 69949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 70049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; 70149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; 70249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; 70349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 70449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 70549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut) { 70649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // matrix format 70749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // [ 0 3 6; 70849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 1 4 7; 70949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 2 5 8] 71049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 71149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // vector format: [0 1 2]^T; 71249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, j; 71349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long temp; 71449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 71549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i=0; i<3; i++) { 71649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp = 0; 71749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (j=0; j<3; j++) { 71849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp += inv_q30_mult(matrix[i+j*3], vecIn[j]); 71949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 72049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow vecOut[i] = temp; 72149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 72249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 72349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 72449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow//============== 1/sqrt(x), 1/x, sqrt(x) Functions ================================ 72549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 72649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Calculates 1/square-root of a fixed-point number (30 bit mantissa, positive): Q1.30 72749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Input must be a positive scaled (2^30) integer 72849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The number is scaled to lie between a range in which a Newton-Raphson 72949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* iteration works best. Corresponding square root of the power of two is returned. 73049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Caller must scale final result by 2^rempow (while avoiding overflow). 73149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] x0, length 1 73249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] rempow, length 1 73349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return scaledSquareRoot on success or zero. 73449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 735cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarolong inv_inverse_sqrt(long x0, int*rempow) 736cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 737cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% Inverse sqrt NR in the neighborhood of 1.3>x>=0.65 738cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% x(k+1) = x(k)*(3 - x0*x(k)^2) 739cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 740cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% Seed equals 1. Works best in this region. 741cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //xx0 = int32(1*2^30); 742cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 743cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long oneoversqrt2, oneandhalf, x0_2; 744cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro unsigned long xx; 745cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int pow2, sq2scale, nr_iters; 746cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //long upscale, sqrt_upscale, upsclimit; 747cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //long downscale, sqrt_downscale, downsclimit; 74849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 74949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Precompute some constants for efficiency 750cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% int32(2^30*1/sqrt(2)) 751cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro oneoversqrt2=759250125L; 752cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% int32(1.5*2^30); 753cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro oneandhalf=1610612736L; 754cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 755cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //// Further scaling into optimal region saves one or more NR iterations. Maps into region (.9, 1.1) 756cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //// int32(0.9/log(2)*2^30) 757cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //upscale = 1394173804L; 758cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //// int32(sqrt(0.9/log(2))*2^30) 759cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //sqrt_upscale = 1223512453L; 760cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // // int32(1.1*log(2)/.9*2^30) 761cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //upsclimit = 909652478L; 762cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //// int32(1.1/log(4)*2^30) 763cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //downscale = 851995103L; 764cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //// int32(sqrt(1.1/log(4))*2^30) 765cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //sqrt_downscale = 956463682L; 766cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // // int32(0.9*log(4)/1.1*2^30) 767cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //downsclimit = 1217881829L; 768cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 769cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = test_limits_and_scale(&x0, &pow2); 770cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 771cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sq2scale=pow2%2; // Find remainder. Is it even or odd? 772cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 773cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 774cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Further scaling to decrease NR iterations 775cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // With the mapping below, 89% of calculations will require 2 NR iterations or less. 776cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // TBD 777cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 778cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 779cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro x0_2 = x0 >>1; // This scaling incorporates factor of 2 in NR iteration below. 780cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Initial condition starts at 1: xx=(1L<<30); 781cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // First iteration is simple: Instead of initializing xx=1, assign to result of first iteration: 782cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // xx= (3/2-x0/2); 783cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% NR formula: xx=xx*(3/2-x0*xx*xx/2); = xx*(1.5 - (x0/2)*xx*xx) 784cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Initialize NR (first iteration). Note we are starting with xx=1, so the first iteration becomes an initialization. 785cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro xx = oneandhalf - x0_2; 786cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters>=2 ) { 787cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Second NR iteration 78849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) ); 789cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters==3 ) { 790cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Third NR iteration. 79149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) ); 792cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Fourth NR iteration: Not needed due to single precision. 793cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 79449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 79549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sq2scale) { 79649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *rempow = (pow2>>1) + 1; // Account for sqrt(2) in denominator, note we multiply if s2scale is true 79749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (inv_q30_mult(xx,oneoversqrt2)); 79849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 79949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 80049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *rempow = pow2>>1; 80149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return xx; 80249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 80349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 80449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 80549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 80649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Calculates square-root of a fixed-point number (30 bit mantissa, positive) 80749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Input must be a positive scaled (2^30) integer 80849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The number is scaled to lie between a range in which a Newton-Raphson 80949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* iteration works best. 81049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] x0, length 1 81149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return scaledSquareRoot on success or zero. **/ 81249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowlong inv_fast_sqrt(long x0) 81349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 81449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 815cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% Square-Root with NR in the neighborhood of 1.3>x>=0.65 (log(2) <= x <= log(4) ) 816cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Two-variable NR iteration: 817cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Initialize: a=x; c=x-1; 818cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // 1st Newton Step: a=a-a*c/2; ( or: a = x - x*(x-1)/2 ) 819cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Iterate: c = c*c*(c-3)/4 820cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // a = a - a*c/2 --> reevaluating c at this step gives error of approximation 821cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 822cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% Seed equals 1. Works best in this region. 823cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //xx0 = int32(1*2^30); 824cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 825cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long sqrt2, oneoversqrt2, one_pt5; 826cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long xx, cc; 827cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int pow2, sq2scale, nr_iters; 82849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 82949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Return if input is zero. Negative should really error out. 830cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0 <= 0L) { 831cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0L; 832cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 833cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 834cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sqrt2 =1518500250L; 835cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro oneoversqrt2=759250125L; 836cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro one_pt5=1610612736L; 837cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 838cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = test_limits_and_scale(&x0, &pow2); 839cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 840cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sq2scale = 0; 841cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (pow2 > 0) 842cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sq2scale=pow2%2; // Find remainder. Is it even or odd? 843cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro pow2 = pow2-sq2scale; // Now pow2 is even. Note we are adding because result is scaled with sqrt(2) 844cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 845cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Sqrt 1st NR iteration 846cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro cc = x0 - (1L<<30); 847cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro xx = x0 - (inv_q30_mult(x0, cc)>>1); 848cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters>=2 ) { 849cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Sqrt second NR iteration 850cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // cc = cc*cc*(cc-3)/4; = cc*cc*(cc/2 - 3/2)/2; 851cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // cc = ( cc*cc*((cc>>1) - onePt5) ) >> 1 852cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1; 85349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = xx - (inv_q30_mult(xx, cc)>>1); 854cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters==3 ) { 855cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Sqrt third NR iteration 856cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1; 85749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = xx - (inv_q30_mult(xx, cc)>>1); 858cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 85949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 86049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sq2scale) 86149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult(xx,oneoversqrt2); 86249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Scale the number with the half of the power of 2 scaling 86349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (pow2>0) 86449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = (xx >> (pow2>>1)); 86549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (pow2 == -1) 86649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult(xx,sqrt2); 86749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return xx; 86849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 86949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 87049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Calculates 1/x of a fixed-point number (30 bit mantissa) 87149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Input must be a scaled (2^30) integer (+/-) 87249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The number is scaled to lie between a range in which a Newton-Raphson 87349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* iteration works best. Corresponding multiplier power of two is returned. 87449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Caller must scale final result by 2^pow (while avoiding overflow). 87549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] x, length 1 87649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] pow, length 1 87749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return scaledOneOverX on success or zero. 87849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 879cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarolong inv_one_over_x(long x0, int*pow) 880cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 881cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% NR for 1/x in the neighborhood of log(2) => x => log(4) 882cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% y(k+1)=y(k)*(2 \ 96 x0*y(k)) 883cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //% with y(0) = 1 as the starting value for NR 884cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 885cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long two, xx; 886cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int numberwasnegative, nr_iters, did_upscale, did_downscale; 887cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 888cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long upscale, downscale, upsclimit, downsclimit; 889cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 89049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *pow = 0; 89149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Return if input is zero. 892cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0 == 0L) { 893cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0L; 894cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 895cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 896cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // This is really (2^31-1), i.e. 1.99999... . 897cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Approximation error is 1e-9. Note 2^31 will overflow to sign bit, so it can't be used here. 898cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro two = 2147483647L; 899cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 900cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(0.92/log(2)*2^30) 901cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro upscale = 1425155444L; 902cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(1.08/upscale*2^30) 903cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro upsclimit = 873697834L; 904cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 905cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(1.08/log(4)*2^30) 906cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro downscale = 836504283L; 907cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(0.92/downscale*2^30) 908cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro downsclimit = 1268000423L; 909cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 910cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Algorithm is intended to work with positive numbers only. Change sign: 911cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro numberwasnegative = 0; 912cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0 < 0L) { 913cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro numberwasnegative = 1; 914cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro x0 = -x0; 915cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 916cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 917cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = test_limits_and_scale(&x0, pow); 918cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 919cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro did_upscale=0; 920cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro did_downscale=0; 921cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Pre-scaling to reduce NR iterations and improve accuracy: 922cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0<=upsclimit) { 923cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro x0 = inv_q30_mult(x0, upscale); 924cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro did_upscale = 1; 925cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // The scaling ALWAYS leaves the number in the 2-NR iterations region: 926cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = 2; 927cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Is x0 in the single NR iteration region (0.994, 1.006) ? 928cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0 > 1067299373 && x0 < 1080184275) 929cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = 1; 930cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (x0>=downsclimit) { 931cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro x0 = inv_q30_mult(x0, downscale); 932cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro did_downscale = 1; 933cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // The scaling ALWAYS leaves the number in the 2-NR iterations region: 934cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = 2; 935cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Is x0 in the single NR iteration region (0.994, 1.006) ? 936cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (x0 > 1067299373 && x0 < 1080184275) 937cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nr_iters = 1; 938cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 939cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 940cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro xx = (two - x0) + 1; // Note 2 will overflow so the computation (2-x) is done with "two" == (2^30-1) 941cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // First NR iteration 94249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); 943cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters>=2 ) { 944cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Second NR iteration 94549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); 946cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( nr_iters==3 ) { 947cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // THird NR iteration. 94849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); 949cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Fourth NR iteration: Not needed due to single precision. 950cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 95149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 95249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 95349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Post-scaling 95449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (did_upscale) 95549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, upscale); 95649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (did_downscale) 95749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = inv_q30_mult( xx, downscale); 95849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 95949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (numberwasnegative) 96049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow xx = -xx; 96149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return xx; 96249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 96349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 96449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Auxiliary function used by inv_OneOverX(), inv_fastSquareRoot(), inv_inverseSqrt(). 96549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Finds the range of the argument, determines the optimal number of Newton-Raphson 96649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* iterations and . Corresponding square root of the power of two is returned. 96749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Restrictions: Number is represented as Q1.30. 96849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Number is betweeen the range 2<x<=0 96949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] x, length 1 97049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] pow, length 1 97149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return # of NR iterations, x0 scaled between log(2) and log(4) and 2^N scaling (N=pow) 97249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 97349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint test_limits_and_scale(long *x0, int *pow) 97449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 975cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long lowerlimit, upperlimit, oneiterlothr, oneiterhithr, zeroiterlothr, zeroiterhithr; 976cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 977cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Lower Limit: ll = int32(log(2)*2^30); 978cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro lowerlimit = 744261118L; 979cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //Upper Limit ul = int32(log(4)*2^30); 980cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro upperlimit = 1488522236L; 981cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(0.9*2^30) 982cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro oneiterlothr = 966367642L; 983cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(1.1*2^30) 984cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro oneiterhithr = 1181116006L; 985cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // int32(0.99*2^30) 986cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro zeroiterlothr=1063004406L; 987cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //int32(1.01*2^30) 988cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro zeroiterhithr=1084479242L; 989cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 99049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Scale number such that Newton Raphson iteration works best: 99149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Find the power of two scaling that leaves the number in the optimal range, 99249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // ll <= number <= ul. Note odd powers have special scaling further below 993cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (*x0 > upperlimit) { 994cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Halving the number will push it in the optimal range since largest value is 2 995cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *x0 = *x0>>1; 996cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *pow=-1; 997cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (*x0 < lowerlimit) { 998cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Find position of highest bit, counting from left, and scale number 999cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *pow=get_highest_bit_position((unsigned long*)x0); 1000cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (*x0 >= upperlimit) { 1001cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Halving the number will push it in the optimal range 1002cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *x0 = *x0>>1; 1003cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *pow=*pow-1; 1004cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1005cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro else if (*x0 < lowerlimit) { 1006cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // Doubling the number will push it in the optimal range 1007cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *x0 = *x0<<1; 1008cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *pow=*pow+1; 1009cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1010cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 1011cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *pow = 0; 1012cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1013cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1014cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( *x0<oneiterlothr || *x0>oneiterhithr ) 1015cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 3; // 3 NR iterations 1016cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ( *x0<zeroiterlothr || *x0>zeroiterhithr ) 101749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 2; // 2 NR iteration 101849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 101949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 1; // 1 NR iteration 102049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 102149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 102249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Auxiliary function used by testLimitsAndScale() 102349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Find the highest nonzero bit in an unsigned 32 bit integer: 102449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] value, length 1. 1025cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro* @return highes bit position. 1026cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro**/int get_highest_bit_position(unsigned long *value) 1027cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 1028cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int position; 1029cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position = 0; 1030cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (*value == 0) return 0; 1031cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1032cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((*value & 0xFFFF0000) == 0) { 1033cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position += 16; 1034cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *value=*value<<16; 1035cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1036cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((*value & 0xFF000000) == 0) { 1037cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position += 8; 1038cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *value=*value<<8; 1039cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1040cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((*value & 0xF0000000) == 0) { 1041cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position += 4; 1042cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *value=*value<<4; 1043cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1044cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((*value & 0xC0000000) == 0) { 1045cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position += 2; 1046cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *value=*value<<2; 1047cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1048cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1049cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // If we got too far into sign bit, shift back. Note we are using an 1050cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // unsigned long here, so right shift is going to shift all the bits. 1051cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((*value & 0x80000000)) { 1052cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro position -= 1; 1053cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *value=*value>>1; 1054cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1055cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return position; 105649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 105749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 105849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* compute real part of quaternion, element[0] 105949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow@param[in] inQuat, 3 elements gyro quaternion 106049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow@param[out] outquat, 4 elements gyro quaternion 106149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 106249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_compute_scalar_part(const long * inQuat, long* outQuat) 106349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 106449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long scalarPart = 0; 106549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 106649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow scalarPart = inv_fast_sqrt((1L<<30) - inv_q30_mult(inQuat[0], inQuat[0]) 106749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow - inv_q30_mult(inQuat[1], inQuat[1]) 106849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow - inv_q30_mult(inQuat[2], inQuat[2]) ); 106949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow outQuat[0] = scalarPart; 107049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow outQuat[1] = inQuat[0]; 107149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow outQuat[2] = inQuat[1]; 107249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow outQuat[3] = inQuat[2]; 107349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 107449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 107549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 107649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 107749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @} 107849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 1079