CMSIS-DSP: Added scalar f32 quaternion functions.

Some correction for RFFT Fast f32 in Python wrapper

ARM.CMSIS.pdsc

@@ -3289,6 +3289,8 @@ and 8-bit Java bytecodes in Jazelle state.
         <!-- DSP sources (core) -->
         <file category="source"   name="CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctions.c"/>
 
+        <file category="source"   name="CMSIS/DSP/Source/QuaternionMathFunctions/QuaternionMathFunctions.c"/>
+
         <file category="source"   name="CMSIS/DSP/Source/BayesFunctions/BayesFunctions.c"/>
         <file category="source"   name="CMSIS/DSP/Source/CommonTables/CommonTables.c"/>
         <file category="source"   name="CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctions.c"/>

CMSIS/DSP/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"
 
 
 

CMSIS/DSP/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_ */

CMSIS/DSP/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);
 

CMSIS/DSP/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))

CMSIS/DSP/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

CMSIS/DSP/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")
+
+
+

CMSIS/DSP/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"