WebSVN – dashGPS – Blame – /branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_cmplx_mag_q31.c
		4	* Description: Q31 complex magnitude
		5	*
		6	* $Date: 27. January 2017
		7	* $Revision: V.1.5.1
		8	*
		9	* Target Processor: Cortex-M cores
		10	* -------------------------------------------------------------------- */
		11	/*
		12	* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
		13	*
		14	* SPDX-License-Identifier: Apache-2.0
		15	*
		16	* Licensed under the Apache License, Version 2.0 (the License); you may
		17	* not use this file except in compliance with the License.
		18	* You may obtain a copy of the License at
		19	*
		20	* www.apache.org/licenses/LICENSE-2.0
		21	*
		22	* Unless required by applicable law or agreed to in writing, software
		23	* distributed under the License is distributed on an AS IS BASIS, WITHOUT
		24	* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
		25	* See the License for the specific language governing permissions and
		26	* limitations under the License.
		27	*/
		28
		29	#include "arm_math.h"
		30
		31	/**
		32	* @ingroup groupCmplxMath
		33	*/
		34
		35	/**
		36	* @addtogroup cmplx_mag
		37	* @{
		38	*/
		39
		40	/**
		41	* @brief Q31 complex magnitude
		42	* @param *pSrc points to the complex input vector
		43	* @param *pDst points to the real output vector
		44	* @param numSamples number of complex samples in the input vector
		45	* @return none.
		46	*
		47	* <b>Scaling and Overflow Behavior:</b>
		48	* \par
		49	* The function implements 1.31 by 1.31 multiplications and finally output is converted into 2.30 format.
		50	* Input down scaling is not required.
		51	*/
		52
		53	void arm_cmplx_mag_q31(
		54	q31_t * pSrc,
		55	q31_t * pDst,
		56	uint32_t numSamples)
		57	{
		58	q31_t real, imag; /* Temporary variables to hold input values */
		59	q31_t acc0, acc1; /* Accumulators */
		60	uint32_t blkCnt; /* loop counter */
		61
		62	#if defined (ARM_MATH_DSP)
		63
		64	/* Run the below code for Cortex-M4 and Cortex-M3 */
		65	q31_t real1, real2, imag1, imag2; /* Temporary variables to hold input values */
		66	q31_t out1, out2, out3, out4; /* Accumulators */
		67	q63_t mul1, mul2, mul3, mul4; /* Temporary variables */
		68
		69
		70	/loop Unrolling /
		71	blkCnt = numSamples >> 2U;
		72
		73	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
		74	** a second loop below computes the remaining 1 to 3 samples. */
		75	while (blkCnt > 0U)
		76	{
		77	/* read complex input from source buffer */
		78	real1 = pSrc[0];
		79	imag1 = pSrc[1];
		80	real2 = pSrc[2];
		81	imag2 = pSrc[3];
		82
		83	/* calculate power of input values */
		84	mul1 = (q63_t) real1 *real1;
		85	mul2 = (q63_t) imag1 *imag1;
		86	mul3 = (q63_t) real2 *real2;
		87	mul4 = (q63_t) imag2 *imag2;
		88
		89	/* get the result to 3.29 format */
		90	out1 = (q31_t) (mul1 >> 33);
		91	out2 = (q31_t) (mul2 >> 33);
		92	out3 = (q31_t) (mul3 >> 33);
		93	out4 = (q31_t) (mul4 >> 33);
		94
		95	/* add real and imaginary accumulators */
		96	out1 = out1 + out2;
		97	out3 = out3 + out4;
		98
		99	/* read complex input from source buffer */
		100	real1 = pSrc[4];
		101	imag1 = pSrc[5];
		102	real2 = pSrc[6];
		103	imag2 = pSrc[7];
		104
		105	/* calculate square root */
		106	arm_sqrt_q31(out1, &pDst[0]);
		107
		108	/* calculate power of input values */
		109	mul1 = (q63_t) real1 *real1;
		110
		111	/* calculate square root */
		112	arm_sqrt_q31(out3, &pDst[1]);
		113
		114	/* calculate power of input values */
		115	mul2 = (q63_t) imag1 *imag1;
		116	mul3 = (q63_t) real2 *real2;
		117	mul4 = (q63_t) imag2 *imag2;
		118
		119	/* get the result to 3.29 format */
		120	out1 = (q31_t) (mul1 >> 33);
		121	out2 = (q31_t) (mul2 >> 33);
		122	out3 = (q31_t) (mul3 >> 33);
		123	out4 = (q31_t) (mul4 >> 33);
		124
		125	/* add real and imaginary accumulators */
		126	out1 = out1 + out2;
		127	out3 = out3 + out4;
		128
		129	/* calculate square root */
		130	arm_sqrt_q31(out1, &pDst[2]);
		131
		132	/* increment destination by 8 to process next samples */
		133	pSrc += 8U;
		134
		135	/* calculate square root */
		136	arm_sqrt_q31(out3, &pDst[3]);
		137
		138	/* increment destination by 4 to process next samples */
		139	pDst += 4U;
		140
		141	/* Decrement the loop counter */
		142	blkCnt--;
		143	}
		144
		145	/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
		146	** No loop unrolling is used. */
		147	blkCnt = numSamples % 0x4U;
		148
		149	#else
		150
		151	/* Run the below code for Cortex-M0 */
		152	blkCnt = numSamples;
		153
		154	#endif /* #if defined (ARM_MATH_DSP) */
		155
		156	while (blkCnt > 0U)
		157	{
		158	/* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
		159	real = *pSrc++;
		160	imag = *pSrc++;
		161	acc0 = (q31_t) (((q63_t) real * real) >> 33);
		162	acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
		163	/* store the result in 2.30 format in the destination buffer. */
		164	arm_sqrt_q31(acc0 + acc1, pDst++);
		165
		166	/* Decrement the loop counter */
		167	blkCnt--;
		168	}
		169	}
		170
		171	/**
		172	* @} end of cmplx_mag group
		173	*/

Subversion Repositories dashGPS

(root)/branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c – Rev 18