6轴加速度传感器+姿态角+互补滤波算法+卡尔曼滤波算法

/* ---------------------------------------------------------------------- * Copyright (C) 2010-2014 ARM Limited. All rights reserved. * * $Date: 19. March 2015 * $Revision: V.1.4.5 * * Project: CMSIS DSP Library * Title: arm_common_tables.c * * Description: This file has common tables like fft twiddle factors, Bitreverse, reciprocal etc which are used across different functions * * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * - Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * - Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * - Neither the name of ARM LIMITED nor the names of its contributors * may be used to endorse or promote products derived from this * software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -------------------------------------------------------------------- */ #include "arm_math.h" #include "arm_common_tables.h" /** * @ingroup groupTransforms */ /** * @addtogroup CFFT_CIFFT Complex FFT Tables * @{ */ /** * \par * Pseudo code for Generation of Bit reversal Table is * \par * <pre>for(l=1;l <= N/4;l++) * { * for(i=0;i<logN2;i++) * { * a[i]=l&(1<<i); * } * for(j=0; j<logN2; j++) * { * if (a[j]!=0) * y[l]+=(1<<((logN2-1)-j)); * } * y[l] = y[l] >> 1; * } </pre> * \par * where N = 4096 logN2 = 12 * \par * N is the maximum FFT Size supported */ /* * @brief Table for bit reversal process */ const uint16_t armBitRevTable[1024] = { 0x400, 0x200, 0x600, 0x100, 0x500, 0x300, 0x700, 0x80, 0x480, 0x280, 0x680, 0x180, 0x580, 0x380, 0x780, 0x40, 0x440, 0x240, 0x640, 0x140, 0x540, 0x340, 0x740, 0xc0, 0x4c0, 0x2c0, 0x6c0, 0x1c0, 0x5c0, 0x3c0, 0x7c0, 0x20, 0x420, 0x220, 0x620, 0x120, 0x520, 0x320, 0x720, 0xa0, 0x4a0, 0x2a0, 0x6a0, 0x1a0, 0x5a0, 0x3a0, 0x7a0, 0x60, 0x460, 0x260, 0x660, 0x160, 0x560, 0x360, 0x760, 0xe0, 0x4e0, 0x2e0, 0x6e0, 0x1e0, 0x5e0, 0x3e0, 0x7e0, 0x10, 0x410, 0x210, 0x610, 0x110, 0x510, 0x310, 0x710, 0x90, 0x490, 0x290, 0x690, 0x190, 0x590, 0x390, 0x790, 0x50, 0x450, 0x250, 0x650, 0x150, 0x550, 0x350, 0x750, 0xd0, 0x4d0, 0x2d0, 0x6d0, 0x1d0, 0x5d0, 0x3d0, 0x7d0, 0x30, 0x430, 0x230, 0x630, 0x130, 0x530, 0x330, 0x730, 0xb0, 0x4b0, 0x2b0, 0x6b0, 0x1b0, 0x5b0, 0x3b0, 0x7b0, 0x70, 0x470, 0x270, 0x670, 0x170, 0x570, 0x370, 0x770, 0xf0, 0x4f0, 0x2f0, 0x6f0, 0x1f0, 0x5f0, 0x3f0, 0x7f0, 0x8, 0x408, 0x208, 0x608, 0x108, 0x508, 0x308, 0x708, 0x88, 0x488, 0x288, 0x688, 0x188, 0x588, 0x388, 0x788, 0x48, 0x448, 0x248, 0x648, 0x148, 0x548, 0x348, 0x748, 0xc8, 0x4c8, 0x2c8, 0x6c8, 0x1c8, 0x5c8, 0x3c8, 0x7c8, 0x28, 0x428, 0x228, 0x628, 0x128, 0x528, 0x328, 0x728, 0xa8, 0x4a8, 0x2a8, 0x6a8, 0x1a8, 0x5a8, 0x3a8, 0x7a8, 0x68, 0x468, 0x268, 0x668, 0x168, 0x568, 0x368, 0x768, 0xe8, 0x4e8, 0x2e8, 0x6e8, 0x1e8, 0x5e8, 0x3e8, 0x7e8, 0x18, 0x418, 0x218, 0x618, 0x118, 0x518, 0x318, 0x718, 0x98, 0x498, 0x298, 0x698, 0x198, 0x598, 0x398, 0x798, 0x58, 0x458, 0x258, 0x658, 0x158, 0x558, 0x358, 0x758, 0xd8, 0x4d8, 0x2d8, 0x6d8, 0x1d8, 0x5d8, 0x3d8, 0x7d8, 0x38, 0x438, 0x238, 0x638, 0x138, 0x538, 0x338, 0x738, 0xb8, 0x4b8, 0x2b8, 0x6b8, 0x1b8, 0x5b8, 0x3b8, 0x7b8, 0x78, 0x478, 0x278, 0x678, 0x178, 0x578, 0x378, 0x778, 0xf8, 0x4f8, 0x2f8, 0x6f8, 0x1f8, 0x5f8, 0x3f8, 0x7f8, 0x4, 0x404, 0x204, 0x604, 0x104, 0x504, 0x304, 0x704, 0x84, 0x484, 0x284, 0x684, 0x184, 0x584, 0x384, 0x784, 0x44, 0x444, 0x244, 0x644, 0x144, 0x544, 0x344, 0x744, 0xc4, 0x4c4, 0x2c4, 0x6c4, 0x1c4, 0x5c4, 0x3c4, 0x7c4, 0x24, 0x424, 0x224, 0x624, 0x124, 0x524, 0x324, 0x724, 0xa4, 0x4a4, 0x2a4, 0x6a4, 0x1a4, 0x5a4, 0x3a4, 0x7a4, 0x64, 0x464, 0x264, 0x664, 0x164, 0x564, 0x364, 0x764, 0xe4, 0x4e4, 0x2e4, 0x6e4, 0x1e4, 0x5e4, 0x3e4, 0x7e4, 0x14, 0x414, 0x214, 0x614, 0x114, 0x514, 0x314, 0x714, 0x94, 0x494, 0x294, 0x694, 0x194, 0x594, 0x394, 0x794, 0x54, 0x454, 0x254, 0x654, 0x154, 0x554, 0x354, 0x754, 0xd4, 0x4d4, 0x2d4, 0x6d4, 0x1d4, 0x5d4, 0x3d4, 0x7d4, 0x34, 0x434, 0x234, 0x634, 0x134, 0x534, 0x334, 0x734, 0xb4, 0x4b4, 0x2b4, 0x6b4, 0x1b4, 0x5b4, 0x3b4, 0x7b4, 0x74, 0x474, 0x274, 0x674, 0x174, 0x574, 0x374, 0x774, 0xf4, 0x4f4, 0x2f4, 0x6f4, 0x1f4, 0x5f4, 0x3f4, 0x7f4, 0xc, 0x40c, 0x20c, 0x60c, 0x10c, 0x50c, 0x30c, 0x70c, 0x8c, 0x48c, 0x28c, 0x68c, 0x18c, 0x58c, 0x38c, 0x78c, 0x4c, 0x44c, 0x24c, 0x64c, 0x14c, 0x54c, 0x34c, 0x74c, 0xcc, 0x4cc, 0x2cc, 0x6cc, 0x1cc, 0x5cc, 0x3cc, 0x7cc, 0x2c, 0x42c, 0x22c, 0x62c, 0x12c, 0x52c, 0x32c, 0x72c, 0xac, 0x4ac, 0x2ac, 0x6ac, 0x1ac, 0x5ac, 0x3ac, 0x7ac, 0x6c, 0x46c, 0x26c, 0x66c, 0x16c, 0x56c, 0x36c, 0x76c, 0xec, 0x4ec, 0x2ec, 0x6ec, 0x1ec, 0x5ec, 0x3ec, 0x7ec, 0x1c, 0x41c, 0x21c, 0x61c, 0x11c, 0x51c, 0x31c, 0x71c, 0x9c, 0x49c, 0x29c, 0x69c, 0x19c, 0x59c, 0x39c, 0x79c, 0x5c, 0x45c, 0x25c, 0x65c, 0x15c, 0x55c, 0x35c, 0x75c, 0xdc, 0x4dc, 0x2dc, 0x6dc, 0x1dc, 0x5dc, 0x3dc, 0x7dc, 0x3c, 0x43c, 0x23c, 0x63c, 0x13c, 0x53c, 0x33c, 0x73c, 0xbc, 0x4bc, 0x2bc, 0x6bc, 0x1bc, 0x5bc, 0x3bc, 0x7bc, 0x7c, 0x47c, 0x27c, 0x67c, 0x17c, 0x57c, 0x37c, 0x77c, 0xfc, 0x4fc, 0x2fc, 0x6fc, 0x1fc, 0x5fc, 0x3fc, 0x7fc, 0x2, 0x402, 0x202, 0x602, 0x102, 0x502, 0x302, 0x702, 0x82, 0x482, 0x282, 0x682, 0x182, 0x582, 0x382, 0x782, 0x42, 0x442, 0x242, 0x642, 0x142, 0x542, 0x342, 0x742, 0xc2, 0x4c2, 0x2c2, 0x6c2, 0x1c2, 0x5c2, 0x3c2, 0x7c2, 0x22, 0x422, 0x222, 0x622, 0x122, 0x522, 0x322, 0x722, 0xa2, 0x4a2, 0x2a2, 0x6a2, 0x1a2, 0x5a2, 0x3a2, 0x7a2, 0x62, 0x462, 0x262, 0x662, 0x162, 0x562, 0x362, 0x762, 0xe2, 0x4e2, 0x2e2, 0x6e2, 0x1e2, 0x5e2, 0x3e2, 0x7e2, 0x12, 0x412, 0x212, 0x612, 0x112, 0x512, 0x312, 0x712, 0x92, 0x492, 0x292, 0x692, 0x192, 0x592, 0x392, 0x792, 0x52, 0x452, 0x252, 0x652, 0x152, 0x552, 0x352, 0x752, 0xd2, 0x4d2, 0x2d2, 0x6d2, 0x1d2, 0x5d2, 0x3d2, 0x7d2, 0x32, 0x432, 0x232, 0x632, 0x132, 0x532, 0x332, 0x732, 0xb2, 0x4b2, 0x2b2, 0x6b2, 0x1b2, 0x5b2, 0x3b2, 0x7b2, 0x72, 0x472, 0x272, 0x672, 0x172, 0x572, 0x372, 0x772, 0xf2, 0x4f2, 0x2f2, 0x6f2, 0x1f2, 0x5f2, 0x3f2, 0x7f2, 0xa, 0x40a, 0x20a, 0x60a, 0x10a, 0x50a, 0x30a, 0x70a, 0x8a, 0x48a, 0x28a, 0x68a, 0x18a, 0x58a, 0x38a, 0x78a, 0x4a, 0x44a, 0x24a, 0x64a, 0x14a, 0x54a, 0x34a, 0x74a, 0xca, 0x4ca, 0x2ca, 0x6ca, 0x1ca, 0x5ca, 0x3ca, 0x7ca, 0x2a, 0x42a, 0x22a, 0x62a, 0x12a, 0x52a, 0x32a, 0x72a, 0xaa, 0x4aa, 0x2aa, 0x6aa, 0x1aa, 0x5aa, 0x3aa, 0x7aa, 0x6a, 0x46a, 0x26a, 0x66a, 0x16a, 0x56a, 0x36a, 0x76a, 0xea, 0x4ea, 0