Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

Dependents:   Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more

functions/FilteringFunctions/arm_fir_decimate_q31.c@3:4098b9d3d571, 2018-06-21 (annotated)

Committer:
xorjoep
Date:
Thu Jun 21 11:56:27 2018 +0000
Revision:
3:4098b9d3d571
Parent:
1:24714b45cd1b 
headers is a folder not a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 1:24714b45cd1b 1 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 2 * Project: CMSIS DSP Library
xorjoep 1:24714b45cd1b 3 * Title: arm_fir_decimate_q31.c
xorjoep 1:24714b45cd1b 4 * Description: Q31 FIR Decimator
xorjoep 1:24714b45cd1b 5 *
xorjoep 1:24714b45cd1b 6 * $Date: 27. January 2017
xorjoep 1:24714b45cd1b 7 * $Revision: V.1.5.1
xorjoep 1:24714b45cd1b 8 *
xorjoep 1:24714b45cd1b 9 * Target Processor: Cortex-M cores
xorjoep 1:24714b45cd1b 10 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 11 /*
xorjoep 1:24714b45cd1b 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
xorjoep 1:24714b45cd1b 13 *
xorjoep 1:24714b45cd1b 14 * SPDX-License-Identifier: Apache-2.0
xorjoep 1:24714b45cd1b 15 *
xorjoep 1:24714b45cd1b 16 * Licensed under the Apache License, Version 2.0 (the License); you may
xorjoep 1:24714b45cd1b 17 * not use this file except in compliance with the License.
xorjoep 1:24714b45cd1b 18 * You may obtain a copy of the License at
xorjoep 1:24714b45cd1b 19 *
xorjoep 1:24714b45cd1b 20 * www.apache.org/licenses/LICENSE-2.0
xorjoep 1:24714b45cd1b 21 *
xorjoep 1:24714b45cd1b 22 * Unless required by applicable law or agreed to in writing, software
xorjoep 1:24714b45cd1b 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
xorjoep 1:24714b45cd1b 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
xorjoep 1:24714b45cd1b 25 * See the License for the specific language governing permissions and
xorjoep 1:24714b45cd1b 26 * limitations under the License.
xorjoep 1:24714b45cd1b 27 */
xorjoep 1:24714b45cd1b 28
xorjoep 1:24714b45cd1b 29 #include "arm_math.h"
xorjoep 1:24714b45cd1b 30
xorjoep 1:24714b45cd1b 31 /**
xorjoep 1:24714b45cd1b 32 * @ingroup groupFilters
xorjoep 1:24714b45cd1b 33 */
xorjoep 1:24714b45cd1b 34
xorjoep 1:24714b45cd1b 35 /**
xorjoep 1:24714b45cd1b 36 * @addtogroup FIR_decimate
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40 /**
xorjoep 1:24714b45cd1b 41 * @brief Processing function for the Q31 FIR decimator.
xorjoep 1:24714b45cd1b 42 * @param[in] *S points to an instance of the Q31 FIR decimator structure.
xorjoep 1:24714b45cd1b 43 * @param[in] *pSrc points to the block of input data.
xorjoep 1:24714b45cd1b 44 * @param[out] *pDst points to the block of output data
xorjoep 1:24714b45cd1b 45 * @param[in] blockSize number of input samples to process per call.
xorjoep 1:24714b45cd1b 46 * @return none
xorjoep 1:24714b45cd1b 47 *
xorjoep 1:24714b45cd1b 48 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 49 * \par
xorjoep 1:24714b45cd1b 50 * The function is implemented using an internal 64-bit accumulator.
xorjoep 1:24714b45cd1b 51 * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
xorjoep 1:24714b45cd1b 52 * Thus, if the accumulator result overflows it wraps around rather than clip.
xorjoep 1:24714b45cd1b 53 * In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (where log2 is read as log to the base 2).
xorjoep 1:24714b45cd1b 54 * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
xorjoep 1:24714b45cd1b 55 *
xorjoep 1:24714b45cd1b 56 * \par
xorjoep 1:24714b45cd1b 57 * Refer to the function <code>arm_fir_decimate_fast_q31()</code> for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4.
xorjoep 1:24714b45cd1b 58 */
xorjoep 1:24714b45cd1b 59
xorjoep 1:24714b45cd1b 60 void arm_fir_decimate_q31(
xorjoep 1:24714b45cd1b 61 const arm_fir_decimate_instance_q31 * S,
xorjoep 1:24714b45cd1b 62 q31_t * pSrc,
xorjoep 1:24714b45cd1b 63 q31_t * pDst,
xorjoep 1:24714b45cd1b 64 uint32_t blockSize)
xorjoep 1:24714b45cd1b 65 {
xorjoep 1:24714b45cd1b 66 q31_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 67 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 68 q31_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 69 q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 70 q31_t *px; /* Temporary pointers for state buffer */
xorjoep 1:24714b45cd1b 71 q31_t *pb; /* Temporary pointers for coefficient buffer */
xorjoep 1:24714b45cd1b 72 q63_t sum0; /* Accumulator */
xorjoep 1:24714b45cd1b 73 uint32_t numTaps = S->numTaps; /* Number of taps */
xorjoep 1:24714b45cd1b 74 uint32_t i, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */
xorjoep 1:24714b45cd1b 75
xorjoep 1:24714b45cd1b 76
xorjoep 1:24714b45cd1b 77 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 78
xorjoep 1:24714b45cd1b 79 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 80
xorjoep 1:24714b45cd1b 81 /* S->pState buffer contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 82 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 83 pStateCurnt = S->pState + (numTaps - 1U);
xorjoep 1:24714b45cd1b 84
xorjoep 1:24714b45cd1b 85 /* Total number of output samples to be computed */
xorjoep 1:24714b45cd1b 86 blkCnt = outBlockSize;
xorjoep 1:24714b45cd1b 87
xorjoep 1:24714b45cd1b 88 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 89 {
xorjoep 1:24714b45cd1b 90 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 91 i = S->M;
xorjoep 1:24714b45cd1b 92
xorjoep 1:24714b45cd1b 93 do
xorjoep 1:24714b45cd1b 94 {
xorjoep 1:24714b45cd1b 95 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 96
xorjoep 1:24714b45cd1b 97 } while (--i);
xorjoep 1:24714b45cd1b 98
xorjoep 1:24714b45cd1b 99 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 100 sum0 = 0;
xorjoep 1:24714b45cd1b 101
xorjoep 1:24714b45cd1b 102 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 103 px = pState;
xorjoep 1:24714b45cd1b 104
xorjoep 1:24714b45cd1b 105 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 106 pb = pCoeffs;
xorjoep 1:24714b45cd1b 107
xorjoep 1:24714b45cd1b 108 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 109 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 110
xorjoep 1:24714b45cd1b 111 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 112 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 113 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 114 {
xorjoep 1:24714b45cd1b 115 /* Read the b[numTaps-1] coefficient */
xorjoep 1:24714b45cd1b 116 c0 = *(pb++);
xorjoep 1:24714b45cd1b 117
xorjoep 1:24714b45cd1b 118 /* Read x[n-numTaps-1] sample */
xorjoep 1:24714b45cd1b 119 x0 = *(px++);
xorjoep 1:24714b45cd1b 120
xorjoep 1:24714b45cd1b 121 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 122 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 123
xorjoep 1:24714b45cd1b 124 /* Read the b[numTaps-2] coefficient */
xorjoep 1:24714b45cd1b 125 c0 = *(pb++);
xorjoep 1:24714b45cd1b 126
xorjoep 1:24714b45cd1b 127 /* Read x[n-numTaps-2] sample */
xorjoep 1:24714b45cd1b 128 x0 = *(px++);
xorjoep 1:24714b45cd1b 129
xorjoep 1:24714b45cd1b 130 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 131 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 132
xorjoep 1:24714b45cd1b 133 /* Read the b[numTaps-3] coefficient */
xorjoep 1:24714b45cd1b 134 c0 = *(pb++);
xorjoep 1:24714b45cd1b 135
xorjoep 1:24714b45cd1b 136 /* Read x[n-numTaps-3] sample */
xorjoep 1:24714b45cd1b 137 x0 = *(px++);
xorjoep 1:24714b45cd1b 138
xorjoep 1:24714b45cd1b 139 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 140 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 141
xorjoep 1:24714b45cd1b 142 /* Read the b[numTaps-4] coefficient */
xorjoep 1:24714b45cd1b 143 c0 = *(pb++);
xorjoep 1:24714b45cd1b 144
xorjoep 1:24714b45cd1b 145 /* Read x[n-numTaps-4] sample */
xorjoep 1:24714b45cd1b 146 x0 = *(px++);
xorjoep 1:24714b45cd1b 147
xorjoep 1:24714b45cd1b 148 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 149 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 150
xorjoep 1:24714b45cd1b 151 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 152 tapCnt--;
xorjoep 1:24714b45cd1b 153 }
xorjoep 1:24714b45cd1b 154
xorjoep 1:24714b45cd1b 155 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 156 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 157
xorjoep 1:24714b45cd1b 158 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 159 {
xorjoep 1:24714b45cd1b 160 /* Read coefficients */
xorjoep 1:24714b45cd1b 161 c0 = *(pb++);
xorjoep 1:24714b45cd1b 162
xorjoep 1:24714b45cd1b 163 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 164 x0 = *(px++);
xorjoep 1:24714b45cd1b 165
xorjoep 1:24714b45cd1b 166 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 167 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 168
xorjoep 1:24714b45cd1b 169 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 170 tapCnt--;
xorjoep 1:24714b45cd1b 171 }
xorjoep 1:24714b45cd1b 172
xorjoep 1:24714b45cd1b 173 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 174 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 175 pState = pState + S->M;
xorjoep 1:24714b45cd1b 176
xorjoep 1:24714b45cd1b 177 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 178 *pDst++ = (q31_t) (sum0 >> 31);
xorjoep 1:24714b45cd1b 179
xorjoep 1:24714b45cd1b 180 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 181 blkCnt--;
xorjoep 1:24714b45cd1b 182 }
xorjoep 1:24714b45cd1b 183
xorjoep 1:24714b45cd1b 184 /* Processing is complete.
xorjoep 1:24714b45cd1b 185 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 186 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 187
xorjoep 1:24714b45cd1b 188 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 189 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 190
xorjoep 1:24714b45cd1b 191 i = (numTaps - 1U) >> 2U;
xorjoep 1:24714b45cd1b 192
xorjoep 1:24714b45cd1b 193 /* copy data */
xorjoep 1:24714b45cd1b 194 while (i > 0U)
xorjoep 1:24714b45cd1b 195 {
xorjoep 1:24714b45cd1b 196 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 197 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 198 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 199 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 200
xorjoep 1:24714b45cd1b 201 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 202 i--;
xorjoep 1:24714b45cd1b 203 }
xorjoep 1:24714b45cd1b 204
xorjoep 1:24714b45cd1b 205 i = (numTaps - 1U) % 0x04U;
xorjoep 1:24714b45cd1b 206
xorjoep 1:24714b45cd1b 207 /* copy data */
xorjoep 1:24714b45cd1b 208 while (i > 0U)
xorjoep 1:24714b45cd1b 209 {
xorjoep 1:24714b45cd1b 210 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 211
xorjoep 1:24714b45cd1b 212 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 213 i--;
xorjoep 1:24714b45cd1b 214 }
xorjoep 1:24714b45cd1b 215
xorjoep 1:24714b45cd1b 216 #else
xorjoep 1:24714b45cd1b 217
xorjoep 1:24714b45cd1b 218 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 219
xorjoep 1:24714b45cd1b 220 /* S->pState buffer contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 221 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 222 pStateCurnt = S->pState + (numTaps - 1U);
xorjoep 1:24714b45cd1b 223
xorjoep 1:24714b45cd1b 224 /* Total number of output samples to be computed */
xorjoep 1:24714b45cd1b 225 blkCnt = outBlockSize;
xorjoep 1:24714b45cd1b 226
xorjoep 1:24714b45cd1b 227 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 228 {
xorjoep 1:24714b45cd1b 229 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 230 i = S->M;
xorjoep 1:24714b45cd1b 231
xorjoep 1:24714b45cd1b 232 do
xorjoep 1:24714b45cd1b 233 {
xorjoep 1:24714b45cd1b 234 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 235
xorjoep 1:24714b45cd1b 236 } while (--i);
xorjoep 1:24714b45cd1b 237
xorjoep 1:24714b45cd1b 238 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 239 sum0 = 0;
xorjoep 1:24714b45cd1b 240
xorjoep 1:24714b45cd1b 241 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 242 px = pState;
xorjoep 1:24714b45cd1b 243
xorjoep 1:24714b45cd1b 244 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 245 pb = pCoeffs;
xorjoep 1:24714b45cd1b 246
xorjoep 1:24714b45cd1b 247 tapCnt = numTaps;
xorjoep 1:24714b45cd1b 248
xorjoep 1:24714b45cd1b 249 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 250 {
xorjoep 1:24714b45cd1b 251 /* Read coefficients */
xorjoep 1:24714b45cd1b 252 c0 = *pb++;
xorjoep 1:24714b45cd1b 253
xorjoep 1:24714b45cd1b 254 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 255 x0 = *px++;
xorjoep 1:24714b45cd1b 256
xorjoep 1:24714b45cd1b 257 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 258 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 259
xorjoep 1:24714b45cd1b 260 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 261 tapCnt--;
xorjoep 1:24714b45cd1b 262 }
xorjoep 1:24714b45cd1b 263
xorjoep 1:24714b45cd1b 264 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 265 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 266 pState = pState + S->M;
xorjoep 1:24714b45cd1b 267
xorjoep 1:24714b45cd1b 268 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 269 *pDst++ = (q31_t) (sum0 >> 31);
xorjoep 1:24714b45cd1b 270
xorjoep 1:24714b45cd1b 271 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 272 blkCnt--;
xorjoep 1:24714b45cd1b 273 }
xorjoep 1:24714b45cd1b 274
xorjoep 1:24714b45cd1b 275 /* Processing is complete.
xorjoep 1:24714b45cd1b 276 ** Now copy the last numTaps - 1 samples to the start of the state buffer.
xorjoep 1:24714b45cd1b 277 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 278
xorjoep 1:24714b45cd1b 279 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 280 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 281
xorjoep 1:24714b45cd1b 282 i = numTaps - 1U;
xorjoep 1:24714b45cd1b 283
xorjoep 1:24714b45cd1b 284 /* copy data */
xorjoep 1:24714b45cd1b 285 while (i > 0U)
xorjoep 1:24714b45cd1b 286 {
xorjoep 1:24714b45cd1b 287 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 288
xorjoep 1:24714b45cd1b 289 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 290 i--;
xorjoep 1:24714b45cd1b 291 }
xorjoep 1:24714b45cd1b 292
xorjoep 1:24714b45cd1b 293 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 294
xorjoep 1:24714b45cd1b 295 }
xorjoep 1:24714b45cd1b 296
xorjoep 1:24714b45cd1b 297 /**
xorjoep 1:24714b45cd1b 298 * @} end of FIR_decimate group
xorjoep 1:24714b45cd1b 299 */