CMSIS-DSP: Added scalar f32 quaternion functions.

Some correction for RFFT Fast f32 in Python wrapper

Include/arm_math.h

@@ -45,6 +45,7 @@
    * - Support Vector Machine functions (SVM)
    * - Bayes classifier functions
    * - Distance functions
+   * - Quaternion functions
    *
    * The library has generally separate functions for operating on 8-bit integers, 16-bit integers,
    * 32-bit integer and 32-bit floating-point values.
@@ -223,6 +224,7 @@
 #include "dsp/fast_math_functions.h"
 #include "dsp/transform_functions.h"
 #include "dsp/filtering_functions.h"
+#include "dsp/quaternion_math_functions.h"
 
 
 

Include/dsp/quaternion_math_functions.h

@@ -0,0 +1,155 @@
+/******************************************************************************
+ * @file     quaternion_math_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ ******************************************************************************/
+/*
+ * 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.
+ */
+
+ 
+#ifndef _QUATERNION_MATH_FUNCTIONS_H_
+#define _QUATERNION_MATH_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupQuaternionMath Quaternion Math Functions
+ * Functions to operates on quaternions and convert between a
+ * rotation and quaternion representation.
+ */
+
+
+/**
+  @brief         Floating-point quaternion Norm.
+  @param[in]     pInputQuaternions       points to the input vector of quaternions
+  @param[out]    pNorms                  points to the output vector of norms
+  @param[in]     nbQuaternions           number of quaternions in each vector
+  @return        none
+ */
+
+
+
+void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 
+    float32_t *pNorms,
+    uint32_t nbQuaternions);
+
+
+/**
+  @brief         Floating-point quaternion inverse.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pInverseQuaternions          points to the output vector of inverse quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+
+void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 
+    float32_t *pInverseQuaternions, 
+    uint32_t nbQuaternions);
+
+/**
+  @brief         Floating-point quaternion conjugates.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pConjugateQuaternions        points to the output vector of conjugate quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_conjugate_f32(const float32_t *inputQuaternions, 
+    float32_t *pConjugateQuaternions, 
+    uint32_t nbQuaternions);
+
+/**
+  @brief         Floating-point normalization of quaternions.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pNormalizedQuaternions       points to the output vector of normalized quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_normalize_f32(const float32_t *inputQuaternions, 
+    float32_t *pNormalizedQuaternions, 
+    uint32_t nbQuaternions);
+
+
+/**
+  @brief         Floating-point product of two quaternions.
+  @param[in]     qa       First quaternion
+  @param[in]     qb       Second quaternion
+  @param[out]    r        Product of two quaternions
+  @return        none
+ */
+void arm_quaternion_product_single_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r);
+
+/**
+  @brief         Floating-point elementwise product two quaternions.
+  @param[in]     qa                  First array of quaternions
+  @param[in]     qb                  Second array of quaternions
+  @param[out]    r                   Elementwise product of quaternions
+  @param[in]     nbQuaternions       Number of quaternions in the array
+  @return        none
+ */
+void arm_quaternion_product_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r,
+    uint32_t nbQuaternions);
+
+/**
+ * @brief Conversion of quaternion to equivalent rotation matrix.
+ * @param[in]       pInputQuaternions points to an array of normalized quaternions
+ * @param[out]      pOutputRotations points to an array of 3x3 rotations (in row order)
+ * @param[in]       nbQuaternions in the array
+ * @return none.
+ *
+ * <b>Format of rotation matrix</b>
+ * \par
+ * The quaternion a + ib + jc + kd is converted into rotation matrix:
+ *   a^2 + b^2 - c^2 - d^2                 2bc - 2ad                 2bd + 2ac
+ *               2bc + 2ad     a^2 - b^2 + c^2 - d^2                 2cd - 2ab
+ *               2bd - 2ac                 2cd + 2ab     a^2 - b^2 - c^2 + d^2
+ *
+ * Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22
+ */
+void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 
+    float32_t *pOutputRotations, 
+    uint32_t nbQuaternions);
+
+/**
+ * @brief Conversion of a rotation matrix to equivalent quaternion.
+ * @param[in]       pInputRotations points to an array 3x3 rotation matrix (in row order)
+ * @param[out]      pOutputQuaternions points to an array of quaternions
+ * @param[in]       nbQuaternions in the array
+ * @return none.
+*/
+void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 
+    float32_t *pOutputQuaternions,  
+    uint32_t nbQuaternions);
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _QUATERNION_MATH_FUNCTIONS_H_ */

PythonWrapper/cmsisdsp_pkg/src/cmsismodule.h

@@ -7914,7 +7914,7 @@ cmsis_arm_rfft_f32(PyObject *obj, PyObject *args)
 
 
     arm_rfft_f32(selfS->instance,pSrc_converted,pDst);
- FLOATARRAY1(pDstOBJ,2*selfS->instance->fftLenReal,pDst);
+ FLOATARRAY1(pDstOBJ,selfS->instance->fftLenReal+1,pDst);
 
     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
 
@@ -8412,11 +8412,11 @@ cmsis_arm_rfft_fast_f32(PyObject *obj, PyObject *args)
     ml_arm_rfft_fast_instance_f32Object *selfS = (ml_arm_rfft_fast_instance_f32Object *)S;
     GETARGUMENT(p,NPY_DOUBLE,double,float32_t);
 
-    pOut=PyMem_Malloc(sizeof(float32_t)*2*selfS->instance->fftLenRFFT);
+    pOut=PyMem_Malloc(sizeof(float32_t)*(selfS->instance->fftLenRFFT));
 
 
     arm_rfft_fast_f32(selfS->instance,p_converted,pOut,(uint8_t)ifftFlag);
- FLOATARRAY1(pOutOBJ,2*selfS->instance->fftLenRFFT,pOut);
+ FLOATARRAY1(pOutOBJ,(selfS->instance->fftLenRFFT),pOut);
 
     PyObject *pythonResult = Py_BuildValue("O",pOutOBJ);
 

PythonWrapper/testrfft_fast.py

@@ -0,0 +1,27 @@
+import cmsisdsp as dsp
+import numpy as np
+from scipy import signal
+import matplotlib.pyplot as plt
+import scipy.fft
+
+
+def chop(A, eps = 1e-6):
+    B = np.copy(A)
+    B[np.abs(A) < eps] = 0
+    return B
+
+nb = 32
+signal = np.cos(2 * np.pi * np.arange(nb) / nb)*np.cos(0.2*2 * np.pi * np.arange(nb) / nb)
+
+#print("{")
+#for x in signal:
+#  print("%f," % x)
+#print("}")
+
+result1=scipy.fft.rfft(signal)
+print(chop(result1))
+rfftf32=dsp.arm_rfft_fast_instance_f32()
+status=dsp.arm_rfft_fast_init_f32(rfftf32,nb)
+print(status)
+resultI = dsp.arm_rfft_fast_f32(rfftf32,signal,0)
+print(chop(resultI))

Source/CMakeLists.txt

@@ -41,6 +41,7 @@ option(SVM                  "Support Vector Machine Functions"  ON)
 option(BAYES                "Bayesian Estimators"               ON)
 option(DISTANCE             "Distance Functions"                ON)
 option(INTERPOLATION        "Interpolation Functions"                ON)
+option(QUATERNIONMATH       "Quaternion Math Functions"                ON)
 
 # When OFF it is the default behavior : all tables are included.
 option(CONFIGTABLE          "Configuration of table allowed"    OFF)
@@ -201,6 +202,11 @@ if (COMPLEXMATH)
   target_link_libraries(CMSISDSP INTERFACE CMSISDSPComplexMath)
 endif()
 
+if (QUATERNIONMATH)
+  add_subdirectory(QuaternionMathFunctions)
+  target_link_libraries(CMSISDSP INTERFACE CMSISDSPQuaternionMath)
+endif()
+
 if (CONTROLLER)
   add_subdirectory(ControllerFunctions)
   # Fast tables inclusion is allowed

Source/QuaternionMathFunctions/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required (VERSION 3.14)
+
+project(CMSISDSPQuaternionMath)
+
+include(configLib)
+include(configDsp)
+
+
+
+add_library(CMSISDSPQuaternionMath STATIC arm_quaternion_norm_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_inverse_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_conjugate_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_normalize_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_product_single_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_product_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion2rotation_f32.c)
+target_sources(CMSISDSPQuaternionMath PRIVATE arm_rotation2quaternion_f32.c)
+
+
+if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16))
+endif()
+
+
+configLib(CMSISDSPQuaternionMath ${ROOT})
+configDsp(CMSISDSPQuaternionMath ${ROOT})
+
+### Includes
+target_include_directories(CMSISDSPQuaternionMath PUBLIC "${DSP}/Include")
+
+
+

Source/QuaternionMathFunctions/QuaternionMathFunctions.c

@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        QuaternionMathFunctions.c
+ * Description:  Combination of all quaternion math function source files.
+ *
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2019-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 "arm_quaternion_norm_f32.c"
+#include "arm_quaternion_inverse_f32.c"
+#include "arm_quaternion_conjugate_f32.c"
+#include "arm_quaternion_normalize_f32.c"
+#include "arm_quaternion_product_single_f32.c"
+#include "arm_quaternion_product_f32.c"
+#include "arm_quaternion2rotation_f32.c"
+#include "arm_rotation2quaternion_f32.c"

Source/QuaternionMathFunctions/arm_quaternion2rotation_f32.c

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion2rotation_f32.c
+ * Description:  Floating-point quaternion 2 rotation conversion
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatConv Quaternion conversions
+
+  Conversions between quaternion and rotation representations.
+ */
+
+/**
+  @ingroup QuatConv
+ */
+
+/**
+  @defgroup QuatRot Quaternion to Rotation
+
+  Conversions from quaternion to rotation.
+ */
+
+/**
+  @addtogroup QuatRot
+  @{
+ */
+
+/**
+   @brief Conversion of quaternion to equivalent rotation matrix.
+   @param[in]       pInputQuaternions points to an array of normalized quaternions
+   @param[out]      pOutputRotations points to an array of 3x3 rotations (in row order)
+   @param[in]       nbQuaternions number of quaternions in the array
+   @return none.
+  
+   @par
+   Format of rotation matrix
+   
+   
+   The quaternion a + ib + jc + kd is converted into rotation matrix:
+   <pre>
+     a^2 + b^2 - c^2 - d^2                 2bc - 2ad                 2bd + 2ac
+                 2bc + 2ad     a^2 - b^2 + c^2 - d^2                 2cd - 2ab
+                 2bd - 2ac                 2cd + 2ab     a^2 - b^2 - c^2 + d^2
+   </pre>
+   Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22
+ */
+void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 
+    float32_t *pOutputRotations, 
+    uint32_t nbQuaternions)
+{
+   for(uint32_t nb=0; nb < nbQuaternions; nb++)
+   {
+        float32_t q00 = SQ(pInputQuaternions[0 + nb * 4]);
+        float32_t q11 = SQ(pInputQuaternions[1 + nb * 4]);
+        float32_t q22 = SQ(pInputQuaternions[2 + nb * 4]);
+        float32_t q33 = SQ(pInputQuaternions[3 + nb * 4]);
+        float32_t q01 =  pInputQuaternions[0 + nb * 4]*pInputQuaternions[1 + nb * 4];
+        float32_t q02 =  pInputQuaternions[0 + nb * 4]*pInputQuaternions[2 + nb * 4];
+        float32_t q03 =  pInputQuaternions[0 + nb * 4]*pInputQuaternions[3 + nb * 4];
+        float32_t q12 =  pInputQuaternions[1 + nb * 4]*pInputQuaternions[2 + nb * 4];
+        float32_t q13 =  pInputQuaternions[1 + nb * 4]*pInputQuaternions[3 + nb * 4];
+        float32_t q23 =  pInputQuaternions[2 + nb * 4]*pInputQuaternions[3 + nb * 4];
+
+        float32_t xx = q00 + q11 - q22 - q33;
+        float32_t yy = q00 - q11 + q22 - q33;
+        float32_t zz = q00 - q11 - q22 + q33;
+        float32_t xy = 2*(q12 - q03);
+        float32_t xz = 2*(q13 + q02);
+        float32_t yx = 2*(q12 + q03);
+        float32_t yz = 2*(q23 - q01);
+        float32_t zx = 2*(q13 - q02);
+        float32_t zy = 2*(q23 + q01);
+
+        pOutputRotations[0 + nb * 9] = xx; pOutputRotations[1 + nb * 9] = xy; pOutputRotations[2 + nb * 9] = xz;
+        pOutputRotations[3 + nb * 9] = yx; pOutputRotations[4 + nb * 9] = yy; pOutputRotations[5 + nb * 9] = yz;
+        pOutputRotations[6 + nb * 9] = zx; pOutputRotations[7 + nb * 9] = zy; pOutputRotations[8 + nb * 9] = zz;
+   }
+}
+
+/**
+  @} end of QuatRot group
+ */

Source/QuaternionMathFunctions/arm_quaternion_conjugate_f32.c

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_conjugate_f32.c
+ * Description:  Floating-point quaternion conjugate
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatConjugate Quaternion Conjugate
+
+  Compute the conjugate of a quaternion.
+ */
+
+/**
+  @addtogroup QuatConjugate
+  @{
+ */
+
+/**
+  @brief         Floating-point quaternion conjugates.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pConjugateQuaternions        points to the output vector of conjugate quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_conjugate_f32(const float32_t *pInputQuaternions, 
+    float32_t *pConjugateQuaternions, 
+    uint32_t nbQuaternions)
+{
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+
+      pConjugateQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0];
+      pConjugateQuaternions[4 * i + 1] = -pInputQuaternions[4 * i + 1];
+      pConjugateQuaternions[4 * i + 2] = -pInputQuaternions[4 * i + 2];
+      pConjugateQuaternions[4 * i + 3] = -pInputQuaternions[4 * i + 3];
+   }
+}
+
+/**
+  @} end of QuatConjugate group
+ */

Source/QuaternionMathFunctions/arm_quaternion_inverse_f32.c

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_inverse_f32.c
+ * Description:  Floating-point quaternion inverse
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatInverse Quaternion Inverse
+
+  Compute the inverse of a quaternion.
+ */
+
+/**
+  @addtogroup QuatInverse
+  @{
+ */
+
+/**
+  @brief         Floating-point quaternion inverse.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pInverseQuaternions          points to the output vector of inverse quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+
+
+
+void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 
+  float32_t *pInverseQuaternions,
+  uint32_t nbQuaternions)
+{
+   float32_t temp;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+
+      temp = SQ(pInputQuaternions[4 * i + 0]) +
+             SQ(pInputQuaternions[4 * i + 1]) +
+             SQ(pInputQuaternions[4 * i + 2]) +
+             SQ(pInputQuaternions[4 * i + 3]);
+
+      pInverseQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0] / temp;
+      pInverseQuaternions[4 * i + 1] = -pInputQuaternions[4 * i + 1] / temp;
+      pInverseQuaternions[4 * i + 2] = -pInputQuaternions[4 * i + 2] / temp;
+      pInverseQuaternions[4 * i + 3] = -pInputQuaternions[4 * i + 3] / temp;
+   }
+}
+
+/**
+  @} end of QuatInverse group
+ */

Source/QuaternionMathFunctions/arm_quaternion_norm_f32.c

@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_norm_f32.c
+ * Description:  Floating-point quaternion Norm
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatNorm Quaternion Norm
+
+  Compute the norm of a quaternion.
+ */
+
+/**
+  @addtogroup QuatNorm
+  @{
+ */
+
+/**
+  @brief         Floating-point quaternion Norm.
+  @param[in]     pInputQuaternions       points to the input vector of quaternions
+  @param[out]    pNorms                  points to the output vector of norms
+  @param[in]     nbQuaternions           number of quaternions in the input vector
+  @return        none
+ */
+
+
+
+void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 
+  float32_t *pNorms,
+  uint32_t nbQuaternions)
+{
+   float32_t temp;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+      temp = SQ(pInputQuaternions[4 * i + 0]) +
+             SQ(pInputQuaternions[4 * i + 1]) +
+             SQ(pInputQuaternions[4 * i + 2]) +
+             SQ(pInputQuaternions[4 * i + 3]);
+      pNorms[i] = sqrtf(temp);
+   }
+}
+
+/**
+  @} end of QuatNorm group
+ */

Source/QuaternionMathFunctions/arm_quaternion_normalize_f32.c

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_normalize_f32.c
+ * Description:  Floating-point quaternion normalization
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatNormalized Quaternion normalization
+
+  Compute a normalized quaternion.
+ */
+
+/**
+  @addtogroup QuatNormalized
+  @{
+ */
+
+/**
+  @brief         Floating-point normalization of quaternions.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pNormalizedQuaternions       points to the output vector of normalized quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions, 
+    float32_t *pNormalizedQuaternions, 
+    uint32_t nbQuaternions)
+{
+   float32_t temp;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+      temp = SQ(pInputQuaternions[4 * i + 0]) +
+             SQ(pInputQuaternions[4 * i + 1]) +
+             SQ(pInputQuaternions[4 * i + 2]) +
+             SQ(pInputQuaternions[4 * i + 3]);
+      temp = sqrtf(temp);
+
+      pNormalizedQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0] / temp;
+      pNormalizedQuaternions[4 * i + 1] = pInputQuaternions[4 * i + 1] / temp;
+      pNormalizedQuaternions[4 * i + 2] = pInputQuaternions[4 * i + 2] / temp;
+      pNormalizedQuaternions[4 * i + 3] = pInputQuaternions[4 * i + 3] / temp;
+   }
+}
+
+/**
+  @} end of QuatNormalized group
+ */

Source/QuaternionMathFunctions/arm_quaternion_product_f32.c

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_product_f32.c
+ * Description:  Floating-point quaternion product
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatProd Quaternion Product
+
+  Compute the product of quaternions.
+ */
+
+/**
+  @addtogroup QuatProd
+  @{
+ */
+
+/**
+  @brief         Floating-point elementwise product two quaternions.
+  @param[in]     qa                  first array of quaternions
+  @param[in]     qb                  second array of quaternions
+  @param[out]    r                   elementwise product of quaternions
+  @param[in]     nbQuaternions       number of quaternions in the array
+  @return        none
+ */
+void arm_quaternion_product_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r,
+    uint32_t nbQuaternions)
+{
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+     arm_quaternion_product_single_f32(qa, qb, r);
+
+     qa += 4;
+     qb += 4;
+     r += 4;
+   }
+}
+
+/**
+  @} end of QuatProd group
+ */

Source/QuaternionMathFunctions/arm_quaternion_product_single_f32.c

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_product_single_f32.c
+ * Description:  Floating-point quaternion product
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatProd Quaternion Product
+
+  Compute the product of quaternions.
+ */
+
+/**
+  @addtogroup QuatProdSingle
+  @{
+ */
+
+/**
+  @brief         Floating-point product of two quaternions.
+  @param[in]     qa       first quaternion
+  @param[in]     qb       second quaternion
+  @param[out]    r        product of two quaternions
+  @return        none
+ */
+void arm_quaternion_product_single_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r)
+{
+    r[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
+    r[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2];
+    r[2] = qa[0] * qb[2] + qa[2] * qb[0] + qa[3] * qb[1] - qa[1] * qb[3];
+    r[3] = qa[0] * qb[3] + qa[3] * qb[0] + qa[1] * qb[2] - qa[2] * qb[1];
+}
+
+/**
+  @} end of QuatProdSingle group
+ */

Source/QuaternionMathFunctions/arm_rotation2quaternion_f32.c

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_rotation2quaternion_f32.c
+ * Description:  Floating-point rotation to quaternion conversion
+ *
+ *
+ * Target Processor: Cortex-M 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/quaternion_math_functions.h"
+#include <math.h>
+
+#define RI(x,y) r[(3*(x) + (y))]
+
+
+/**
+  @ingroup QuatConv
+ */
+
+/**
+  @defgroup RotQuat Rotation to Quaternion
+
+  Conversions from rotation to quaternion.
+ */
+
+/**
+  @addtogroup RotQuat
+  @{
+ */
+
+/**
+ * @brief Conversion of a rotation matrix to an equivalent quaternion.
+ * @param[in]       pInputRotations points to an array 3x3 rotation matrix (in row order)
+ * @param[out]      pOutputQuaternions points to an array quaternions
+ * @param[in]       nbQuaternions number of quaternions in the array
+ * @return none.
+ *
+ * q and -q are representing the same rotation. This ambiguity must be taken into
+ * account when using the output of this function.
+ * 
+ */
+void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 
+    float32_t *pOutputQuaternions,  
+    uint32_t nbQuaternions)
+{
+   for(uint32_t nb=0; nb < nbQuaternions; nb++)
+   {
+       const float32_t *r=&pInputRotations[nb*9];
+       float32_t *q=&pOutputQuaternions[nb*4];
+
+       float32_t trace = RI(0,0) + RI(1,1) + RI(2,2);
+
+       float32_t doubler;
+       float32_t s;
+
+
+
+      if (trace > 0)
+      {
+        doubler = sqrtf(trace + 1.0) * 2; // invs=4*qw
+        s = 1.0 / doubler;
+        q[0] = 0.25 * doubler;
+        q[1] = (RI(2,1) - RI(1,2)) * s;
+        q[2] = (RI(0,2) - RI(2,0)) * s;
+        q[3] = (RI(1,0) - RI(0,1)) * s;
+      }
+      else if ((RI(0,0) > RI(1,1)) && (RI(0,0) > RI(2,2)) )
+      {
+        doubler = sqrtf(1.0 + RI(0,0) - RI(1,1) - RI(2,2)) * 2; // invs=4*qx
+        s = 1.0 / doubler;
+        q[0] = (RI(2,1) - RI(1,2)) * s;
+        q[1] = 0.25 * doubler;
+        q[2] = (RI(0,1) + RI(1,0)) * s;
+        q[3] = (RI(0,2) + RI(2,0)) * s;
+      }
+      else if (RI(1,1) > RI(2,2))
+      {
+        doubler = sqrtf(1.0 + RI(1,1) - RI(0,0) - RI(2,2)) * 2; // invs=4*qy
+        s = 1.0 / doubler;
+        q[0] = (RI(0,2) - RI(2,0)) * s;
+        q[1] = (RI(0,1) + RI(1,0)) * s;
+        q[2] = 0.25 * doubler;
+        q[3] = (RI(1,2) + RI(2,1)) * s;
+      }
+      else
+      {
+        doubler = sqrtf(1.0 + RI(2,2) - RI(0,0) - RI(1,1)) * 2; // invs=4*qz
+        s = 1.0 / doubler;
+        q[0] = (RI(1,0) - RI(0,1)) * s;
+        q[1] = (RI(0,2) + RI(2,0)) * s;
+        q[2] = (RI(1,2) + RI(2,1)) * s;
+        q[3] = 0.25 * doubler;
+      }
+
+    }
+}
+
+/**
+  @} end of RotQuat group
+ */

Testing/CMakeLists.txt

@@ -249,6 +249,11 @@ if (BASICMATH)
   Source/Tests/BasicTestsQ7.cpp)
 endif()
 
+if (QUATERNIONMATH)
+  set(QUATERNIONMATHSRC Source/Tests/QuaternionTestsF32.cpp
+  )
+endif()
+
 if (COMPLEXMATH)
   set(COMPLEXMATHSRC Source/Tests/ComplexTestsF32.cpp
   Source/Tests/ComplexTestsQ31.cpp
@@ -336,6 +341,13 @@ if (DISTANCE)
   Source/Tests/DistanceTestsU32.cpp)
 endif()
 
+if (INTERPOLATION)
+  set(INTERPOLATIONSRC Source/Tests/InterpolationTestsF32.cpp
+     Source/Tests/InterpolationTestsQ31.cpp
+     Source/Tests/InterpolationTestsQ15.cpp
+     Source/Tests/InterpolationTestsQ7.cpp)
+endif()
+
 set(TESTSRC 
      ${BASICMATHSRC}
      ${COMPLEXMATHSRC}
@@ -349,14 +361,12 @@ set(TESTSRC
      ${SVMSRC}
      ${BAYESSRC}
      ${DISTANCESRC}
-     Source/Tests/InterpolationTestsF32.cpp
+     ${QUATERNIONMATHSRC}
-     Source/Tests/InterpolationTestsQ31.cpp
+     ${INTERPOLATIONSRC}
-     Source/Tests/InterpolationTestsQ15.cpp
+     #Source/Tests/ExampleCategoryF32.cpp
-     Source/Tests/InterpolationTestsQ7.cpp
+     #Source/Tests/ExampleCategoryQ31.cpp
-     Source/Tests/ExampleCategoryF32.cpp
+     #Source/Tests/ExampleCategoryQ15.cpp
-     Source/Tests/ExampleCategoryQ31.cpp
+     #Source/Tests/ExampleCategoryQ7.cpp
-     Source/Tests/ExampleCategoryQ15.cpp
-     Source/Tests/ExampleCategoryQ7.cpp
   )
 
 

Testing/Include/Tests/QuaternionTestsF32.h

@@ -0,0 +1,20 @@
+#include "Test.h"
+#include "Pattern.h"
+
+#include "dsp/quaternion_math_functions.h"
+
+class QuaternionTestsF32:public Client::Suite
+    {
+        public:
+            QuaternionTestsF32(Testing::testID_t id);
+            virtual void setUp(Testing::testID_t,std::vector<Testing::param_t>& params,Client::PatternMgr *mgr);
+            virtual void tearDown(Testing::testID_t,Client::PatternMgr *mgr);
+        private:
+            #include "QuaternionTestsF32_decl.h"
+            
+            Client::Pattern<float32_t> input1;
+            Client::Pattern<float32_t> input2;
+            Client::LocalPattern<float32_t> output;
+            // Reference patterns are not loaded when we are in dump mode
+            Client::RefPattern<float32_t> ref;
+    };

Testing/Parameters/DSP/QuaternionMaths/QuaternionMathsF32/Params1.txt

@@ -0,0 +1,82 @@
+81
+1
+1
+1
+1
+1
+3
+1
+1
+5
+1
+3
+1
+1
+3
+3
+1
+3
+5
+1
+5
+1
+1
+5
+3
+1
+5
+5
+3
+1
+1
+3
+1
+3
+3
+1
+5
+3
+3
+1
+3
+3
+3
+3
+3
+5
+3
+5
+1
+3
+5
+3
+3
+5
+5
+5
+1
+1
+5
+1
+3
+5
+1
+5
+5
+3
+1
+5
+3
+3
+5
+3
+5
+5
+5
+1
+5
+5
+3
+5
+5
+5

Testing/PatternGeneration/Quaternion.py

@@ -0,0 +1,79 @@
+import os.path
+import numpy as np
+import itertools
+import Tools
+from pyquaternion import Quaternion
+
+# mult, multvec, inverse, conjugate, normalize rot2quat, quat2rot , norm
+def flattenQuat(l):
+    return(np.array([list(x) for x in l]).reshape(4*len(l)))
+
+def flattenRot(l):
+    return(np.array([list(x) for x in l]).reshape(9*len(l)))
+
+# q and -q are representing the same rotation.
+# So there is an ambiguity for the tests.
+# We force the real part of be positive.
+def mkQuaternion(mat):
+    q=Quaternion(matrix=mat)
+    if q.scalar < 0:
+        return(-q)
+    else:
+        return(q)
+
+def writeTests(config,format):
+    NBSAMPLES=128
+
+    a=[Quaternion.random() for x in range(NBSAMPLES)]
+    b=[Quaternion.random() for x in range(NBSAMPLES)]
+
+    config.writeInput(1, flattenQuat(a))
+    config.writeInput(2, flattenQuat(b))
+
+    normTest = [x.norm for x in a]
+    config.writeReference(1, normTest)
+
+    inverseTest = [x.inverse for x in a]
+    config.writeReference(2, flattenQuat(inverseTest))
+
+    conjugateTest = [x.conjugate for x in a]
+    config.writeReference(3, flattenQuat(conjugateTest))
+
+    normalizeTest = [x.normalised for x in a]
+    config.writeReference(4, flattenQuat(normalizeTest))
+
+    multTest = [a[i] * b[i] for i in range(NBSAMPLES)]
+    config.writeReference(5, flattenQuat(multTest))
+
+    quat2RotTest = [x.rotation_matrix for x in a]
+    config.writeReference(6, flattenRot(quat2RotTest))
+
+    config.writeInput(7, flattenRot(quat2RotTest))
+    rot2QuatTest = [mkQuaternion(x) for x in quat2RotTest]
+    config.writeReference(7, flattenQuat(rot2QuatTest))
+
+
+
+    
+
+
+def generatePatterns():
+    PATTERNDIR = os.path.join("Patterns","DSP","QuaternionMaths","QuaternionMaths")
+    PARAMDIR = os.path.join("Parameters","DSP","QuaternionMaths","QuaternionMaths")
+    
+    configf32=Tools.Config(PATTERNDIR,PARAMDIR,"f32")
+    configf16=Tools.Config(PATTERNDIR,PARAMDIR,"f16")
+    
+    
+    writeTests(configf32,0)
+    writeTests(configf16,16)
+
+
+    # Params just as example
+    someLists=[[1,3,5],[1,3,5],[1,3,5]]
+    
+    r=np.array([element for element in itertools.product(*someLists)])
+    configf32.writeParam(1, r.reshape(81))
+
+if __name__ == '__main__':
+  generatePatterns()

Testing/Patterns/DSP/QuaternionMaths/QuaternionMathsF16/Reference1_f16.txt

@@ -0,0 +1,258 @@
+H
+128
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00
+// 1.000000
+0x3c00

Testing/Patterns/DSP/QuaternionMaths/QuaternionMathsF32/Reference1_f32.txt

@@ -0,0 +1,258 @@
+W
+128
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000
+// 1.000000
+0x3f800000

Testing/Source/Tests/QuaternionTestsF32.cpp

@@ -0,0 +1,234 @@
+#include "QuaternionTestsF32.h"
+#include <stdio.h>
+#include "Error.h"
+
+#define SNR_THRESHOLD 120
+
+/* 
+
+Reference patterns are generated with
+a double precision computation.
+
+*/
+#define REL_ERROR (1.0e-6)
+#define ABS_ERROR (1.0e-7)
+
+
+
+    void QuaternionTestsF32::test_quaternion_norm_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion_norm_f32(inp1,outp,output.nbSamples());
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion_inverse_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion_inverse_f32(inp1,outp,input1.nbSamples() >> 2);
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion_conjugate_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion_conjugate_f32(inp1,outp,input1.nbSamples() >> 2);
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion_normalize_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion_normalize_f32(inp1,outp,input1.nbSamples() >> 2);
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion_prod_single_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        const float32_t *inp2=input2.ptr();
+        float32_t *outp=output.ptr();
+
+        for(uint32_t i=0; i < input1.nbSamples() >> 2; i++)
+        {
+           arm_quaternion_product_single_f32(inp1,inp2,outp);
+           outp += 4;
+           inp1 += 4;
+           inp2 += 4;
+        }
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion_product_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        const float32_t *inp2=input2.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion_product_f32(inp1,inp2,outp,input1.nbSamples() >> 2);
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_quaternion2rotation_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        arm_quaternion2rotation_f32(inp1,outp,input1.nbSamples() >> 2);
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+    void QuaternionTestsF32::test_rotation2quaternion_f32()
+    {
+        const float32_t *inp1=input1.ptr();
+        float32_t *outp=output.ptr();
+
+        /*
+
+        q and -q are representing the same rotation.
+        To remove the ambiguity we force the real part ot be positive.
+        Same convention followed in Python script.
+
+        */
+
+        arm_rotation2quaternion_f32(inp1,outp,output.nbSamples() >> 2);
+
+        /*  Remove ambiguity */
+        for(uint32_t i=0; i < output.nbSamples() >> 2 ; i++)
+        {
+            if (outp[0] < 0.0f)
+            {
+                outp[0] = -outp[0];
+                outp[1] = -outp[1];
+                outp[2] = -outp[2];
+                outp[3] = -outp[3];
+            }
+
+            outp += 4;
+        }
+
+        ASSERT_EMPTY_TAIL(output);
+
+        ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
+
+        ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
+
+    } 
+
+
+ 
+    void QuaternionTestsF32::setUp(Testing::testID_t id,std::vector<Testing::param_t>& params,Client::PatternMgr *mgr)
+    {
+      
+       (void)params;
+
+       Testing::nbSamples_t nb=MAX_NB_SAMPLES; 
+
+       
+       switch(id)
+       {
+          case QuaternionTestsF32::TEST_QUATERNION_NORM_F32_1:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_NORM_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION_INVERSE_F32_2:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_INVERSE_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION_CONJUGATE_F32_3:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_CONJUGATE_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION_NORMALIZE_F32_4:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_NORMALIZE_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION_PROD_SINGLE_F32_5:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            input2.reload(QuaternionTestsF32::INPUT2_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_MULT_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION_PRODUCT_F32_6:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            input2.reload(QuaternionTestsF32::INPUT2_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_MULT_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_QUATERNION2ROTATION_F32_7:
+            input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_QUAT2ROT_F32_ID,mgr,nb);
+          break;
+
+          case QuaternionTestsF32::TEST_ROTATION2QUATERNION_F32_8:
+            input1.reload(QuaternionTestsF32::INPUT7_F32_ID,mgr,nb);
+            ref.reload(QuaternionTestsF32::REF_ROT2QUAT_F32_ID,mgr,nb);
+          break;
+
+       }
+      
+
+       
+
+       output.create(ref.nbSamples(),QuaternionTestsF32::OUT_SAMPLES_F32_ID,mgr);
+    }
+
+    void QuaternionTestsF32::tearDown(Testing::testID_t id,Client::PatternMgr *mgr)
+    {
+        (void)id;
+        output.dump(mgr);
+    }

Testing/desc.txt

@@ -585,6 +585,42 @@ group Root {
           }
         }
 
+        group Quaternion Tests {
+           class = QuaternionTests
+           folder = QuaternionMaths
+
+           suite Quaternion Tests F32{
+              class = QuaternionTestsF32
+              folder = QuaternionMathsF32
+   
+              Pattern INPUT1_F32_ID : Input1_f32.txt 
+              Pattern INPUT2_F32_ID : Input2_f32.txt 
+              Pattern INPUT7_F32_ID : Input7_f32.txt 
+   
+              Pattern REF_NORM_F32_ID : Reference1_f32.txt
+              Pattern REF_INVERSE_F32_ID : Reference2_f32.txt
+              Pattern REF_CONJUGATE_F32_ID : Reference3_f32.txt
+              Pattern REF_NORMALIZE_F32_ID : Reference4_f32.txt
+              Pattern REF_MULT_F32_ID : Reference5_f32.txt
+              Pattern REF_QUAT2ROT_F32_ID : Reference6_f32.txt
+              Pattern REF_ROT2QUAT_F32_ID : Reference7_f32.txt
+            
+   
+              Output  OUT_SAMPLES_F32_ID : Output
+   
+              Functions {
+                Test arm_quaternion_norm_f32:test_quaternion_norm_f32
+                Test arm_quaternion_inverse_f32:test_quaternion_inverse_f32
+                Test arm_quaternion_conjugate_f32:test_quaternion_conjugate_f32
+                Test arm_quaternion_normalize_f32:test_quaternion_normalize_f32
+                Test arm_quaternion_prod_single_f32:test_quaternion_prod_single_f32
+                Test arm_quaternion_product_f32:test_quaternion_product_f32
+                Test arm_quaternion2rotation_f32:test_quaternion2rotation_f32
+                Test arm_rotation2quaternion_f32:test_rotation2quaternion_f32
+              }
+           }
+        }
+
         group Basic Tests {
            class = BasicTests
            folder = BasicMaths

cmsisdspconfig.py

@@ -56,6 +56,7 @@ config["SVM"]=True
 config["BAYES"]=True        
 config["DISTANCE"]=True     
 config["INTERPOLATION"]=True
+config["QUATERNIONMATH"]=True
 
 config["LOOPUNROLL"]=True
 config["ROUNDING"]=False
@@ -91,6 +92,8 @@ defaulton["SVM"]=True
 defaulton["BAYES"]=True        
 defaulton["DISTANCE"]=True     
 defaulton["INTERPOLATION"]=True
+defaulton["QUATERNIONMATH"]=True
+
 
 CFFTSIZE=[16,32,64,128,256,512,1024,2048,4096]
 CFFTDATATYPE=['F64','F32','F16','Q31','Q15']
@@ -479,7 +482,7 @@ def configCMake(config):
                        "SVM",           
                        "BAYES",         
                        "DISTANCE",      
-                       "INTERPOLATION"])  
+                       "INTERPOLATION","QUATERNIONMATH"])  
     configMake(config)
 
 genconfig(config,"CFFT",CFFTSIZE,CFFTDATATYPE)