WebSVN – dashGPS – Blame – /branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_sin_cos_q31.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_sin_cos_q31.c
		4	* Description: Cosine & Sine calculation for Q31 values
		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	#include "arm_common_tables.h"
		31
		32	/**
		33	* @ingroup groupController
		34	*/
		35
		36	/**
		37	* @addtogroup SinCos
		38	* @{
		39	*/
		40
		41	/**
		42	* @brief Q31 sin_cos function.
		43	* @param[in] theta scaled input value in degrees
		44	* @param[out] *pSinVal points to the processed sine output.
		45	* @param[out] *pCosVal points to the processed cosine output.
		46	* @return none.
		47	*
		48	* The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179].
		49	*
		50	*/
		51
		52	void arm_sin_cos_q31(
		53	q31_t theta,
		54	q31_t * pSinVal,
		55	q31_t * pCosVal)
		56	{
		57	q31_t fract; /* Temporary variables for input, output */
		58	uint16_t indexS, indexC; /* Index variable */
		59	q31_t f1, f2, d1, d2; /* Two nearest output values */
		60	q31_t Dn, Df;
		61	q63_t temp;
		62
		63	/* Calculate the nearest index */
		64	indexS = (uint32_t)theta >> CONTROLLER_Q31_SHIFT;
		65	indexC = (indexS + 128) & 0x1ff;
		66
		67	/* Calculation of fractional value */
		68	fract = (theta - (indexS << CONTROLLER_Q31_SHIFT)) << 8;
		69
		70	/* Read two nearest values of input value from the cos & sin tables */
		71	f1 = sinTable_q31[indexC+0];
		72	f2 = sinTable_q31[indexC+1];
		73	d1 = -sinTable_q31[indexS+0];
		74	d2 = -sinTable_q31[indexS+1];
		75
		76	Dn = 0x1921FB5; // delta between the two points (fixed), in this case 2*pi/FAST_MATH_TABLE_SIZE
		77	Df = f2 - f1; // delta between the values of the functions
		78	temp = Dn*((q63_t)d1 + d2);
		79	temp = temp - ((q63_t)Df << 32);
		80	temp = (q63_t)fract*(temp >> 31);
		81	temp = temp + ((3(q63_t)Df << 31) - (d2 + ((q63_t)d1 << 1))Dn);
		82	temp = (q63_t)fract*(temp >> 31);
		83	temp = temp + (q63_t)d1*Dn;
		84	temp = (q63_t)fract*(temp >> 31);
		85
		86	/* Calculation of cosine value */
		87	*pCosVal = clip_q63_to_q31((temp >> 31) + (q63_t)f1);
		88
		89	/* Read two nearest values of input value from the cos & sin tables */
		90	f1 = sinTable_q31[indexS+0];
		91	f2 = sinTable_q31[indexS+1];
		92	d1 = sinTable_q31[indexC+0];
		93	d2 = sinTable_q31[indexC+1];
		94
		95	Df = f2 - f1; // delta between the values of the functions
		96	temp = Dn*((q63_t)d1 + d2);
		97	temp = temp - ((q63_t)Df << 32);
		98	temp = (q63_t)fract*(temp >> 31);
		99	temp = temp + ((3(q63_t)Df << 31) - (d2 + ((q63_t)d1 << 1))Dn);
		100	temp = (q63_t)fract*(temp >> 31);
		101	temp = temp + (q63_t)d1*Dn;
		102	temp = (q63_t)fract*(temp >> 31);
		103
		104	/* Calculation of sine value */
		105	*pSinVal = clip_q63_to_q31((temp >> 31) + (q63_t)f1);
		106	}
		107
		108	/**
		109	* @} end of SinCos group
		110	*/

Subversion Repositories dashGPS

(root)/branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_sin_cos_q31.c – Rev 2