1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2012 ARM Limited. All rights reserved.
3 *
4 * $Date: 17. January 2013
5 * $Revision: V1.4.0 b
6 *
7 * Project: CMSIS DSP Library
8 *
9 * Title: math_helper.c
10 *
11 * Description: Definition of all helper functions required.
12 *
13 * Target Processor: Cortex-M4/Cortex-M3
14 *
15 * Redistribution and use in source and binary forms, with or without
16 * modification, are permitted provided that the following conditions
17 * are met:
18 * - Redistributions of source code must retain the above copyright
19 * notice, this list of conditions and the following disclaimer.
20 * - Redistributions in binary form must reproduce the above copyright
21 * notice, this list of conditions and the following disclaimer in
22 * the documentation and/or other materials provided with the
23 * distribution.
24 * - Neither the name of ARM LIMITED nor the names of its contributors
25 * may be used to endorse or promote products derived from this
26 * software without specific prior written permission.
27 *
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39 * POSSIBILITY OF SUCH DAMAGE.
40 * -------------------------------------------------------------------- */
41
42 /* ----------------------------------------------------------------------
43 * Include standard header files
44 * -------------------------------------------------------------------- */
45 #include<math.h>
46
47 /* ----------------------------------------------------------------------
48 * Include project header files
49 * -------------------------------------------------------------------- */
50 #include "math_helper.h"
51
52 /**
53 * @brief Caluclation of SNR
54 * @param[in] pRef Pointer to the reference buffer
55 * @param[in] pTest Pointer to the test buffer
56 * @param[in] buffSize total number of samples
57 * @return SNR
58 * The function Caluclates signal to noise ratio for the reference output
59 * and test output
60 */
61
arm_snr_f32(float * pRef,float * pTest,uint32_t buffSize)62 float arm_snr_f32(float *pRef, float *pTest, uint32_t buffSize)
63 {
64 float EnergySignal = 0.0, EnergyError = 0.0;
65 uint32_t i;
66 float SNR;
67 int temp;
68 int *test;
69
70 for (i = 0; i < buffSize; i++)
71 {
72 /* Checking for a NAN value in pRef array */
73 test = (int *)(&pRef[i]);
74 temp = *test;
75
76 if (temp == 0x7FC00000)
77 {
78 return(0);
79 }
80
81 /* Checking for a NAN value in pTest array */
82 test = (int *)(&pTest[i]);
83 temp = *test;
84
85 if (temp == 0x7FC00000)
86 {
87 return(0);
88 }
89 EnergySignal += pRef[i] * pRef[i];
90 EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]);
91 }
92
93 /* Checking for a NAN value in EnergyError */
94 test = (int *)(&EnergyError);
95 temp = *test;
96
97 if (temp == 0x7FC00000)
98 {
99 return(0);
100 }
101
102
103 SNR = 10 * (float32_t)log10 ((double)EnergySignal / (double)EnergyError);
104
105 return (SNR);
106
107 }
108
109
110 /**
111 * @brief Provide guard bits for Input buffer
112 * @param[in,out] input_buf Pointer to input buffer
113 * @param[in] blockSize block Size
114 * @param[in] guard_bits guard bits
115 * @return none
116 * The function Provides the guard bits for the buffer
117 * to avoid overflow
118 */
119
arm_provide_guard_bits_q15(q15_t * input_buf,uint32_t blockSize,uint32_t guard_bits)120 void arm_provide_guard_bits_q15 (q15_t * input_buf, uint32_t blockSize,
121 uint32_t guard_bits)
122 {
123 uint32_t i;
124
125 for (i = 0; i < blockSize; i++)
126 {
127 input_buf[i] = input_buf[i] >> guard_bits;
128 }
129 }
130
131 /**
132 * @brief Converts float to fixed in q12.20 format
133 * @param[in] pIn pointer to input buffer
134 * @param[out] pOut pointer to outputbuffer
135 * @param[in] numSamples number of samples in the input buffer
136 * @return none
137 * The function converts floating point values to fixed point(q12.20) values
138 */
139
arm_float_to_q12_20(float * pIn,q31_t * pOut,uint32_t numSamples)140 void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples)
141 {
142 uint32_t i;
143
144 for (i = 0; i < numSamples; i++)
145 {
146 /* 1048576.0f corresponds to pow(2, 20) */
147 pOut[i] = (q31_t) (pIn[i] * 1048576.0f);
148
149 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
150
151 if (pIn[i] == (float) 1.0)
152 {
153 pOut[i] = 0x000FFFFF;
154 }
155 }
156 }
157
158 /**
159 * @brief Compare MATLAB Reference Output and ARM Test output
160 * @param[in] pIn Pointer to Ref buffer
161 * @param[in] pOut Pointer to Test buffer
162 * @param[in] numSamples number of samples in the buffer
163 * @return maximum difference
164 */
165
arm_compare_fixed_q15(q15_t * pIn,q15_t * pOut,uint32_t numSamples)166 uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t *pOut, uint32_t numSamples)
167 {
168 uint32_t i;
169 int32_t diff, diffCrnt = 0;
170 int32_t maxDiff = 0;
171
172 for (i = 0; i < numSamples; i++)
173 {
174 diff = pIn[i] - pOut[i];
175 diffCrnt = (diff > 0) ? diff : -diff;
176
177 if (diffCrnt > maxDiff)
178 {
179 maxDiff = diffCrnt;
180 }
181 }
182
183 return(maxDiff);
184 }
185
186 /**
187 * @brief Compare MATLAB Reference Output and ARM Test output
188 * @param[in] pIn Pointer to Ref buffer
189 * @param[in] pOut Pointer to Test buffer
190 * @param[in] numSamples number of samples in the buffer
191 * @return maximum difference
192 */
193
arm_compare_fixed_q31(q31_t * pIn,q31_t * pOut,uint32_t numSamples)194 uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t * pOut, uint32_t numSamples)
195 {
196 uint32_t i;
197 int32_t diff, diffCrnt = 0;
198 int32_t maxDiff = 0;
199
200 for (i = 0; i < numSamples; i++)
201 {
202 diff = pIn[i] - pOut[i];
203 diffCrnt = (diff > 0) ? diff : -diff;
204
205 if (diffCrnt > maxDiff)
206 {
207 maxDiff = diffCrnt;
208 }
209 }
210
211 return(maxDiff);
212 }
213
214 /**
215 * @brief Provide guard bits for Input buffer
216 * @param[in,out] input_buf Pointer to input buffer
217 * @param[in] blockSize block Size
218 * @param[in] guard_bits guard bits
219 * @return none
220 * The function Provides the guard bits for the buffer
221 * to avoid overflow
222 */
223
arm_provide_guard_bits_q31(q31_t * input_buf,uint32_t blockSize,uint32_t guard_bits)224 void arm_provide_guard_bits_q31 (q31_t * input_buf,
225 uint32_t blockSize,
226 uint32_t guard_bits)
227 {
228 uint32_t i;
229
230 for (i = 0; i < blockSize; i++)
231 {
232 input_buf[i] = input_buf[i] >> guard_bits;
233 }
234 }
235
236 /**
237 * @brief Provide guard bits for Input buffer
238 * @param[in,out] input_buf Pointer to input buffer
239 * @param[in] blockSize block Size
240 * @param[in] guard_bits guard bits
241 * @return none
242 * The function Provides the guard bits for the buffer
243 * to avoid overflow
244 */
245
arm_provide_guard_bits_q7(q7_t * input_buf,uint32_t blockSize,uint32_t guard_bits)246 void arm_provide_guard_bits_q7 (q7_t * input_buf,
247 uint32_t blockSize,
248 uint32_t guard_bits)
249 {
250 uint32_t i;
251
252 for (i = 0; i < blockSize; i++)
253 {
254 input_buf[i] = input_buf[i] >> guard_bits;
255 }
256 }
257
258
259
260 /**
261 * @brief Caluclates number of guard bits
262 * @param[in] num_adds number of additions
263 * @return guard bits
264 * The function Caluclates the number of guard bits
265 * depending on the numtaps
266 */
267
arm_calc_guard_bits(uint32_t num_adds)268 uint32_t arm_calc_guard_bits (uint32_t num_adds)
269 {
270 uint32_t i = 1, j = 0;
271
272 if (num_adds == 1)
273 {
274 return (0);
275 }
276
277 while (i < num_adds)
278 {
279 i = i * 2;
280 j++;
281 }
282
283 return (j);
284 }
285
286 /**
287 * @brief Apply guard bits to buffer
288 * @param[in,out] pIn pointer to input buffer
289 * @param[in] numSamples number of samples in the input buffer
290 * @param[in] guard_bits guard bits
291 * @return none
292 */
293
arm_apply_guard_bits(float32_t * pIn,uint32_t numSamples,uint32_t guard_bits)294 void arm_apply_guard_bits (float32_t *pIn,
295 uint32_t numSamples,
296 uint32_t guard_bits)
297 {
298 uint32_t i;
299
300 for (i = 0; i < numSamples; i++)
301 {
302 pIn[i] = pIn[i] * arm_calc_2pow(guard_bits);
303 }
304 }
305
306 /**
307 * @brief Calculates pow(2, numShifts)
308 * @param[in] numShifts number of shifts
309 * @return pow(2, numShifts)
310 */
arm_calc_2pow(uint32_t numShifts)311 uint32_t arm_calc_2pow(uint32_t numShifts)
312 {
313
314 uint32_t i, val = 1;
315
316 for (i = 0; i < numShifts; i++)
317 {
318 val = val * 2;
319 }
320
321 return(val);
322 }
323
324
325
326 /**
327 * @brief Converts float to fixed q14
328 * @param[in] pIn pointer to input buffer
329 * @param[out] pOut pointer to output buffer
330 * @param[in] numSamples number of samples in the buffer
331 * @return none
332 * The function converts floating point values to fixed point values
333 */
334
arm_float_to_q14(float * pIn,q15_t * pOut,uint32_t numSamples)335 void arm_float_to_q14 (float *pIn, q15_t *pOut, uint32_t numSamples)
336 {
337 uint32_t i;
338
339 for (i = 0; i < numSamples; i++)
340 {
341 /* 16384.0f corresponds to pow(2, 14) */
342 pOut[i] = (q15_t) (pIn[i] * 16384.0f);
343
344 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
345
346 if (pIn[i] == (float) 2.0)
347 {
348 pOut[i] = 0x7FFF;
349 }
350
351 }
352
353 }
354
355
356 /**
357 * @brief Converts float to fixed q30 format
358 * @param[in] pIn pointer to input buffer
359 * @param[out] pOut pointer to output buffer
360 * @param[in] numSamples number of samples in the buffer
361 * @return none
362 * The function converts floating point values to fixed point values
363 */
364
arm_float_to_q30(float * pIn,q31_t * pOut,uint32_t numSamples)365 void arm_float_to_q30 (float *pIn, q31_t * pOut, uint32_t numSamples)
366 {
367 uint32_t i;
368
369 for (i = 0; i < numSamples; i++)
370 {
371 /* 1073741824.0f corresponds to pow(2, 30) */
372 pOut[i] = (q31_t) (pIn[i] * 1073741824.0f);
373
374 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
375
376 if (pIn[i] == (float) 2.0)
377 {
378 pOut[i] = 0x7FFFFFFF;
379 }
380 }
381 }
382
383 /**
384 * @brief Converts float to fixed q30 format
385 * @param[in] pIn pointer to input buffer
386 * @param[out] pOut pointer to output buffer
387 * @param[in] numSamples number of samples in the buffer
388 * @return none
389 * The function converts floating point values to fixed point values
390 */
391
arm_float_to_q29(float * pIn,q31_t * pOut,uint32_t numSamples)392 void arm_float_to_q29 (float *pIn, q31_t *pOut, uint32_t numSamples)
393 {
394 uint32_t i;
395
396 for (i = 0; i < numSamples; i++)
397 {
398 /* 1073741824.0f corresponds to pow(2, 30) */
399 pOut[i] = (q31_t) (pIn[i] * 536870912.0f);
400
401 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
402
403 if (pIn[i] == (float) 4.0)
404 {
405 pOut[i] = 0x7FFFFFFF;
406 }
407 }
408 }
409
410
411 /**
412 * @brief Converts float to fixed q28 format
413 * @param[in] pIn pointer to input buffer
414 * @param[out] pOut pointer to output buffer
415 * @param[in] numSamples number of samples in the buffer
416 * @return none
417 * The function converts floating point values to fixed point values
418 */
419
arm_float_to_q28(float * pIn,q31_t * pOut,uint32_t numSamples)420 void arm_float_to_q28 (float *pIn, q31_t *pOut, uint32_t numSamples)
421 {
422 uint32_t i;
423
424 for (i = 0; i < numSamples; i++)
425 {
426 /* 268435456.0f corresponds to pow(2, 28) */
427 pOut[i] = (q31_t) (pIn[i] * 268435456.0f);
428
429 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
430
431 if (pIn[i] == (float) 8.0)
432 {
433 pOut[i] = 0x7FFFFFFF;
434 }
435 }
436 }
437
438 /*
439
440 Conflicting with the new clip functions in CMSIS-DSP and not used
441 in the examples.
442
443 */
444 #if 0
445 /**
446 * @brief Clip the float values to +/- 1
447 * @param[in,out] pIn input buffer
448 * @param[in] numSamples number of samples in the buffer
449 * @return none
450 * The function converts floating point values to fixed point values
451 */
452
453 void arm_clip_f32 (float *pIn, uint32_t numSamples)
454 {
455 uint32_t i;
456
457 for (i = 0; i < numSamples; i++)
458 {
459 if (pIn[i] > 1.0f)
460 {
461 pIn[i] = 1.0;
462 }
463 else if ( pIn[i] < -1.0f)
464 {
465 pIn[i] = -1.0;
466 }
467
468 }
469 }
470
471 #endif
472
473
