Subversion Repositories dashGPS

Rev

Rev 2 | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  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.  */
  111.