playground-stm32/nucleo_f446re_playground
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Initial commit

Browse source
This commit is contained in:
Attila Body 2025-06-09 18:06:36 +02:00
commit ce3dd83b9f
Signed by: abody
GPG key ID: BD0C6214E68FB5CF
1470 changed files with 1054449 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

34
Drivers/CMSIS/DSP/Source/InterpolationFunctions/CMakeLists.txt Normal file
View file

@ -0,0 +1,34 @@
cmake_minimum_required (VERSION 3.14)
project(CMSISDSPInterpolation)
include(configLib)
include(configDsp)
add_library(CMSISDSPInterpolation STATIC)
target_sources(CMSISDSPInterpolation PRIVATE arm_bilinear_interp_f32.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_bilinear_interp_q15.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_bilinear_interp_q31.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_bilinear_interp_q7.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_linear_interp_f32.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_linear_interp_q15.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_linear_interp_q31.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_linear_interp_q7.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_spline_interp_f32.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_spline_interp_init_f32.c)
configLib(CMSISDSPInterpolation ${ROOT})
configDsp(CMSISDSPInterpolation ${ROOT})
### Includes
target_include_directories(CMSISDSPInterpolation PUBLIC "${DSP}/Include")
if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16))
target_sources(CMSISDSPInterpolation PRIVATE arm_bilinear_interp_f16.c)
target_sources(CMSISDSPInterpolation PRIVATE arm_linear_interp_f16.c)
endif()

41
Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctions.c Normal file
View file

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: InterpolationFunctions.c
* Description: Combination of all interpolation function source files.
*
* $Date: 22. July 2020
* $Revision: V1.0.0
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2020 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "arm_bilinear_interp_f32.c"
#include "arm_bilinear_interp_q15.c"
#include "arm_bilinear_interp_q31.c"
#include "arm_bilinear_interp_q7.c"
#include "arm_linear_interp_f32.c"
#include "arm_linear_interp_q15.c"
#include "arm_linear_interp_q31.c"
#include "arm_linear_interp_q7.c"
#include "arm_spline_interp_f32.c"
#include "arm_spline_interp_init_f32.c"

33
Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctionsF16.c Normal file
View file

@ -0,0 +1,33 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: InterpolationFunctions.c
* Description: Combination of all interpolation function source files.
*
* $Date: 22. July 2020
* $Revision: V1.0.0
*
* Target Processor: Cortex-M cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2020 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "arm_bilinear_interp_f16.c"
#include "arm_linear_interp_f16.c"

168
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f16.c Normal file
View file

@ -0,0 +1,168 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_bilinear_interp_f16.c
* Description: Floating-point bilinear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions_f16.h"
#if defined(ARM_FLOAT16_SUPPORTED)
/**
@ingroup groupInterpolation
*/
/**
* @defgroup BilinearInterpolate Bilinear Interpolation
*
* Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
* The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
* determines values between the grid points.
* Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
* Bilinear interpolation is often used in image processing to rescale images.
* The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
*
* <b>Algorithm</b>
* \par
* The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
* For floating-point, the instance structure is defined as:
* <pre>
* typedef struct
* {
* uint16_t numRows;
* uint16_t numCols;
* float16_t *pData;
* } arm_bilinear_interp_instance_f16;
* </pre>
*
* \par
* where <code>numRows</code> specifies the number of rows in the table;
* <code>numCols</code> specifies the number of columns in the table;
* and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
* The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
* That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
*
* \par
* Let <code>(x, y)</code> specify the desired interpolation point. Then define:
* <pre>
* XF = floor(x)
* YF = floor(y)
* </pre>
* \par
* The interpolated output point is computed as:
* <pre>
* f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
* + f(XF+1, YF) * (x-XF)*(1-(y-YF))
* + f(XF, YF+1) * (1-(x-XF))*(y-YF)
* + f(XF+1, YF+1) * (x-XF)*(y-YF)
* </pre>
* Note that the coordinates (x, y) contain integer and fractional components.
* The integer components specify which portion of the table to use while the
* fractional components control the interpolation processor.
*
* \par
* if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
*/
/**
* @addtogroup BilinearInterpolate
* @{
*/
/**
* @brief Floating-point bilinear interpolation.
* @param[in,out] S points to an instance of the interpolation structure.
* @param[in] X interpolation coordinate.
* @param[in] Y interpolation coordinate.
* @return out interpolated value.
*/
float16_t arm_bilinear_interp_f16(
const arm_bilinear_interp_instance_f16 * S,
float16_t X,
float16_t Y)
{
float16_t out;
float16_t f00, f01, f10, f11;
float16_t *pData = S->pData;
int32_t xIndex, yIndex, index;
float16_t xdiff, ydiff;
float16_t b1, b2, b3, b4;
xIndex = (int32_t) X;
yIndex = (int32_t) Y;
/* Care taken for table outside boundary */
/* Returns zero output when values are outside table boundary */
if (xIndex < 0 || xIndex > (S->numCols - 2) || yIndex < 0 || yIndex > (S->numRows - 2))
{
return (0);
}
/* Calculation of index for two nearest points in X-direction */
index = (xIndex ) + (yIndex ) * S->numCols;
/* Read two nearest points in X-direction */
f00 = pData[index];
f01 = pData[index + 1];
/* Calculation of index for two nearest points in Y-direction */
index = (xIndex ) + (yIndex+1) * S->numCols;
/* Read two nearest points in Y-direction */
f10 = pData[index];
f11 = pData[index + 1];
/* Calculation of intermediate values */
b1 = f00;
b2 = (_Float16)f01 - (_Float16)f00;
b3 = (_Float16)f10 - (_Float16)f00;
b4 = (_Float16)f00 - (_Float16)f01 - (_Float16)f10 + (_Float16)f11;
/* Calculation of fractional part in X */
xdiff = (_Float16)X - (_Float16)xIndex;
/* Calculation of fractional part in Y */
ydiff = (_Float16)Y - (_Float16)yIndex;
/* Calculation of bi-linear interpolated output */
out = (_Float16)b1 + (_Float16)b2 * (_Float16)xdiff +
(_Float16)b3 * (_Float16)ydiff + (_Float16)b4 * (_Float16)xdiff * (_Float16)ydiff;
/* return to application */
return (out);
}
/**
* @} end of BilinearInterpolate group
*/
#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */

161
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f32.c Normal file
View file

@ -0,0 +1,161 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_bilinear_interp_f32.c
* Description: Floating-point bilinear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @defgroup BilinearInterpolate Bilinear Interpolation
*
* Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
* The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
* determines values between the grid points.
* Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
* Bilinear interpolation is often used in image processing to rescale images.
* The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
*
* <b>Algorithm</b>
* \par
* The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
* For floating-point, the instance structure is defined as:
* <pre>
* typedef struct
* {
* uint16_t numRows;
* uint16_t numCols;
* float32_t *pData;
* } arm_bilinear_interp_instance_f32;
* </pre>
*
* \par
* where <code>numRows</code> specifies the number of rows in the table;
* <code>numCols</code> specifies the number of columns in the table;
* and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
* The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
* That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
*
* \par
* Let <code>(x, y)</code> specify the desired interpolation point. Then define:
* <pre>
* XF = floor(x)
* YF = floor(y)
* </pre>
* \par
* The interpolated output point is computed as:
* <pre>
* f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
* + f(XF+1, YF) * (x-XF)*(1-(y-YF))
* + f(XF, YF+1) * (1-(x-XF))*(y-YF)
* + f(XF+1, YF+1) * (x-XF)*(y-YF)
* </pre>
* Note that the coordinates (x, y) contain integer and fractional components.
* The integer components specify which portion of the table to use while the
* fractional components control the interpolation processor.
*
* \par
* if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
*/
/**
* @addtogroup BilinearInterpolate
* @{
*/
/**
* @brief Floating-point bilinear interpolation.
* @param[in,out] S points to an instance of the interpolation structure.
* @param[in] X interpolation coordinate.
* @param[in] Y interpolation coordinate.
* @return out interpolated value.
*/
float32_t arm_bilinear_interp_f32(
const arm_bilinear_interp_instance_f32 * S,
float32_t X,
float32_t Y)
{
float32_t out;
float32_t f00, f01, f10, f11;
float32_t *pData = S->pData;
int32_t xIndex, yIndex, index;
float32_t xdiff, ydiff;
float32_t b1, b2, b3, b4;
xIndex = (int32_t) X;
yIndex = (int32_t) Y;
/* Care taken for table outside boundary */
/* Returns zero output when values are outside table boundary */
if (xIndex < 0 || xIndex > (S->numCols - 2) || yIndex < 0 || yIndex > (S->numRows - 2))
{
return (0);
}
/* Calculation of index for two nearest points in X-direction */
index = (xIndex ) + (yIndex ) * S->numCols;
/* Read two nearest points in X-direction */
f00 = pData[index];
f01 = pData[index + 1];
/* Calculation of index for two nearest points in Y-direction */
index = (xIndex ) + (yIndex+1) * S->numCols;
/* Read two nearest points in Y-direction */
f10 = pData[index];
f11 = pData[index + 1];
/* Calculation of intermediate values */
b1 = f00;
b2 = f01 - f00;
b3 = f10 - f00;
b4 = f00 - f01 - f10 + f11;
/* Calculation of fractional part in X */
xdiff = X - xIndex;
/* Calculation of fractional part in Y */
ydiff = Y - yIndex;
/* Calculation of bi-linear interpolated output */
out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;
/* return to application */
return (out);
}
/**
* @} end of BilinearInterpolate group
*/

121
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q15.c Normal file
View file

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q15.c
* Description: Q15 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup BilinearInterpolate
* @{
*/
/**
* @brief Q15 bilinear interpolation.
* @param[in,out] S points to an instance of the interpolation structure.
* @param[in] X interpolation coordinate in 12.20 format.
* @param[in] Y interpolation coordinate in 12.20 format.
* @return out interpolated value.
*/
q15_t arm_bilinear_interp_q15(
arm_bilinear_interp_instance_q15 * S,
q31_t X,
q31_t Y)
{
q63_t acc = 0; /* output */
q31_t out; /* Temporary output */
q15_t x1, x2, y1, y2; /* Nearest output values */
q31_t xfract, yfract; /* X, Y fractional parts */
int32_t rI, cI; /* Row and column indices */
q15_t *pYData = S->pData; /* pointer to output table values */
uint32_t nCols = S->numCols; /* num of rows */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
rI = ((X & (q31_t)0xFFF00000) >> 20);
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
cI = ((Y & (q31_t)0xFFF00000) >> 20);
/* Care taken for table outside boundary */
/* Returns zero output when values are outside table boundary */
if (rI < 0 || rI > (S->numCols - 2) || cI < 0 || cI > (S->numRows - 2))
{
return (0);
}
/* 20 bits for the fractional part */
/* xfract should be in 12.20 format */
xfract = (X & 0x000FFFFF);
/* Read two nearest output values from the index */
x1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) ];
x2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) + 1];
/* 20 bits for the fractional part */
/* yfract should be in 12.20 format */
yfract = (Y & 0x000FFFFF);
/* Read two nearest output values from the index */
y1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) ];
y2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) + 1];
/* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */
/* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */
/* convert 13.35 to 13.31 by right shifting and out is in 1.31 */
out = (q31_t) (((q63_t) x1 * (0x0FFFFF - xfract)) >> 4U);
acc = ((q63_t) out * (0x0FFFFF - yfract));
/* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */
out = (q31_t) (((q63_t) x2 * (0x0FFFFF - yfract)) >> 4U);
acc += ((q63_t) out * (xfract));
/* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */
out = (q31_t) (((q63_t) y1 * (0x0FFFFF - xfract)) >> 4U);
acc += ((q63_t) out * (yfract));
/* y2 * (xfract) * (yfract) in 1.51 and adding to acc */
out = (q31_t) (((q63_t) y2 * (xfract)) >> 4U);
acc += ((q63_t) out * (yfract));
/* acc is in 13.51 format and down shift acc by 36 times */
/* Convert out to 1.15 format */
return ((q15_t)(acc >> 36));
}
/**
* @} end of BilinearInterpolate group
*/

119
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q31.c Normal file
View file

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q31.c
* Description: Q31 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup BilinearInterpolate
* @{
*/
/**
* @brief Q31 bilinear interpolation.
* @param[in,out] S points to an instance of the interpolation structure.
* @param[in] X interpolation coordinate in 12.20 format.
* @param[in] Y interpolation coordinate in 12.20 format.
* @return out interpolated value.
*/
q31_t arm_bilinear_interp_q31(
arm_bilinear_interp_instance_q31 * S,
q31_t X,
q31_t Y)
{
q31_t out; /* Temporary output */
q31_t acc = 0; /* output */
q31_t xfract, yfract; /* X, Y fractional parts */
q31_t x1, x2, y1, y2; /* Nearest output values */
int32_t rI, cI; /* Row and column indices */
q31_t *pYData = S->pData; /* pointer to output table values */
uint32_t nCols = S->numCols; /* num of rows */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
rI = ((X & (q31_t)0xFFF00000) >> 20);
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
cI = ((Y & (q31_t)0xFFF00000) >> 20);
/* Care taken for table outside boundary */
/* Returns zero output when values are outside table boundary */
if (rI < 0 || rI > (S->numCols - 2) || cI < 0 || cI > (S->numRows - 2))
{
return (0);
}
/* 20 bits for the fractional part */
/* shift left xfract by 11 to keep 1.31 format */
xfract = (X & 0x000FFFFF) << 11U;
/* Read two nearest output values from the index */
x1 = pYData[(rI) + (int32_t)nCols * (cI) ];
x2 = pYData[(rI) + (int32_t)nCols * (cI) + 1];
/* 20 bits for the fractional part */
/* shift left yfract by 11 to keep 1.31 format */
yfract = (Y & 0x000FFFFF) << 11U;
/* Read two nearest output values from the index */
y1 = pYData[(rI) + (int32_t)nCols * (cI + 1) ];
y2 = pYData[(rI) + (int32_t)nCols * (cI + 1) + 1];
/* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */
out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));
acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));
/* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */
out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));
acc += ((q31_t) ((q63_t) out * (xfract) >> 32));
/* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */
out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));
acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
/* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */
out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));
acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
/* Convert acc to 1.31(q31) format */
return ((q31_t)(acc << 2));
}
/**
* @} end of BilinearInterpolate group
*/

117
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q7.c Normal file
View file

@ -0,0 +1,117 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q7.c
* Description: Q7 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup BilinearInterpolate
* @{
*/
/**
* @brief Q7 bilinear interpolation.
* @param[in,out] S points to an instance of the interpolation structure.
* @param[in] X interpolation coordinate in 12.20 format.
* @param[in] Y interpolation coordinate in 12.20 format.
* @return out interpolated value.
*/
q7_t arm_bilinear_interp_q7(
arm_bilinear_interp_instance_q7 * S,
q31_t X,
q31_t Y)
{
q63_t acc = 0; /* output */
q31_t out; /* Temporary output */
q31_t xfract, yfract; /* X, Y fractional parts */
q7_t x1, x2, y1, y2; /* Nearest output values */
int32_t rI, cI; /* Row and column indices */
q7_t *pYData = S->pData; /* pointer to output table values */
uint32_t nCols = S->numCols; /* num of rows */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
rI = ((X & (q31_t)0xFFF00000) >> 20);
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
cI = ((Y & (q31_t)0xFFF00000) >> 20);
/* Care taken for table outside boundary */
/* Returns zero output when values are outside table boundary */
if (rI < 0 || rI > (S->numCols - 2) || cI < 0 || cI > (S->numRows - 2))
{
return (0);
}
/* 20 bits for the fractional part */
/* xfract should be in 12.20 format */
xfract = (X & (q31_t)0x000FFFFF);
/* Read two nearest output values from the index */
x1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) ];
x2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) + 1];
/* 20 bits for the fractional part */
/* yfract should be in 12.20 format */
yfract = (Y & (q31_t)0x000FFFFF);
/* Read two nearest output values from the index */
y1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) ];
y2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) + 1];
/* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */
out = ((x1 * (0xFFFFF - xfract)));
acc = (((q63_t) out * (0xFFFFF - yfract)));
/* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */
out = ((x2 * (0xFFFFF - yfract)));
acc += (((q63_t) out * (xfract)));
/* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */
out = ((y1 * (0xFFFFF - xfract)));
acc += (((q63_t) out * (yfract)));
/* y2 * (xfract) * (yfract) in 2.22 and adding to acc */
out = ((y2 * (yfract)));
acc += (((q63_t) out * (xfract)));
/* acc in 16.47 format and down shift by 40 to convert to 1.7 format */
return ((q7_t)(acc >> 40));
}
/**
* @} end of BilinearInterpolate group
*/

132
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f16.c Normal file
View file

@ -0,0 +1,132 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_f16.c
* Description: Floating-point linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions_f16.h"
#if defined(ARM_FLOAT16_SUPPORTED)
/**
@ingroup groupInterpolation
*/
/**
* @defgroup LinearInterpolate Linear Interpolation
*
* Linear interpolation is a method of curve fitting using linear polynomials.
* Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
*
* \par
* \image html LinearInterp.gif "Linear interpolation"
*
* \par
* A Linear Interpolate function calculates an output value(y), for the input(x)
* using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
*
* \par Algorithm:
* <pre>
* y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
* where x0, x1 are nearest values of input x
* y0, y1 are nearest values to output y
* </pre>
*
* \par
* This set of functions implements Linear interpolation process
* for Q7, Q15, Q31, and floating-point data types. The functions operate on a single
* sample of data and each call to the function returns a single processed value.
* <code>S</code> points to an instance of the Linear Interpolate function data structure.
* <code>x</code> is the input sample value. The functions returns the output value.
*
* \par
* if x is outside of the table boundary, Linear interpolation returns first value of the table
* if x is below input range and returns last value of table if x is above range.
*/
/**
* @addtogroup LinearInterpolate
* @{
*/
/**
* @brief Process function for the floating-point Linear Interpolation Function.
* @param[in,out] S is an instance of the floating-point Linear Interpolation structure
* @param[in] x input sample to process
* @return y processed output sample.
*
*/
float16_t arm_linear_interp_f16(
arm_linear_interp_instance_f16 * S,
float16_t x)
{
float16_t y;
float16_t x0, x1; /* Nearest input values */
float16_t y0, y1; /* Nearest output values */
float16_t xSpacing = S->xSpacing; /* spacing between input values */
int32_t i; /* Index variable */
float16_t *pYData = S->pYData; /* pointer to output table */
/* Calculation of index */
i = (int32_t) (((_Float16)x - (_Float16)S->x1) / (_Float16)xSpacing);
if (i < 0)
{
/* Iniatilize output for below specified range as least output value of table */
y = pYData[0];
}
else if ((uint32_t)i >= (S->nValues - 1))
{
/* Iniatilize output for above specified range as last output value of table */
y = pYData[S->nValues - 1];
}
else
{
/* Calculation of nearest input values */
x0 = (_Float16)S->x1 + (_Float16)i * (_Float16)xSpacing;
x1 = (_Float16)S->x1 + (_Float16)(i + 1) * (_Float16)xSpacing;
/* Read of nearest output values */
y0 = pYData[i];
y1 = pYData[i + 1];
/* Calculation of output */
y = (_Float16)y0 + ((_Float16)x - (_Float16)x0) *
(((_Float16)y1 - (_Float16)y0) / ((_Float16)x1 - (_Float16)x0));
}
/* returns output value */
return (y);
}
/**
* @} end of LinearInterpolate group
*/
#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */

125
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f32.c Normal file
View file

@ -0,0 +1,125 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_f32.c
* Description: Floating-point linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @defgroup LinearInterpolate Linear Interpolation
*
* Linear interpolation is a method of curve fitting using linear polynomials.
* Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
*
* \par
* \image html LinearInterp.gif "Linear interpolation"
*
* \par
* A Linear Interpolate function calculates an output value(y), for the input(x)
* using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
*
* \par Algorithm:
* <pre>
* y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
* where x0, x1 are nearest values of input x
* y0, y1 are nearest values to output y
* </pre>
*
* \par
* This set of functions implements Linear interpolation process
* for Q7, Q15, Q31, and floating-point data types. The functions operate on a single
* sample of data and each call to the function returns a single processed value.
* <code>S</code> points to an instance of the Linear Interpolate function data structure.
* <code>x</code> is the input sample value. The functions returns the output value.
*
* \par
* if x is outside of the table boundary, Linear interpolation returns first value of the table
* if x is below input range and returns last value of table if x is above range.
*/
/**
* @addtogroup LinearInterpolate
* @{
*/
/**
* @brief Process function for the floating-point Linear Interpolation Function.
* @param[in,out] S is an instance of the floating-point Linear Interpolation structure
* @param[in] x input sample to process
* @return y processed output sample.
*
*/
float32_t arm_linear_interp_f32(
arm_linear_interp_instance_f32 * S,
float32_t x)
{
float32_t y;
float32_t x0, x1; /* Nearest input values */
float32_t y0, y1; /* Nearest output values */
float32_t xSpacing = S->xSpacing; /* spacing between input values */
int32_t i; /* Index variable */
float32_t *pYData = S->pYData; /* pointer to output table */
/* Calculation of index */
i = (int32_t) ((x - S->x1) / xSpacing);
if (i < 0)
{
/* Iniatilize output for below specified range as least output value of table */
y = pYData[0];
}
else if ((uint32_t)i >= (S->nValues - 1))
{
/* Iniatilize output for above specified range as last output value of table */
y = pYData[S->nValues - 1];
}
else
{
/* Calculation of nearest input values */
x0 = S->x1 + i * xSpacing;
x1 = S->x1 + (i + 1) * xSpacing;
/* Read of nearest output values */
y0 = pYData[i];
y1 = pYData[i + 1];
/* Calculation of output */
y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));
}
/* returns output value */
return (y);
}
/**
* @} end of LinearInterpolate group
*/

101
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q15.c Normal file
View file

@ -0,0 +1,101 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q15.c
* Description: Q15 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup LinearInterpolate
* @{
*/
/**
*
* @brief Process function for the Q15 Linear Interpolation Function.
* @param[in] pYData pointer to Q15 Linear Interpolation table
* @param[in] x input sample to process
* @param[in] nValues number of table values
* @return y processed output sample.
*
* \par
* Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
* This function can support maximum of table size 2^12.
*
*/
q15_t arm_linear_interp_q15(
const q15_t * pYData,
q31_t x,
uint32_t nValues)
{
q63_t y; /* output */
q15_t y0, y1; /* Nearest output values */
q31_t fract; /* fractional part */
int32_t index; /* Index to read nearest output values */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
index = ((x & (int32_t)0xFFF00000) >> 20);
if (index >= (int32_t)(nValues - 1))
{
return (pYData[nValues - 1]);
}
else if (index < 0)
{
return (pYData[0]);
}
else
{
/* 20 bits for the fractional part */
/* fract is in 12.20 format */
fract = (x & 0x000FFFFF);
/* Read two nearest output values from the index */
y0 = pYData[index];
y1 = pYData[index + 1];
/* Calculation of y0 * (1-fract) and y is in 13.35 format */
y = ((q63_t) y0 * (0xFFFFF - fract));
/* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
y += ((q63_t) y1 * (fract));
/* convert y to 1.15 format */
return (q15_t) (y >> 20);
}
}
/**
* @} end of LinearInterpolate group
*/

103
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q31.c Normal file
View file

@ -0,0 +1,103 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q31.c
* Description: Q31 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup LinearInterpolate
* @{
*/
/**
*
* @brief Process function for the Q31 Linear Interpolation Function.
* @param[in] pYData pointer to Q31 Linear Interpolation table
* @param[in] x input sample to process
* @param[in] nValues number of table values
* @return y processed output sample.
*
* \par
* Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
* This function can support maximum of table size 2^12.
*
*/
q31_t arm_linear_interp_q31(
const q31_t * pYData,
q31_t x,
uint32_t nValues)
{
q31_t y; /* output */
q31_t y0, y1; /* Nearest output values */
q31_t fract; /* fractional part */
int32_t index; /* Index to read nearest output values */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
index = ((x & (q31_t)0xFFF00000) >> 20);
if (index >= (int32_t)(nValues - 1))
{
return (pYData[nValues - 1]);
}
else if (index < 0)
{
return (pYData[0]);
}
else
{
/* 20 bits for the fractional part */
/* shift left by 11 to keep fract in 1.31 format */
fract = (x & 0x000FFFFF) << 11;
/* Read two nearest output values from the index in 1.31(q31) format */
y0 = pYData[index];
y1 = pYData[index + 1];
/* Calculation of y0 * (1-fract) and y is in 2.30 format */
y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
/* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
y += ((q31_t) (((q63_t) y1 * fract) >> 32));
/* Convert y to 1.31 format */
return (y << 1U);
}
}
/**
* @} end of LinearInterpolate group
*/

99
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q7.c Normal file
View file

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_linear_interp_q7.c
* Description: Q7 linear interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
* @addtogroup LinearInterpolate
* @{
*/
/**
*
* @brief Process function for the Q7 Linear Interpolation Function.
* @param[in] pYData pointer to Q7 Linear Interpolation table
* @param[in] x input sample to process
* @param[in] nValues number of table values
* @return y processed output sample.
*
* \par
* Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
* This function can support maximum of table size 2^12.
*/
q7_t arm_linear_interp_q7(
const q7_t * pYData,
q31_t x,
uint32_t nValues)
{
q31_t y; /* output */
q7_t y0, y1; /* Nearest output values */
q31_t fract; /* fractional part */
uint32_t index; /* Index to read nearest output values */
/* Input is in 12.20 format */
/* 12 bits for the table index */
/* Index value calculation */
if (x < 0)
{
return (pYData[0]);
}
index = (x >> 20) & 0xfff;
if (index >= (nValues - 1))
{
return (pYData[nValues - 1]);
}
else
{
/* 20 bits for the fractional part */
/* fract is in 12.20 format */
fract = (x & 0x000FFFFF);
/* Read two nearest output values from the index and are in 1.7(q7) format */
y0 = pYData[index];
y1 = pYData[index + 1];
/* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
y = ((y0 * (0xFFFFF - fract)));
/* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
y += (y1 * fract);
/* convert y to 1.7(q7) format */
return (q7_t) (y >> 20);
}
}
/**
* @} end of LinearInterpolate group
*/

283
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_f32.c Normal file
View file

@ -0,0 +1,283 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_spline_interp_f32.c
* Description: Floating-point cubic spline interpolation
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
@defgroup SplineInterpolate Cubic Spline Interpolation
Spline interpolation is a method of interpolation where the interpolant
is a piecewise-defined polynomial called "spline".
@par Introduction
Given a function f defined on the interval [a,b], a set of n nodes x(i)
where a=x(1)<x(2)<...<x(n)=b and a set of n values y(i) = f(x(i)),
a cubic spline interpolant S(x) is defined as:
<pre>
S1(x) x(1) < x < x(2)
S(x) = ...
Sn-1(x) x(n-1) < x < x(n)
</pre>
where
<pre>
Si(x) = a_i+b_i(x-xi)+c_i(x-xi)^2+d_i(x-xi)^3 i=1, ..., n-1
</pre>
@par Algorithm
Having defined h(i) = x(i+1) - x(i)
<pre>
h(i-1)c(i-1)+2[h(i-1)+h(i)]c(i)+h(i)c(i+1) = 3/h(i)*[a(i+1)-a(i)]-3/h(i-1)*[a(i)-a(i-1)] i=2, ..., n-1
</pre>
It is possible to write the previous conditions in matrix form (Ax=B).
In order to solve the system two boundary conidtions are needed.
- Natural spline: S1''(x1)=2*c(1)=0 ; Sn''(xn)=2*c(n)=0
In matrix form:
<pre>
| 1 0 0 ... 0 0 0 || c(1) | | 0 |
| h(0) 2[h(0)+h(1)] h(1) ... 0 0 0 || c(2) | | 3/h(2)*[a(3)-a(2)]-3/h(1)*[a(2)-a(1)] |
| ... ... ... ... ... ... ... || ... |=| ... |
| 0 0 0 ... h(n-2) 2[h(n-2)+h(n-1)] h(n-1) || c(n-1) | | 3/h(n-1)*[a(n)-a(n-1)]-3/h(n-2)*[a(n-1)-a(n-2)] |
| 0 0 0 ... 0 0 1 || c(n) | | 0 |
</pre>
- Parabolic runout spline: S1''(x1)=2*c(1)=S2''(x2)=2*c(2) ; Sn-1''(xn-1)=2*c(n-1)=Sn''(xn)=2*c(n)
In matrix form:
<pre>
| 1 -1 0 ... 0 0 0 || c(1) | | 0 |
| h(0) 2[h(0)+h(1)] h(1) ... 0 0 0 || c(2) | | 3/h(2)*[a(3)-a(2)]-3/h(1)*[a(2)-a(1)] |
| ... ... ... ... ... ... ... || ... |=| ... |
| 0 0 0 ... h(n-2) 2[h(n-2)+h(n-1)] h(n-1) || c(n-1) | | 3/h(n-1)*[a(n)-a(n-1)]-3/h(n-2)*[a(n-1)-a(n-2)] |
| 0 0 0 ... 0 -1 1 || c(n) | | 0 |
</pre>
A is a tridiagonal matrix (a band matrix of bandwidth 3) of size N=n+1. The factorization
algorithms (A=LU) can be simplified considerably because a large number of zeros appear
in regular patterns. The Crout method has been used:
1) Solve LZ=B
<pre>
u(1,2) = A(1,2)/A(1,1)
z(1) = B(1)/l(11)
FOR i=2, ..., N-1
l(i,i) = A(i,i)-A(i,i-1)u(i-1,i)
u(i,i+1) = a(i,i+1)/l(i,i)
z(i) = [B(i)-A(i,i-1)z(i-1)]/l(i,i)
l(N,N) = A(N,N)-A(N,N-1)u(N-1,N)
z(N) = [B(N)-A(N,N-1)z(N-1)]/l(N,N)
</pre>
2) Solve UX=Z
<pre>
c(N)=z(N)
FOR i=N-1, ..., 1
c(i)=z(i)-u(i,i+1)c(i+1)
</pre>
c(i) for i=1, ..., n-1 are needed to compute the n-1 polynomials.
b(i) and d(i) are computed as:
- b(i) = [y(i+1)-y(i)]/h(i)-h(i)*[c(i+1)+2*c(i)]/3
- d(i) = [c(i+1)-c(i)]/[3*h(i)]
Moreover, a(i)=y(i).
@par Behaviour outside the given intervals
It is possible to compute the interpolated vector for x values outside the
input range (xq<x(1); xq>x(n)). The coefficients used to compute the y values for
xq<x(1) are going to be the ones used for the first interval, while for xq>x(n) the
coefficients used for the last interval.
*/
/**
@addtogroup SplineInterpolate
@{
*/
/**
* @brief Processing function for the floating-point cubic spline interpolation.
* @param[in] S points to an instance of the floating-point spline structure.
* @param[in] xq points to the x values of the interpolated data points.
* @param[out] pDst points to the block of output data.
* @param[in] blockSize number of samples of output data.
*/
void arm_spline_f32(
arm_spline_instance_f32 * S,
const float32_t * xq,
float32_t * pDst,
uint32_t blockSize)
{
const float32_t * x = S->x;
const float32_t * y = S->y;
int32_t n = S->n_x;
/* Coefficients (a==y for i<=n-1) */
float32_t * b = (S->coeffs);
float32_t * c = (S->coeffs)+(n-1);
float32_t * d = (S->coeffs)+(2*(n-1));
const float32_t * pXq = xq;
int32_t blkCnt = (int32_t)blockSize;
int32_t blkCnt2;
int32_t i;
float32_t x_sc;
#ifdef ARM_MATH_NEON
float32x4_t xiv;
float32x4_t aiv;
float32x4_t biv;
float32x4_t civ;
float32x4_t div;
float32x4_t xqv;
float32x4_t temp;
float32x4_t diff;
float32x4_t yv;
#endif
/* Create output for x(i)<x<x(i+1) */
for (i=0; i<n-1; i++)
{
#ifdef ARM_MATH_NEON
xiv = vdupq_n_f32(x[i]);
aiv = vdupq_n_f32(y[i]);
biv = vdupq_n_f32(b[i]);
civ = vdupq_n_f32(c[i]);
div = vdupq_n_f32(d[i]);
while( *(pXq+4) <= x[i+1] && blkCnt > 4 )
{
/* Load [xq(k) xq(k+1) xq(k+2) xq(k+3)] */
xqv = vld1q_f32(pXq);
pXq+=4;
/* Compute [xq(k)-x(i) xq(k+1)-x(i) xq(k+2)-x(i) xq(k+3)-x(i)] */
diff = vsubq_f32(xqv, xiv);
temp = diff;
/* y(i) = a(i) + ... */
yv = aiv;
/* ... + b(i)*(x-x(i)) + ... */
yv = vmlaq_f32(yv, biv, temp);
/* ... + c(i)*(x-x(i))^2 + ... */
temp = vmulq_f32(temp, diff);
yv = vmlaq_f32(yv, civ, temp);
/* ... + d(i)*(x-x(i))^3 */
temp = vmulq_f32(temp, diff);
yv = vmlaq_f32(yv, div, temp);
/* Store [y(k) y(k+1) y(k+2) y(k+3)] */
vst1q_f32(pDst, yv);
pDst+=4;
blkCnt-=4;
}
#endif
while( *pXq <= x[i+1] && blkCnt > 0 )
{
x_sc = *pXq++;
*pDst = y[i]+b[i]*(x_sc-x[i])+c[i]*(x_sc-x[i])*(x_sc-x[i])+d[i]*(x_sc-x[i])*(x_sc-x[i])*(x_sc-x[i]);
pDst++;
blkCnt--;
}
}
/* Create output for remaining samples (x>=x(n)) */
#ifdef ARM_MATH_NEON
/* Compute 4 outputs at a time */
blkCnt2 = blkCnt >> 2;
while(blkCnt2 > 0)
{
/* Load [xq(k) xq(k+1) xq(k+2) xq(k+3)] */
xqv = vld1q_f32(pXq);
pXq+=4;
/* Compute [xq(k)-x(i) xq(k+1)-x(i) xq(k+2)-x(i) xq(k+3)-x(i)] */
diff = vsubq_f32(xqv, xiv);
temp = diff;
/* y(i) = a(i) + ... */
yv = aiv;
/* ... + b(i)*(x-x(i)) + ... */
yv = vmlaq_f32(yv, biv, temp);
/* ... + c(i)*(x-x(i))^2 + ... */
temp = vmulq_f32(temp, diff);
yv = vmlaq_f32(yv, civ, temp);
/* ... + d(i)*(x-x(i))^3 */
temp = vmulq_f32(temp, diff);
yv = vmlaq_f32(yv, div, temp);
/* Store [y(k) y(k+1) y(k+2) y(k+3)] */
vst1q_f32(pDst, yv);
pDst+=4;
blkCnt2--;
}
/* Tail */
blkCnt2 = blkCnt & 3;
#else
blkCnt2 = blkCnt;
#endif
while(blkCnt2 > 0)
{
x_sc = *pXq++;
*pDst = y[i-1]+b[i-1]*(x_sc-x[i-1])+c[i-1]*(x_sc-x[i-1])*(x_sc-x[i-1])+d[i-1]*(x_sc-x[i-1])*(x_sc-x[i-1])*(x_sc-x[i-1]);
pDst++;
blkCnt2--;
}
}
/**
@} end of SplineInterpolate group
*/

175
Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_init_f32.c Normal file
View file

@ -0,0 +1,175 @@
/* ----------------------------------------------------------------------
* Project: CMSIS DSP Library
* Title: arm_spline_interp_init_f32.c
* Description: Floating-point cubic spline initialization function
*
* $Date: 23 April 2021
* $Revision: V1.9.0
*
* Target Processor: Cortex-M and Cortex-A cores
* -------------------------------------------------------------------- */
/*
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the License); you may
* not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "dsp/interpolation_functions.h"
/**
@ingroup groupInterpolation
*/
/**
@addtogroup SplineInterpolate
@{
@par Initialization function
The initialization function takes as input two arrays that the user has to allocate:
<code>coeffs</code> will contain the b, c, and d coefficients for the (n-1) intervals
(n is the number of known points), hence its size must be 3*(n-1); <code>tempBuffer</code>
is temporally used for internal computations and its size is n+n-1.
@par
The x input array must be strictly sorted in ascending order and it must
not contain twice the same value (x(i)<x(i+1)).
*/
/**
* @brief Initialization function for the floating-point cubic spline interpolation.
* @param[in,out] S points to an instance of the floating-point spline structure.
* @param[in] type type of cubic spline interpolation (boundary conditions)
* @param[in] x points to the x values of the known data points.
* @param[in] y points to the y values of the known data points.
* @param[in] n number of known data points.
* @param[in] coeffs coefficients array for b, c, and d
* @param[in] tempBuffer buffer array for internal computations
*
*/
void arm_spline_init_f32(
arm_spline_instance_f32 * S,
arm_spline_type type,
const float32_t * x,
const float32_t * y,
uint32_t n,
float32_t * coeffs,
float32_t * tempBuffer)
{
/*** COEFFICIENTS COMPUTATION ***/
/* Type (boundary conditions):
- Natural spline ( S1''(x1) = 0 ; Sn''(xn) = 0 )
- Parabolic runout spline ( S1''(x1) = S2''(x2) ; Sn-1''(xn-1) = Sn''(xn) ) */
/* (n-1)-long buffers for b, c, and d coefficients */
float32_t * b = coeffs;
float32_t * c = coeffs+(n-1);
float32_t * d = coeffs+(2*(n-1));
float32_t * u = tempBuffer; /* (n-1)-long scratch buffer for u elements */
float32_t * z = tempBuffer+(n-1); /* n-long scratch buffer for z elements */
float32_t hi, hm1; /* h(i) and h(i-1) */
float32_t Bi; /* B(i), i-th element of matrix B=LZ */
float32_t li; /* l(i), i-th element of matrix L */
float32_t cp1; /* Temporary value for c(i+1) */
int32_t i; /* Loop counter */
S->x = x;
S->y = y;
S->n_x = n;
/* == Solve LZ=B to obtain z(i) and u(i) == */
/* -- Row 1 -- */
/* B(0) = 0, not computed */
/* u(1,2) = a(1,2)/a(1,1) = a(1,2) */
if(type == ARM_SPLINE_NATURAL)
u[0] = 0; /* a(1,2) = 0 */
else if(type == ARM_SPLINE_PARABOLIC_RUNOUT)
u[0] = -1; /* a(1,2) = -1 */
z[0] = 0; /* z(1) = B(1)/a(1,1) = 0 always */
/* -- Rows 2 to N-1 (N=n+1) -- */
hm1 = x[1] - x[0]; /* Initialize h(i-1) = h(1) = x(2)-x(1) */
for (i=1; i<(int32_t)n-1; i++)
{
/* Compute B(i) */
hi = x[i+1]-x[i];
Bi = 3*(y[i+1]-y[i])/hi - 3*(y[i]-y[i-1])/hm1;
/* l(i) = a(i)-a(i,i-1)*u(i-1) = 2[h(i-1)+h(i)]-h(i-1)*u(i-1) */
li = 2*(hi+hm1) - hm1*u[i-1];
/* u(i) = a(i,i+1)/l(i) = h(i)/l(i) */
u[i] = hi/li;
/* z(i) = [B(i)-h(i-1)*z(i-1)]/l(i) */
z[i] = (Bi-hm1*z[i-1])/li;
/* Update h(i-1) for next iteration */
hm1 = hi;
}
/* -- Row N -- */
/* l(N) = a(N,N)-a(N,N-1)u(N-1) */
/* z(N) = [-a(N,N-1)z(N-1)]/l(N) */
if(type == ARM_SPLINE_NATURAL)
{
/* li = 1; a(N,N) = 1; a(N,N-1) = 0 */
z[n-1] = 0; /* a(N,N-1) = 0 */
}
else if(type == ARM_SPLINE_PARABOLIC_RUNOUT)
{
li = 1+u[n-2]; /* a(N,N) = 1; a(N,N-1) = -1 */
z[n-1] = z[n-2]/li; /* a(N,N-1) = -1 */
}
/* == Solve UX = Z to obtain c(i) and */
/* compute b(i) and d(i) from c(i) == */
cp1 = z[n-1]; /* Initialize c(i+1) = c(N) = z(N) */
for (i=n-2; i>=0; i--)
{
/* c(i) = z(i)-u(i+1)c(i+1) */
c[i] = z[i]-u[i]*cp1;
hi = x[i+1]-x[i];
/* b(i) = [y(i+1)-y(i)]/h(i)-h(i)*[c(i+1)+2*c(i)]/3 */
b[i] = (y[i+1]-y[i])/hi-hi*(cp1+2*c[i])/3;
/* d(i) = [c(i+1)-c(i)]/[3*h(i)] */
d[i] = (cp1-c[i])/(3*hi);
/* Update c(i+1) for next iteration */
cp1 = c[i];
}
/* == Finally, store the coefficients in the instance == */
S->coeffs = coeffs;
}
/**
@} end of SplineInterpolate group
*/