17494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* 27494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $License: 37494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 47494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall See included License.txt for License information. 57494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $ 67494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 77494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 87494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/******************************************************************************* 97494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * $Id:$ 117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ******************************************************************************/ 137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @defgroup ML_MATH_FUNC ml_math_func 167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Motion Library - Math Functions 177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Common math functions the Motion Library 187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @{ 207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @file ml_math_func.c 217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Math Functions. 227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "mlmath.h" 257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "ml_math_func.h" 267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "mlinclude.h" 277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include <string.h> 287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** @internal 307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Does the cross product of compass by gravity, then converts that 317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * to the world frame using the quaternion, then computes the angle that 327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * is made. 337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] compass Compass Vector (Body Frame), length 3 357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] grav Gravity Vector (Body Frame), length 3 367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] quat Quaternion, Length 4 377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Angle Cross Product makes after quaternion rotation. 387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallfloat inv_compass_angle(const long *compass, const long *grav, const float *quat) 407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float cgcross[4], q1[4], q2[4], qi[4]; 427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float angW; 437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Compass cross Gravity 457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cgcross[0] = 0.f; 467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; 477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; 487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; 497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Now convert cross product into world frame 517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_multf(quat, cgcross, q1); 527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_invertf(quat, qi); 537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_multf(q1, qi, q2); 547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Protect against atan2 of 0,0 567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if ((q2[2] == 0.f) && (q2[1] == 0.f)) 577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return 0.f; 587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // This is the unfiltered heading correction 607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall angW = -atan2f(q2[2], q2[1]); 617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return angW; 627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief The gyro data magnitude squared : 667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. 677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] gyro Gyro data scaled with 1 dps = 2^16 687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return the computed magnitude squared output of the gyroscope. 697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallunsigned long inv_get_gyro_sum_of_sqr(const long *gyro) 717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned long gmag = 0; 737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long temp; 747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk; 757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk = 0; kk < 3; ++kk) { 777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); 787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall gmag += temp * temp; 797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return gmag; 827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a multiply and shift by 29. These are good functions to write in assembly on 857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * with devices with small memory where you want to get rid of the long long which some 867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * assemblers don't handle well 877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] a 887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] b 897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return ((long long)a*b)>>29 907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_q29_mult(long a, long b) 927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef UMPL_ELIMINATE_64BIT 947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)((float)a * b / (1L << 29)); 967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#else 987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long long temp; 997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 1007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp = (long long)a * b; 1017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)(temp >> 29); 1027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 1037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a multiply and shift by 30. These are good functions to write in assembly on 1077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * with devices with small memory where you want to get rid of the long long which some 1087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * assemblers don't handle well 1097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] a 1107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] b 1117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return ((long long)a*b)>>30 1127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_q30_mult(long a, long b) 1147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef UMPL_ELIMINATE_64BIT 1167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 1177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)((float)a * b / (1L << 30)); 1187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 1197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#else 1207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long long temp; 1217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 1227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp = (long long)a * b; 1237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)(temp >> 30); 1247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 1257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifndef UMPL_ELIMINATE_64BIT 1297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_q30_div(long a, long b) 1307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long long temp; 1327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 1337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall temp = (((long long)a) << 30) / b; 1347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)temp; 1357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 1367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a multiply and shift by shift. These are good functions to write 1407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * in assembly on with devices with small memory where you want to get rid of 1417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * the long long which some assemblers don't handle well 1427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] a First multicand 1437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] b Second multicand 1447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] shift Shift amount after multiplying 1457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return ((long long)a*b)<<shift 1467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifndef UMPL_ELIMINATE_64BIT 1487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_q_shift_mult(long a, long b, int shift) 1497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long result; 1517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall result = (long)(((long long)a * b) >> shift); 1527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return result; 1537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a fixed point quaternion multiply. 1577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled 1587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to 2^30 1597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled 1607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to 2^30 1617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] qProd Product after quaternion multiply. Length 4. 1627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* 1.0 scaled to 2^30. 1637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_mult(const long *q1, const long *q2, long *qProd) 1657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 1677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - 1687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); 1697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + 1717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); 1727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + 1747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); 1757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - 1777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); 1787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a fixed point quaternion addition. 1817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] q1 First Quaternion term, length 4. 1.0 scaled 1827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to 2^30 1837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled 1847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to 2^30 1857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] qSum Sum after quaternion summation. Length 4. 1867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* 1.0 scaled to 2^30. 1877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_add(long *q1, long *q2, long *qSum) 1897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 1917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[0] = q1[0] + q2[0]; 1927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[1] = q1[1] + q2[1]; 1937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[2] = q1[2] + q2[2]; 1947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[3] = q1[3] + q2[3]; 1957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_vector_normalize(long *vec, int length) 1987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double normSF = 0; 2017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int ii; 2027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (ii = 0; ii < length; ii++) { 2037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF += 2047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); 2057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (normSF > 0) { 2077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = 1 / sqrt(normSF); 2087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (ii = 0; ii < length; ii++) { 2097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall vec[ii] = (int)((double)vec[ii] * normSF); 2107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 2127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall vec[0] = 1073741824L; 2137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (ii = 1; ii < length; ii++) { 2147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall vec[ii] = 0; 2157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_normalize(long *q) 2207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_vector_normalize(q, 4); 2237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_invert(const long *q, long *qInverted) 2267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[0] = q[0]; 2297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[1] = -q[1]; 2307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[2] = -q[2]; 2317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[3] = -q[3]; 2327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralldouble quaternion_to_rotation_angle(const long *quat) { 2357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double quat0 = (double )quat[0] / 1073741824; 2367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (quat0 > 1.0f) { 2377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall quat0 = 1.0; 2387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else if (quat0 < -1.0f) { 2397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall quat0 = -1.0; 2407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 2417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return acos(quat0)*2*180/M_PI; 2437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Rotates a 3-element vector by Rotation defined by Q 2467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 2477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_rotate(const long *q, const long *in, long *out) 2487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long q_temp1[4], q_temp2[4]; 2507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long in4[4], out4[4]; 2517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Fixme optimize 2537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall in4[0] = 0; 2547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(&in4[1], in, 3 * sizeof(long)); 2557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_mult(q, in4, q_temp1); 2567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_invert(q, q_temp2); 2577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q_mult(q_temp1, q_temp2, out4); 2587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memcpy(out, &out4[1], 3 * sizeof(long)); 2597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_multf(const float *q1, const float *q2, float *qProd) 2627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[0] = 2657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); 2667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[1] = 2677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); 2687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[2] = 2697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); 2707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qProd[3] = 2717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); 2727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_addf(const float *q1, const float *q2, float *qSum) 2757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[0] = q1[0] + q2[0]; 2787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[1] = q1[1] + q2[1]; 2797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[2] = q1[2] + q2[2]; 2807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qSum[3] = q1[3] + q2[3]; 2817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 2827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_normalizef(float *q) 2847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 2857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 2867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float normSF = 0; 2877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float xHalf = 0; 2887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 2897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (normSF < 2) { 2907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall xHalf = 0.5f * normSF; 2917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 2927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 2937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 2947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 2957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[0] *= normSF; 2967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[1] *= normSF; 2977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[2] *= normSF; 2987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[3] *= normSF; 2997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 3007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[0] = 1.0; 3017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[1] = 0.0; 3027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[2] = 0.0; 3037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[3] = 0.0; 3047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 3067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Performs a length 4 vector normalization with a square root. 3097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. 3107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 3117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_norm4(float *q) 3127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float mag; 3147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 3157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (mag) { 3167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[0] /= mag; 3177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[1] /= mag; 3187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[2] /= mag; 3197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[3] /= mag; 3207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 3217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[0] = 1.f; 3227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[1] = 0.f; 3237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[2] = 0.f; 3247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall q[3] = 0.f; 3257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_q_invertf(const float *q, float *qInverted) 3297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall INVENSENSE_FUNC_START; 3317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[0] = q[0]; 3327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[1] = -q[1]; 3337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[2] = -q[2]; 3347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall qInverted[3] = -q[3]; 3357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 3387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Converts a quaternion to a rotation matrix. 3397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] quat 4-element quaternion in fixed point. One is 2^30. 3407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] rot Rotation matrix in fixed point. One is 2^30. The 3417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * First 3 elements of the rotation matrix, represent 3427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * the first row of the matrix. Rotation matrix multiplied 3437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * by a 3 element column vector transform a vector from Body 3447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * to World. 3457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 3467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_quaternion_to_rotation(const long *quat, long *rot) 3477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[0] = 3497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], 3507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall quat[0]) - 3517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1073741824L; 3527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[1] = 3537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); 3547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[2] = 3557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); 3567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[3] = 3577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); 3587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[4] = 3597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], 3607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall quat[0]) - 3617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1073741824L; 3627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[5] = 3637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); 3647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[6] = 3657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); 3667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[7] = 3677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); 3687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[8] = 3697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], 3707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall quat[0]) - 3717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1073741824L; 3727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 3757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Converts a quaternion to a rotation vector. A rotation vector is 3767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * a method to represent a 4-element quaternion vector in 3-elements. 3777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * To get the quaternion from the 3-elements, The last 3-elements of 3787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * the quaternion will be the given rotation vector. The first element 3797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * of the quaternion will be the positive value that will be required 3807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. 3817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] quat 4-element quaternion in fixed point. One is 2^30. 3827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[out] rot Rotation vector in fixed point. One is 2^30. 3837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 3847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_quaternion_to_rotation_vector(const long *quat, long *rot) 3857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 3867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[0] = quat[1]; 3877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[1] = quat[2]; 3887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[2] = quat[3]; 3897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (quat[0] < 0.0) { 3917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[0] = -rot[0]; 3927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[1] = -rot[1]; 3937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall rot[2] = -rot[2]; 3947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 3957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 3967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 3977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts a 32-bit long to a big endian byte stream */ 3987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallunsigned char *inv_int32_to_big8(long x, unsigned char *big8) 3997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[0] = (unsigned char)((x >> 24) & 0xff); 4017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[1] = (unsigned char)((x >> 16) & 0xff); 4027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[2] = (unsigned char)((x >> 8) & 0xff); 4037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[3] = (unsigned char)(x & 0xff); 4047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return big8; 4057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts a big endian byte stream into a 32-bit long */ 4087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralllong inv_big8_to_int32(const unsigned char *big8) 4097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long x; 4117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) 4127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall | ((long)big8[3]); 4137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return x; 4147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts a big endian byte stream into a 16-bit integer (short) */ 4177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallshort inv_big8_to_int16(const unsigned char *big8) 4187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall short x; 4207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall x = ((short)big8[0] << 8) | ((short)big8[1]); 4217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return x; 4227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts a little endian byte stream into a 16-bit integer (short) */ 4257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallshort inv_little8_to_int16(const unsigned char *little8) 4267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall short x; 4287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall x = ((short)little8[1] << 8) | ((short)little8[0]); 4297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return x; 4307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts a 16-bit short to a big endian byte stream */ 4337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallunsigned char *inv_int16_to_big8(short x, unsigned char *big8) 4347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[0] = (unsigned char)((x >> 8) & 0xff); 4367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall big8[1] = (unsigned char)(x & 0xff); 4377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return big8; 4387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) 4417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int k, l, i, j; 4437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (i = 0, k = 0; i < *n; i++, k++) { 4447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (j = 0, l = 0; j < *n; j++, l++) { 4457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (i == x) 4467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall i++; 4477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (j == y) 4487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall j++; 4497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *(b + 6 * k + l) = *(a + 6 * i + j); 4507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *n = *n - 1; 4537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) 4567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int k, l, i, j; 4587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (i = 0, k = 0; i < *n; i++, k++) { 4597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (j = 0, l = 0; j < *n; j++, l++) { 4607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (i == x) 4617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall i++; 4627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (j == y) 4637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall j++; 4647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *(b + 6 * k + l) = *(a + 6 * i + j); 4657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *n = *n - 1; 4687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallfloat inv_matrix_det(float *p, int *n) 4717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float d[6][6], sum = 0; 4737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int i, j, m; 4747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall m = *n; 4757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (*n == 2) 4767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (*p ** (p + 7) - *(p + 1) ** (p + 6)); 4777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (i = 0, j = 0; j < m; j++) { 4787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *n = m; 4797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_matrix_det_inc(p, &d[0][0], n, i, j); 4807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sum = 4817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sum + *(p + 6 * i + j) * SIGNM(i + 4827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall j) * 4837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_matrix_det(&d[0][0], n); 4847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 4857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (sum); 4877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 4887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 4897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralldouble inv_matrix_detd(double *p, int *n) 4907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 4917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double d[6][6], sum = 0; 4927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int i, j, m; 4937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall m = *n; 4947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (*n == 2) 4957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (*p ** (p + 7) - *(p + 1) ** (p + 6)); 4967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (i = 0, j = 0; j < m; j++) { 4977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *n = m; 4987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_matrix_det_incd(p, &d[0][0], n, i, j); 4997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sum = 5007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall sum + *(p + 6 * i + j) * SIGNM(i + 5017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall j) * 5027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_matrix_detd(&d[0][0], n); 5037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (sum); 5067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Wraps angle from (-M_PI,M_PI] 5097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] ang Angle in radians to wrap 5107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return Wrapped angle from (-M_PI,M_PI] 5117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 5127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallfloat inv_wrap_angle(float ang) 5137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (ang > M_PI) 5157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return ang - 2 * (float)M_PI; 5167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (ang <= -(float)M_PI) 5177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return ang + 2 * (float)M_PI; 5187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else 5197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return ang; 5207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Finds the minimum angle difference ang1-ang2 such that difference 5237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * is between [-M_PI,M_PI] 5247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] ang1 5257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @param[in] ang2 5267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return angle difference ang1-ang2 5277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 5287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallfloat inv_angle_diff(float ang1, float ang2) 5297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float d; 5317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ang1 = inv_wrap_angle(ang1); 5327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ang2 = inv_wrap_angle(ang2); 5337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall d = ang1 - ang2; 5347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (d > M_PI) 5357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall d -= 2 * (float)M_PI; 5367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (d < -(float)M_PI) 5377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall d += 2 * (float)M_PI; 5387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return d; 5397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** bernstein hash, derived from public domain source */ 5427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralluint32_t inv_checksum(const unsigned char *str, int len) 5437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall uint32_t hash = 5381; 5457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int i, c; 5467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (i = 0; i < len; i++) { 5487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall c = *(str + i); 5497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ 5507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 5517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return hash; 5537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic unsigned short inv_row_2_scale(const signed char *row) 5567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned short b; 5587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (row[0] > 0) 5607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 0; 5617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (row[0] < 0) 5627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 4; 5637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (row[1] > 0) 5647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 1; 5657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (row[1] < 0) 5667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 5; 5677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (row[2] > 0) 5687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 2; 5697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else if (row[2] < 0) 5707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 6; 5717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall else 5727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall b = 7; // error 5737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return b; 5747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 5757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. 5787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] mtx Orientation matrix to convert to a scalar. 5797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the 5807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent 5817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* the column the one is on for the second row with bit number 5 being the sign. 5827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* The next 2 bits (6 and 7) represent the column the one is on for the third row with 5837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* bit number 8 being the sign. In binary the identity matrix would therefor be: 5847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* 010_001_000 or 0x88 in hex. 5857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 5867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallunsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) 5877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 5887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned short scalar; 5907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 5917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* 5927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall XYZ 010_001_000 Identity Matrix 5937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall XZY 001_010_000 5947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall YXZ 010_000_001 5957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall YZX 000_010_001 5967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ZXY 001_000_010 5977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ZYX 000_001_010 5987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 5997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall scalar = inv_row_2_scale(mtx); 6017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall scalar |= inv_row_2_scale(mtx + 3) << 3; 6027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall scalar |= inv_row_2_scale(mtx + 6) << 6; 6037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return scalar; 6067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Uses the scalar orientation value to convert from chip frame to body frame 6097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar that represent how to go from chip to body frame 6107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] input Input vector, length 3 6117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] output Output vector, length 3 6127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_convert_to_body(unsigned short orientation, const long *input, long *output) 6147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); 6167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); 6177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); 6187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Uses the scalar orientation value to convert from body frame to chip frame 6217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar that represent how to go from chip to body frame 6227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] input Input vector, length 3 6237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] output Output vector, length 3 6247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_convert_to_chip(unsigned short orientation, const long *input, long *output) 6267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); 6287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); 6297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); 6307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Uses the scalar orientation value to convert from chip frame to body frame and 6347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* apply appropriate scaling. 6357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] orientation A scalar that represent how to go from chip to body frame 6367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sensitivity Sensitivity scale 6377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] input Input vector, length 3 6387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] output Output vector, length 3 6397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output) 6417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[0] = inv_q30_mult(input[orientation & 0x03] * 6437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall SIGNSET(orientation & 0x004), sensitivity); 6447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * 6457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall SIGNSET(orientation & 0x020), sensitivity); 6467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * 6477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall SIGNSET(orientation & 0x100), sensitivity); 6487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 6497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 6507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** find a norm for a vector 65133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @param[in] x a vector [3x1] 65233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall* @return the normalize vector. 6537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 6547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralldouble inv_vector_norm(const float *x) 6557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 6567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); 6577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 65833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 65933ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { 66033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall int i; 66133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // initial state to zero 66233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[0] = 0; 66333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[1] = 0; 66433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 66533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // set up coefficients 66633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall for (i=0; i<5; i++) { 66733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->c[i] = pBiquadCoeff[i]; 66833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } 66933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 67033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 67133ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) 67233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall{ 67333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float divider; 67433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->input = input; 67533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->output = input; 67633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); 67733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[1] = pFilter->state[0]; 67833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 67933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 68033ce91b37062fa63af192f5643de93f3beebe854JP Abgrallfloat inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { 68133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float stateZero; 68233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 68333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->input = input; 68433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // calculate the new state; 68533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] 68633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall - pFilter->c[3]*pFilter->state[1]; 68733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 68833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] 68933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall + pFilter->c[1]*pFilter->state[1]; 69033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 69133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall // update the output and state 69233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->output = pFilter->output * pFilter->c[4]; 69333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[1] = pFilter->state[0]; 69433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall pFilter->state[0] = stateZero; 69533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall return pFilter->output; 69633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 69733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 69833ce91b37062fa63af192f5643de93f3beebe854JP Abgrallvoid inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { 69933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 70033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; 70133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; 70233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; 70333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall} 70433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 7057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 7067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @} 7077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 708