00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "arm_math.h"
00028
00057 static const q15_t cosTableQ15[259] = {
00058 0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
00059 0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
00060 0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
00061 0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
00062 0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
00063 0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
00064 0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
00065 0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
00066 0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
00067 0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
00068 0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
00069 0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
00070 0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
00071 0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
00072 0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
00073 0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
00074 0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
00075 0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
00076 0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
00077 0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
00078 0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
00079 0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
00080 0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
00081 0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
00082 0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
00083 0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
00084 0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
00085 0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
00086 0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
00087 0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
00088 0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
00089 0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
00090 0x7ff6, 0x7fff, 0x7ff6
00091 };
00092
00093
00102 q15_t arm_cos_q15(
00103 q15_t x)
00104 {
00105 q31_t cosVal;
00106 q15_t *tablePtr;
00107 q15_t fract, in, in2;
00108 q31_t wa, wb, wc, wd;
00109 q15_t a, b, c, d;
00110 q15_t fractCube, fractSquare;
00111 q15_t oneBy6 = 0x1555;
00112 q15_t tableSpacing = TABLE_SPACING_Q15;
00113 int32_t index;
00114
00115 in = x;
00116
00117
00118 index = (int32_t) in / tableSpacing;
00119
00120
00121 in2 = (q15_t) index *tableSpacing;
00122
00123
00124 fract = (in - in2) << 8;
00125
00126
00127 fractSquare = (q15_t) ((fract * fract) >> 15);
00128
00129
00130 fractCube = (q15_t) ((fractSquare * fract) >> 15);
00131
00132
00133 tablePtr = (q15_t *) & cosTableQ15[index];
00134
00135
00136
00137
00138 wa = (q31_t) oneBy6 *fractCube;
00139 wa += (q31_t) 0x2AAA * fract;
00140 wa = -(wa >> 15);
00141 wa += (fractSquare >> 1u);
00142
00143
00144 a = *tablePtr++;
00145
00146
00147 cosVal = a * wa;
00148
00149
00150 wb = (((fractCube >> 1u) - fractSquare) - (fract >> 1u)) + 0x7FFF;
00151
00152
00153 b = *tablePtr++;
00154
00155
00156 cosVal += b * wb;
00157
00158
00159 wc = -(q31_t) fractCube + fractSquare;
00160 wc = (wc >> 1u) + fract;
00161
00162
00163 c = *tablePtr++;
00164
00165
00166 cosVal += c * wc;
00167
00168
00169
00170 fractCube = fractCube - fract;
00171 wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
00172
00173
00174 d = *tablePtr++;
00175
00176
00177 cosVal += d * wd;
00178
00179
00180 return ((q15_t) (cosVal >> 15u));
00181
00182 }
00183