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