diff options
Diffstat (limited to 'Drivers/CMSIS/DSP/Source/InterpolationFunctions')
15 files changed, 1812 insertions, 0 deletions
diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/CMakeLists.txt new file mode 100644 index 0000000..7ce79cf --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/CMakeLists.txt @@ -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()
\ No newline at end of file diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctions.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctions.c new file mode 100644 index 0000000..8462395 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctions.c @@ -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" + + + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctionsF16.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctionsF16.c new file mode 100644 index 0000000..d778de9 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/InterpolationFunctionsF16.c @@ -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" + + + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f16.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f16.c new file mode 100644 index 0000000..799ed78 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f16.c @@ -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) */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f32.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f32.c new file mode 100644 index 0000000..fc001d7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_f32.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q15.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q15.c new file mode 100644 index 0000000..6726736 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q15.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q31.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q31.c new file mode 100644 index 0000000..cc8560e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q31.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q7.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q7.c new file mode 100644 index 0000000..204342e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_bilinear_interp_q7.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f16.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f16.c new file mode 100644 index 0000000..f4ae1e1 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f16.c @@ -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) */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f32.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f32.c new file mode 100644 index 0000000..fc60bec --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_f32.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q15.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q15.c new file mode 100644 index 0000000..e6339f3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q15.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q31.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q31.c new file mode 100644 index 0000000..f0c7560 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q31.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q7.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q7.c new file mode 100644 index 0000000..8d15432 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_linear_interp_q7.c @@ -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 + */ + diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_f32.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_f32.c new file mode 100644 index 0000000..3e3d091 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_f32.c @@ -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 + */ diff --git a/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_init_f32.c b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_init_f32.c new file mode 100644 index 0000000..ae3f2da --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/InterpolationFunctions/arm_spline_interp_init_f32.c @@ -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 + */ + |