Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

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

functions/FilteringFunctions/arm_lms_norm_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_lms_norm_q31.c
xorjoep 1:24714b45cd1b 4 * Description: Processing function for the Q31 NLMS filter
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 LMS_NORM
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40 /**
xorjoep 1:24714b45cd1b 41 * @brief Processing function for Q31 normalized LMS filter.
xorjoep 1:24714b45cd1b 42 * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
xorjoep 1:24714b45cd1b 43 * @param[in] *pSrc points to the block of input data.
xorjoep 1:24714b45cd1b 44 * @param[in] *pRef points to the block of reference data.
xorjoep 1:24714b45cd1b 45 * @param[out] *pOut points to the block of output data.
xorjoep 1:24714b45cd1b 46 * @param[out] *pErr points to the block of error data.
xorjoep 1:24714b45cd1b 47 * @param[in] blockSize number of samples to process.
xorjoep 1:24714b45cd1b 48 * @return none.
xorjoep 1:24714b45cd1b 49 *
xorjoep 1:24714b45cd1b 50 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 51 * \par
xorjoep 1:24714b45cd1b 52 * The function is implemented using an internal 64-bit accumulator.
xorjoep 1:24714b45cd1b 53 * The accumulator has a 2.62 format and maintains full precision of the intermediate
xorjoep 1:24714b45cd1b 54 * multiplication results but provides only a single guard bit.
xorjoep 1:24714b45cd1b 55 * Thus, if the accumulator result overflows it wraps around rather than clip.
xorjoep 1:24714b45cd1b 56 * In order to avoid overflows completely the input signal must be scaled down by
xorjoep 1:24714b45cd1b 57 * log2(numTaps) bits. The reference signal should not be scaled down.
xorjoep 1:24714b45cd1b 58 * After all multiply-accumulates are performed, the 2.62 accumulator is shifted
xorjoep 1:24714b45cd1b 59 * and saturated to 1.31 format to yield the final result.
xorjoep 1:24714b45cd1b 60 * The output signal and error signal are in 1.31 format.
xorjoep 1:24714b45cd1b 61 *
xorjoep 1:24714b45cd1b 62 * \par
xorjoep 1:24714b45cd1b 63 * In this filter, filter coefficients are updated for each sample and the
xorjoep 1:24714b45cd1b 64 * updation of filter cofficients are saturted.
xorjoep 1:24714b45cd1b 65 *
xorjoep 1:24714b45cd1b 66 */
xorjoep 1:24714b45cd1b 67
xorjoep 1:24714b45cd1b 68 void arm_lms_norm_q31(
xorjoep 1:24714b45cd1b 69 arm_lms_norm_instance_q31 * S,
xorjoep 1:24714b45cd1b 70 q31_t * pSrc,
xorjoep 1:24714b45cd1b 71 q31_t * pRef,
xorjoep 1:24714b45cd1b 72 q31_t * pOut,
xorjoep 1:24714b45cd1b 73 q31_t * pErr,
xorjoep 1:24714b45cd1b 74 uint32_t blockSize)
xorjoep 1:24714b45cd1b 75 {
xorjoep 1:24714b45cd1b 76 q31_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 77 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 78 q31_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 79 q31_t *px, *pb; /* Temporary pointers for state and coefficient buffers */
xorjoep 1:24714b45cd1b 80 q31_t mu = S->mu; /* Adaptive factor */
xorjoep 1:24714b45cd1b 81 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
xorjoep 1:24714b45cd1b 82 uint32_t tapCnt, blkCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 83 q63_t energy; /* Energy of the input */
xorjoep 1:24714b45cd1b 84 q63_t acc; /* Accumulator */
xorjoep 1:24714b45cd1b 85 q31_t e = 0, d = 0; /* error, reference data sample */
xorjoep 1:24714b45cd1b 86 q31_t w = 0, in; /* weight factor and state */
xorjoep 1:24714b45cd1b 87 q31_t x0; /* temporary variable to hold input sample */
xorjoep 1:24714b45cd1b 88 // uint32_t shift = 32U - ((uint32_t) S->postShift + 1U); /* Shift to be applied to the output */
xorjoep 1:24714b45cd1b 89 q31_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
xorjoep 1:24714b45cd1b 90 q31_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
xorjoep 1:24714b45cd1b 91 q31_t coef; /* Temporary variable for coef */
xorjoep 1:24714b45cd1b 92 q31_t acc_l, acc_h; /* temporary input */
xorjoep 1:24714b45cd1b 93 uint32_t uShift = ((uint32_t) S->postShift + 1U);
xorjoep 1:24714b45cd1b 94 uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
xorjoep 1:24714b45cd1b 95
xorjoep 1:24714b45cd1b 96 energy = S->energy;
xorjoep 1:24714b45cd1b 97 x0 = S->x0;
xorjoep 1:24714b45cd1b 98
xorjoep 1:24714b45cd1b 99 /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 100 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 101 pStateCurnt = &(S->pState[(numTaps - 1U)]);
xorjoep 1:24714b45cd1b 102
xorjoep 1:24714b45cd1b 103 /* Loop over blockSize number of values */
xorjoep 1:24714b45cd1b 104 blkCnt = blockSize;
xorjoep 1:24714b45cd1b 105
xorjoep 1:24714b45cd1b 106
xorjoep 1:24714b45cd1b 107 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 108
xorjoep 1:24714b45cd1b 109 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 110
xorjoep 1:24714b45cd1b 111 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 112 {
xorjoep 1:24714b45cd1b 113
xorjoep 1:24714b45cd1b 114 /* Copy the new input sample into the state buffer */
xorjoep 1:24714b45cd1b 115 *pStateCurnt++ = *pSrc;
xorjoep 1:24714b45cd1b 116
xorjoep 1:24714b45cd1b 117 /* Initialize pState pointer */
xorjoep 1:24714b45cd1b 118 px = pState;
xorjoep 1:24714b45cd1b 119
xorjoep 1:24714b45cd1b 120 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 121 pb = (pCoeffs);
xorjoep 1:24714b45cd1b 122
xorjoep 1:24714b45cd1b 123 /* Read the sample from input buffer */
xorjoep 1:24714b45cd1b 124 in = *pSrc++;
xorjoep 1:24714b45cd1b 125
xorjoep 1:24714b45cd1b 126 /* Update the energy calculation */
xorjoep 1:24714b45cd1b 127 energy = (q31_t) ((((q63_t) energy << 32) -
xorjoep 1:24714b45cd1b 128 (((q63_t) x0 * x0) << 1)) >> 32);
xorjoep 1:24714b45cd1b 129 energy = (q31_t) (((((q63_t) in * in) << 1) + (energy << 32)) >> 32);
xorjoep 1:24714b45cd1b 130
xorjoep 1:24714b45cd1b 131 /* Set the accumulator to zero */
xorjoep 1:24714b45cd1b 132 acc = 0;
xorjoep 1:24714b45cd1b 133
xorjoep 1:24714b45cd1b 134 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 135 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 136
xorjoep 1:24714b45cd1b 137 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 138 {
xorjoep 1:24714b45cd1b 139 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 140 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 141 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 142 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 143 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 144
xorjoep 1:24714b45cd1b 145 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 146 tapCnt--;
xorjoep 1:24714b45cd1b 147 }
xorjoep 1:24714b45cd1b 148
xorjoep 1:24714b45cd1b 149 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 150 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 151
xorjoep 1:24714b45cd1b 152 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 153 {
xorjoep 1:24714b45cd1b 154 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 155 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 156
xorjoep 1:24714b45cd1b 157 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 158 tapCnt--;
xorjoep 1:24714b45cd1b 159 }
xorjoep 1:24714b45cd1b 160
xorjoep 1:24714b45cd1b 161 /* Converting the result to 1.31 format */
xorjoep 1:24714b45cd1b 162 /* Calc lower part of acc */
xorjoep 1:24714b45cd1b 163 acc_l = acc & 0xffffffff;
xorjoep 1:24714b45cd1b 164
xorjoep 1:24714b45cd1b 165 /* Calc upper part of acc */
xorjoep 1:24714b45cd1b 166 acc_h = (acc >> 32) & 0xffffffff;
xorjoep 1:24714b45cd1b 167
xorjoep 1:24714b45cd1b 168 acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
xorjoep 1:24714b45cd1b 169
xorjoep 1:24714b45cd1b 170 /* Store the result from accumulator into the destination buffer. */
xorjoep 1:24714b45cd1b 171 *pOut++ = (q31_t) acc;
xorjoep 1:24714b45cd1b 172
xorjoep 1:24714b45cd1b 173 /* Compute and store error */
xorjoep 1:24714b45cd1b 174 d = *pRef++;
xorjoep 1:24714b45cd1b 175 e = d - (q31_t) acc;
xorjoep 1:24714b45cd1b 176 *pErr++ = e;
xorjoep 1:24714b45cd1b 177
xorjoep 1:24714b45cd1b 178 /* Calculates the reciprocal of energy */
xorjoep 1:24714b45cd1b 179 postShift = arm_recip_q31(energy + DELTA_Q31,
xorjoep 1:24714b45cd1b 180 &oneByEnergy, &S->recipTable[0]);
xorjoep 1:24714b45cd1b 181
xorjoep 1:24714b45cd1b 182 /* Calculation of product of (e * mu) */
xorjoep 1:24714b45cd1b 183 errorXmu = (q31_t) (((q63_t) e * mu) >> 31);
xorjoep 1:24714b45cd1b 184
xorjoep 1:24714b45cd1b 185 /* Weighting factor for the normalized version */
xorjoep 1:24714b45cd1b 186 w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift));
xorjoep 1:24714b45cd1b 187
xorjoep 1:24714b45cd1b 188 /* Initialize pState pointer */
xorjoep 1:24714b45cd1b 189 px = pState;
xorjoep 1:24714b45cd1b 190
xorjoep 1:24714b45cd1b 191 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 192 pb = (pCoeffs);
xorjoep 1:24714b45cd1b 193
xorjoep 1:24714b45cd1b 194 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 195 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 196
xorjoep 1:24714b45cd1b 197 /* Update filter coefficients */
xorjoep 1:24714b45cd1b 198 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 199 {
xorjoep 1:24714b45cd1b 200 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 201
xorjoep 1:24714b45cd1b 202 /* coef is in 2.30 format */
xorjoep 1:24714b45cd1b 203 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 204 /* get coef in 1.31 format by left shifting */
xorjoep 1:24714b45cd1b 205 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 206 /* update coefficient buffer to next coefficient */
xorjoep 1:24714b45cd1b 207 pb++;
xorjoep 1:24714b45cd1b 208
xorjoep 1:24714b45cd1b 209 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 210 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 211 pb++;
xorjoep 1:24714b45cd1b 212
xorjoep 1:24714b45cd1b 213 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 214 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 215 pb++;
xorjoep 1:24714b45cd1b 216
xorjoep 1:24714b45cd1b 217 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 218 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 219 pb++;
xorjoep 1:24714b45cd1b 220
xorjoep 1:24714b45cd1b 221 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 222 tapCnt--;
xorjoep 1:24714b45cd1b 223 }
xorjoep 1:24714b45cd1b 224
xorjoep 1:24714b45cd1b 225 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 226 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 227
xorjoep 1:24714b45cd1b 228 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 229 {
xorjoep 1:24714b45cd1b 230 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 231 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 232 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 233 pb++;
xorjoep 1:24714b45cd1b 234
xorjoep 1:24714b45cd1b 235 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 236 tapCnt--;
xorjoep 1:24714b45cd1b 237 }
xorjoep 1:24714b45cd1b 238
xorjoep 1:24714b45cd1b 239 /* Read the sample from state buffer */
xorjoep 1:24714b45cd1b 240 x0 = *pState;
xorjoep 1:24714b45cd1b 241
xorjoep 1:24714b45cd1b 242 /* Advance state pointer by 1 for the next sample */
xorjoep 1:24714b45cd1b 243 pState = pState + 1;
xorjoep 1:24714b45cd1b 244
xorjoep 1:24714b45cd1b 245 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 246 blkCnt--;
xorjoep 1:24714b45cd1b 247 }
xorjoep 1:24714b45cd1b 248
xorjoep 1:24714b45cd1b 249 /* Save energy and x0 values for the next frame */
xorjoep 1:24714b45cd1b 250 S->energy = (q31_t) energy;
xorjoep 1:24714b45cd1b 251 S->x0 = x0;
xorjoep 1:24714b45cd1b 252
xorjoep 1:24714b45cd1b 253 /* Processing is complete. Now copy the last numTaps - 1 samples to the
xorjoep 1:24714b45cd1b 254 satrt of the state buffer. This prepares the state buffer for the
xorjoep 1:24714b45cd1b 255 next function call. */
xorjoep 1:24714b45cd1b 256
xorjoep 1:24714b45cd1b 257 /* Points to the start of the pState buffer */
xorjoep 1:24714b45cd1b 258 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 259
xorjoep 1:24714b45cd1b 260 /* Loop unrolling for (numTaps - 1U) samples copy */
xorjoep 1:24714b45cd1b 261 tapCnt = (numTaps - 1U) >> 2U;
xorjoep 1:24714b45cd1b 262
xorjoep 1:24714b45cd1b 263 /* copy data */
xorjoep 1:24714b45cd1b 264 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 265 {
xorjoep 1:24714b45cd1b 266 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 267 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 268 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 269 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 270
xorjoep 1:24714b45cd1b 271 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 272 tapCnt--;
xorjoep 1:24714b45cd1b 273 }
xorjoep 1:24714b45cd1b 274
xorjoep 1:24714b45cd1b 275 /* Calculate remaining number of copies */
xorjoep 1:24714b45cd1b 276 tapCnt = (numTaps - 1U) % 0x4U;
xorjoep 1:24714b45cd1b 277
xorjoep 1:24714b45cd1b 278 /* Copy the remaining q31_t data */
xorjoep 1:24714b45cd1b 279 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 280 {
xorjoep 1:24714b45cd1b 281 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 282
xorjoep 1:24714b45cd1b 283 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 284 tapCnt--;
xorjoep 1:24714b45cd1b 285 }
xorjoep 1:24714b45cd1b 286
xorjoep 1:24714b45cd1b 287 #else
xorjoep 1:24714b45cd1b 288
xorjoep 1:24714b45cd1b 289 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 290
xorjoep 1:24714b45cd1b 291 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 292 {
xorjoep 1:24714b45cd1b 293
xorjoep 1:24714b45cd1b 294 /* Copy the new input sample into the state buffer */
xorjoep 1:24714b45cd1b 295 *pStateCurnt++ = *pSrc;
xorjoep 1:24714b45cd1b 296
xorjoep 1:24714b45cd1b 297 /* Initialize pState pointer */
xorjoep 1:24714b45cd1b 298 px = pState;
xorjoep 1:24714b45cd1b 299
xorjoep 1:24714b45cd1b 300 /* Initialize pCoeffs pointer */
xorjoep 1:24714b45cd1b 301 pb = pCoeffs;
xorjoep 1:24714b45cd1b 302
xorjoep 1:24714b45cd1b 303 /* Read the sample from input buffer */
xorjoep 1:24714b45cd1b 304 in = *pSrc++;
xorjoep 1:24714b45cd1b 305
xorjoep 1:24714b45cd1b 306 /* Update the energy calculation */
xorjoep 1:24714b45cd1b 307 energy =
xorjoep 1:24714b45cd1b 308 (q31_t) ((((q63_t) energy << 32) - (((q63_t) x0 * x0) << 1)) >> 32);
xorjoep 1:24714b45cd1b 309 energy = (q31_t) (((((q63_t) in * in) << 1) + (energy << 32)) >> 32);
xorjoep 1:24714b45cd1b 310
xorjoep 1:24714b45cd1b 311 /* Set the accumulator to zero */
xorjoep 1:24714b45cd1b 312 acc = 0;
xorjoep 1:24714b45cd1b 313
xorjoep 1:24714b45cd1b 314 /* Loop over numTaps number of values */
xorjoep 1:24714b45cd1b 315 tapCnt = numTaps;
xorjoep 1:24714b45cd1b 316
xorjoep 1:24714b45cd1b 317 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 318 {
xorjoep 1:24714b45cd1b 319 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 320 acc += ((q63_t) (*px++)) * (*pb++);
xorjoep 1:24714b45cd1b 321
xorjoep 1:24714b45cd1b 322 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 323 tapCnt--;
xorjoep 1:24714b45cd1b 324 }
xorjoep 1:24714b45cd1b 325
xorjoep 1:24714b45cd1b 326 /* Converting the result to 1.31 format */
xorjoep 1:24714b45cd1b 327 /* Converting the result to 1.31 format */
xorjoep 1:24714b45cd1b 328 /* Calc lower part of acc */
xorjoep 1:24714b45cd1b 329 acc_l = acc & 0xffffffff;
xorjoep 1:24714b45cd1b 330
xorjoep 1:24714b45cd1b 331 /* Calc upper part of acc */
xorjoep 1:24714b45cd1b 332 acc_h = (acc >> 32) & 0xffffffff;
xorjoep 1:24714b45cd1b 333
xorjoep 1:24714b45cd1b 334 acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
xorjoep 1:24714b45cd1b 335
xorjoep 1:24714b45cd1b 336
xorjoep 1:24714b45cd1b 337 //acc = (q31_t) (acc >> shift);
xorjoep 1:24714b45cd1b 338
xorjoep 1:24714b45cd1b 339 /* Store the result from accumulator into the destination buffer. */
xorjoep 1:24714b45cd1b 340 *pOut++ = (q31_t) acc;
xorjoep 1:24714b45cd1b 341
xorjoep 1:24714b45cd1b 342 /* Compute and store error */
xorjoep 1:24714b45cd1b 343 d = *pRef++;
xorjoep 1:24714b45cd1b 344 e = d - (q31_t) acc;
xorjoep 1:24714b45cd1b 345 *pErr++ = e;
xorjoep 1:24714b45cd1b 346
xorjoep 1:24714b45cd1b 347 /* Calculates the reciprocal of energy */
xorjoep 1:24714b45cd1b 348 postShift =
xorjoep 1:24714b45cd1b 349 arm_recip_q31(energy + DELTA_Q31, &oneByEnergy, &S->recipTable[0]);
xorjoep 1:24714b45cd1b 350
xorjoep 1:24714b45cd1b 351 /* Calculation of product of (e * mu) */
xorjoep 1:24714b45cd1b 352 errorXmu = (q31_t) (((q63_t) e * mu) >> 31);
xorjoep 1:24714b45cd1b 353
xorjoep 1:24714b45cd1b 354 /* Weighting factor for the normalized version */
xorjoep 1:24714b45cd1b 355 w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift));
xorjoep 1:24714b45cd1b 356
xorjoep 1:24714b45cd1b 357 /* Initialize pState pointer */
xorjoep 1:24714b45cd1b 358 px = pState;
xorjoep 1:24714b45cd1b 359
xorjoep 1:24714b45cd1b 360 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 361 pb = (pCoeffs);
xorjoep 1:24714b45cd1b 362
xorjoep 1:24714b45cd1b 363 /* Loop over numTaps number of values */
xorjoep 1:24714b45cd1b 364 tapCnt = numTaps;
xorjoep 1:24714b45cd1b 365
xorjoep 1:24714b45cd1b 366 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 367 {
xorjoep 1:24714b45cd1b 368 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 369 /* coef is in 2.30 format */
xorjoep 1:24714b45cd1b 370 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
xorjoep 1:24714b45cd1b 371 /* get coef in 1.31 format by left shifting */
xorjoep 1:24714b45cd1b 372 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
xorjoep 1:24714b45cd1b 373 /* update coefficient buffer to next coefficient */
xorjoep 1:24714b45cd1b 374 pb++;
xorjoep 1:24714b45cd1b 375
xorjoep 1:24714b45cd1b 376 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 377 tapCnt--;
xorjoep 1:24714b45cd1b 378 }
xorjoep 1:24714b45cd1b 379
xorjoep 1:24714b45cd1b 380 /* Read the sample from state buffer */
xorjoep 1:24714b45cd1b 381 x0 = *pState;
xorjoep 1:24714b45cd1b 382
xorjoep 1:24714b45cd1b 383 /* Advance state pointer by 1 for the next sample */
xorjoep 1:24714b45cd1b 384 pState = pState + 1;
xorjoep 1:24714b45cd1b 385
xorjoep 1:24714b45cd1b 386 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 387 blkCnt--;
xorjoep 1:24714b45cd1b 388 }
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 /* Save energy and x0 values for the next frame */
xorjoep 1:24714b45cd1b 391 S->energy = (q31_t) energy;
xorjoep 1:24714b45cd1b 392 S->x0 = x0;
xorjoep 1:24714b45cd1b 393
xorjoep 1:24714b45cd1b 394 /* Processing is complete. Now copy the last numTaps - 1 samples to the
xorjoep 1:24714b45cd1b 395 start of the state buffer. This prepares the state buffer for the
xorjoep 1:24714b45cd1b 396 next function call. */
xorjoep 1:24714b45cd1b 397
xorjoep 1:24714b45cd1b 398 /* Points to the start of the pState buffer */
xorjoep 1:24714b45cd1b 399 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 400
xorjoep 1:24714b45cd1b 401 /* Loop for (numTaps - 1U) samples copy */
xorjoep 1:24714b45cd1b 402 tapCnt = (numTaps - 1U);
xorjoep 1:24714b45cd1b 403
xorjoep 1:24714b45cd1b 404 /* Copy the remaining q31_t data */
xorjoep 1:24714b45cd1b 405 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 406 {
xorjoep 1:24714b45cd1b 407 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 408
xorjoep 1:24714b45cd1b 409 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 410 tapCnt--;
xorjoep 1:24714b45cd1b 411 }
xorjoep 1:24714b45cd1b 412
xorjoep 1:24714b45cd1b 413 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 414
xorjoep 1:24714b45cd1b 415 }
xorjoep 1:24714b45cd1b 416
xorjoep 1:24714b45cd1b 417 /**
xorjoep 1:24714b45cd1b 418 * @} end of LMS_NORM group
xorjoep 1:24714b45cd1b 419 */