initial commit

10 files changed, 1148 insertions, 0 deletions

diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/CMakeLists.txt
new file mode 100644
index 0000000..267626f
--- /dev/null
+++ b/Drivers/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")
+
+
+
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/QuaternionMathFunctions.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/QuaternionMathFunctions.c
new file mode 100644
index 0000000..7c48178
--- /dev/null
+++ b/Drivers/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"
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion2rotation_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion2rotation_f32.c
new file mode 100644
index 0000000..2105341
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion2rotation_f32.c
@@ -0,0 +1,181 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion2rotation_f32.c
+ * Description:  Floating-point quaternion 2 rotation conversion
+ *
+ * $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/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
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 
+    float32_t *pOutputRotations, 
+    uint32_t nbQuaternions)
+{
+  f32x4_t vec0,vec1, vec2 ,vec3;
+  float32_t q2q3, tmp1, tmp2 ;
+
+  for(uint32_t nb=0; nb < nbQuaternions; nb++)
+  {
+
+    // q0 q1 q2 q3
+    vec0 = vld1q(pInputQuaternions);
+
+    // q0^2 q1^2 q2^2 q3^2
+    vec1 = vmulq(vec0,vec0);
+
+    // q0^2 q1q0 q2q0 q3q0
+    vec2 = vmulq_n_f32(vec0, vgetq_lane(vec0,0));
+
+    // 2 (q0^2 q1q0 q2q0 q3q0)
+    vec2 = vmulq_n_f32(vec2, 2.0f);
+    
+
+    // 2 q2q3
+    q2q3 = vgetq_lane(vec0,2) * vgetq_lane(vec0,3);
+    q2q3 = q2q3 * 2.0f;
+
+    // 2 (q0q1 q1^2 q2q1 q3q1)
+    vec3 = vmulq_n_f32(vec0, vgetq_lane(vec0,1));
+    vec3 = vmulq_n_f32(vec3, 2.0f);
+   
+
+    
+    vec0 = vsetq_lane(vgetq_lane(vec1,0) + vgetq_lane(vec1,1),vec0,0);
+    vec0 = vsetq_lane(vgetq_lane(vec0,0) - vgetq_lane(vec1,2),vec0,0);
+    vec0 = vsetq_lane(vgetq_lane(vec0,0) - vgetq_lane(vec1,3),vec0,0);
+    vec0 = vsetq_lane(vgetq_lane(vec3,2) - vgetq_lane(vec2,3),vec0,1);
+    vec0 = vsetq_lane(vgetq_lane(vec3,3) + vgetq_lane(vec2,2),vec0,2);
+    vec0 = vsetq_lane(vgetq_lane(vec3,2) + vgetq_lane(vec2,3),vec0,3);
+
+    vst1q(pOutputRotations, vec0);
+    pOutputRotations += 4;
+
+    tmp1 = vgetq_lane(vec1,0) - vgetq_lane(vec1,1);
+    tmp2 = vgetq_lane(vec1,2) - vgetq_lane(vec1,3);
+
+  
+    vec0 = vsetq_lane(tmp1 + tmp2,vec0,0);
+    vec0 = vsetq_lane(q2q3 - vgetq_lane(vec2,1) ,vec0,1);
+    vec0 = vsetq_lane(vgetq_lane(vec3,3) - vgetq_lane(vec2,2),vec0,2);
+    vec0 = vsetq_lane(q2q3 + vgetq_lane(vec2,1) ,vec0,3);
+
+    vst1q(pOutputRotations, vec0);
+    pOutputRotations += 4;
+
+    *pOutputRotations = tmp1 - tmp2;
+    pOutputRotations ++;
+
+    pInputQuaternions += 4;
+  }
+}
+
+#else
+void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 
+    float32_t *pOutputRotations, 
+    uint32_t nbQuaternions)
+{
+   uint32_t nb;
+   for(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;
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatRot group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_conjugate_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_conjugate_f32.c
new file mode 100644
index 0000000..213a5bb
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_conjugate_f32.c
@@ -0,0 +1,98 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_conjugate_f32.c
+ * Description:  Floating-point quaternion conjugate
+ *
+ * $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/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
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+void arm_quaternion_conjugate_f32(const float32_t *pInputQuaternions, 
+    float32_t *pConjugateQuaternions, 
+    uint32_t nbQuaternions)
+{
+   f32x4_t vec1;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+      vec1 = vld1q(pInputQuaternions);
+      
+
+      vec1 = vsetq_lane_f32(-vgetq_lane(vec1, 0),vec1,0);
+      vec1 = vnegq_f32(vec1);
+
+      vst1q(pConjugateQuaternions, vec1);
+
+
+      pInputQuaternions   += 4;
+      pConjugateQuaternions += 4;
+   }
+}
+#else
+void arm_quaternion_conjugate_f32(const float32_t *pInputQuaternions, 
+    float32_t *pConjugateQuaternions, 
+    uint32_t nbQuaternions)
+{
+   uint32_t i;
+   for(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];
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatConjugate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_inverse_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_inverse_f32.c
new file mode 100644
index 0000000..d82bbbf
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_inverse_f32.c
@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_inverse_f32.c
+ * Description:  Floating-point quaternion inverse
+ *
+ * $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/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
+ */
+
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 
+  float32_t *pInverseQuaternions,
+  uint32_t nbQuaternions)
+{
+   f32x4_t vec1,vec2;
+   float32_t squaredSum;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+     
+      vec1 = vld1q(pInputQuaternions);
+      vec2 = vmulq(vec1,vec1);
+      squaredSum = vecAddAcrossF32Mve(vec2);
+      
+
+      vec1 = vmulq_n_f32(vec1, 1.0f / squaredSum);
+      vec1 = vsetq_lane_f32(-vgetq_lane(vec1, 0),vec1,0);
+      vec1 = vnegq_f32(vec1);
+
+      vst1q(pInverseQuaternions, vec1);
+
+
+      pInputQuaternions   += 4;
+      pInverseQuaternions += 4;
+
+   }
+}
+
+#else
+void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 
+  float32_t *pInverseQuaternions,
+  uint32_t nbQuaternions)
+{
+   float32_t temp;
+
+   uint32_t i;
+   for(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;
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatInverse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_norm_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_norm_f32.c
new file mode 100644
index 0000000..af6653a
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_norm_f32.c
@@ -0,0 +1,102 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_norm_f32.c
+ * Description:  Floating-point quaternion Norm
+ *
+ * $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/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
+ */
+
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 
+  float32_t *pNorms,
+  uint32_t nbQuaternions)
+{
+  f32x4_t vec1;
+  float32_t squaredSum;
+
+  for(uint32_t i=0; i < nbQuaternions; i++)
+  {
+       vec1 = vld1q(pInputQuaternions);
+       vec1 = vmulq(vec1,vec1);
+       squaredSum = vecAddAcrossF32Mve(vec1);
+       arm_sqrt_f32(squaredSum,pNorms);
+
+       pInputQuaternions+= 4;
+       pNorms ++;
+  }
+
+}
+
+#else
+
+void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 
+  float32_t *pNorms,
+  uint32_t nbQuaternions)
+{
+   float32_t temp;
+   uint32_t i;
+
+   for(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);
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatNorm group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_normalize_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_normalize_f32.c
new file mode 100644
index 0000000..605e558
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_normalize_f32.c
@@ -0,0 +1,107 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_normalize_f32.c
+ * Description:  Floating-point quaternion normalization
+ *
+ * $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/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
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions, 
+    float32_t *pNormalizedQuaternions, 
+    uint32_t nbQuaternions)
+{
+   f32x4_t vec1,vec2;
+   float32_t squaredSum,norm;
+
+   for(uint32_t i=0; i < nbQuaternions; i++)
+   {
+      vec1 = vld1q(pInputQuaternions);
+      vec2 = vmulq(vec1,vec1);
+      squaredSum = vecAddAcrossF32Mve(vec2);
+      arm_sqrt_f32(squaredSum,&norm);
+      vec1 = vmulq_n_f32(vec1, 1.0f / norm);
+      vst1q(pNormalizedQuaternions, vec1);
+
+      pInputQuaternions += 4;
+      pNormalizedQuaternions += 4;
+
+   }
+}
+
+#else
+void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions, 
+    float32_t *pNormalizedQuaternions, 
+    uint32_t nbQuaternions)
+{
+   float32_t temp;
+
+   uint32_t i;
+   for(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;
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatNormalized group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_f32.c
new file mode 100644
index 0000000..83ebf5e
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_f32.c
@@ -0,0 +1,149 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_product_f32.c
+ * Description:  Floating-point quaternion product
+ *
+ * $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/quaternion_math_functions.h"
+#include <math.h>
+
+/**
+  @ingroup groupQuaternionMath
+ */
+
+/**
+  @defgroup QuatProd Quaternion Product
+
+  Compute the product of quaternions.
+ */
+
+/**
+  @ingroup QuatProd
+ */
+
+/**
+  @defgroup QuatProdVect Elementwise Quaternion Product
+
+  Compute the elementwise product of quaternions.
+ */
+
+/**
+  @addtogroup QuatProdVect
+  @{
+ */
+
+/**
+  @brief         Floating-point elementwise product two quaternions.
+  @param[in]     qa                  first array of quaternions
+  @param[in]     qb                  second array of quaternions
+  @param[out]    qr                   elementwise product of quaternions
+  @param[in]     nbQuaternions       number of quaternions in the array
+  @return        none
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+void arm_quaternion_product_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *qr,
+    uint32_t nbQuaternions)
+{
+    static uint32_t patternA[4] = { 0, 1, 0, 1 };
+    static uint32_t patternB[4] = { 3, 2, 3, 2 };
+    static uint32_t patternC[4] = { 3, 2, 1, 0 };
+    static float32_t   signA[4] = { -1, -1, 1, 1 };
+
+    uint32x4_t vecA = vld1q_u32(patternA);
+    uint32x4_t vecB = vld1q_u32(patternB);
+    uint32x4_t vecC = vld1q_u32(patternC);
+    f32x4_t vecSignA = vld1q_f32(signA);
+
+    while (nbQuaternions > 0U)
+    {
+        f32x4_t vecTmpA, vecTmpB, vecAcc;
+
+        vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecA);
+        vecTmpB = vld1q(qb);
+        /*
+         * vcmul(r, [a1, a2, a1, a2], [b1, b2, b3, b4], 0)
+         */
+        vecAcc = vcmulq(vecTmpA, vecTmpB);
+        /*
+         * vcmla(r, [a1, a2, a1, a2], [b1, b2, b3, b4], 90)
+         */
+        vecAcc = vcmlaq_rot90(vecAcc, vecTmpA, vecTmpB);
+
+        vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecB);
+        vecTmpB = vldrwq_gather_shifted_offset_f32(qb, vecC);
+        /*
+         * build [-b4, -b3, b2, b1]
+         */
+        vecTmpB = vecTmpB * vecSignA;
+        /*
+         * vcmla(r, [a4, a3, a4, a3], [-b4, -b3, b2, b1], 270)
+         */
+        vecAcc = vcmlaq_rot270(vecAcc, vecTmpA, vecTmpB);
+        /*
+         * vcmla(r, [a4, a3, a4, a3], [-b4, -b3, b2, b1], 0)
+         */
+        vecAcc = vcmlaq(vecAcc, vecTmpA, vecTmpB);
+        /*
+         * store accumulator
+         */
+        vst1q_f32(qr, vecAcc);
+
+        /* move to next quaternion */
+        qa += 4;
+        qb += 4;
+        qr += 4;
+
+        nbQuaternions--;
+    }
+}
+
+#else
+
+void arm_quaternion_product_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *qr,
+    uint32_t nbQuaternions)
+{
+   uint32_t i;
+   for(i=0; i < nbQuaternions; i++)
+   {
+     arm_quaternion_product_single_f32(qa, qb, qr);
+
+     qa += 4;
+     qb += 4;
+     qr += 4;
+   }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatProdVect group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_single_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_single_f32.c
new file mode 100644
index 0000000..96f23e9
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_quaternion_product_single_f32.c
@@ -0,0 +1,107 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_quaternion_product_single_f32.c
+ * Description:  Floating-point quaternion product
+ *
+ * $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/quaternion_math_functions.h"
+#include <math.h>
+
+
+/**
+  @ingroup QuatProd
+ */
+
+/**
+  @defgroup QuatProdSingle Quaternion Product
+
+  Compute the  product of two quaternions.
+ */
+
+/**
+  @addtogroup QuatProdSingle
+  @{
+ */
+
+/**
+  @brief         Floating-point product of two quaternions.
+  @param[in]     qa       first quaternion
+  @param[in]     qb       second quaternion
+  @param[out]    qr       product of two quaternions
+  @return        none
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+void arm_quaternion_product_single_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *qr)
+{
+    static uint32_t patternA[4] = { 0, 1, 0, 1 };
+    static uint32_t patternB[4] = { 3, 2, 3, 2 };
+    static uint32_t patternC[4] = { 3, 2, 1, 0 };
+    static float32_t signA[4] = { -1, -1, 1, 1 };
+
+    uint32x4_t vecA = vld1q_u32(patternA);
+    uint32x4_t vecB = vld1q_u32(patternB);
+    uint32x4_t vecC = vld1q_u32(patternC);
+    f32x4_t vecSignA = vld1q_f32(signA);
+
+
+    f32x4_t vecTmpA, vecTmpB, vecAcc;
+
+    vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecA);
+    vecTmpB = vld1q_f32(qb);
+
+    vecAcc = vcmulq_f32(vecTmpA, vecTmpB);
+    vecAcc = vcmlaq_rot90_f32(vecAcc, vecTmpA, vecTmpB);
+
+    vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecB);
+    vecTmpB = vldrwq_gather_shifted_offset_f32(qb, vecC);
+
+    vecTmpB = vecTmpB * vecSignA;
+
+    vecAcc = vcmlaq_rot270_f32(vecAcc, vecTmpA, vecTmpB);
+    vecAcc = vcmlaq_f32(vecAcc, vecTmpA, vecTmpB);
+
+    vst1q_f32(qr, vecAcc);
+}
+
+#else
+void arm_quaternion_product_single_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *qr)
+{
+    qr[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
+    qr[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2];
+    qr[2] = qa[0] * qb[2] + qa[2] * qb[0] + qa[3] * qb[1] - qa[1] * qb[3];
+    qr[3] = qa[0] * qb[3] + qa[3] * qb[0] + qa[1] * qb[2] - qa[2] * qb[1];
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of QuatProdSingle group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_rotation2quaternion_f32.c b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_rotation2quaternion_f32.c
new file mode 100644
index 0000000..5d57492
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/QuaternionMathFunctions/arm_rotation2quaternion_f32.c
@@ -0,0 +1,225 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_rotation2quaternion_f32.c
+ * Description:  Floating-point rotation to quaternion conversion
+ *
+ * $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/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.
+ * 
+ */
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+
+#include "arm_helium_utils.h"
+
+#define R00  vgetq_lane(q1,0)
+#define R01  vgetq_lane(q1,1)
+#define R02  vgetq_lane(q1,2)
+#define R10  vgetq_lane(q1,3)
+#define R11  vgetq_lane(q2,0)
+#define R12  vgetq_lane(q2,1)
+#define R20  vgetq_lane(q2,2)
+#define R21  vgetq_lane(q2,3)
+#define R22  ro22
+
+void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 
+    float32_t *pOutputQuaternions,  
+    uint32_t nbQuaternions)
+{
+   float32_t ro22, trace;
+   f32x4_t q1,q2, q; 
+
+   float32_t doubler;
+   float32_t s;
+
+   q = vdupq_n_f32(0.0f);
+
+   for(uint32_t nb=0; nb < nbQuaternions; nb++)
+   {
+      q1 = vld1q(pInputRotations);
+      pInputRotations += 4;
+
+      q2 = vld1q(pInputRotations);
+      pInputRotations += 4;
+
+      ro22 = *pInputRotations++;
+
+      trace = R00 + R11 + R22;
+
+
+      if (trace > 0)
+      {
+        (void)arm_sqrt_f32(trace + 1.0f, &doubler) ; // invs=4*qw
+        doubler = 2.0f*doubler;
+        s = 1.0f / doubler;
+
+        q1 = vmulq_n_f32(q1,s);
+        q2 = vmulq_n_f32(q2,s);
+
+        q[0] = 0.25f * doubler;
+        q[1] = R21 - R12;
+        q[2] = R02 - R20;
+        q[3] = R10 - R01;
+      }
+      else if ((R00 > R11) && (R00 > R22) )
+      {
+        (void)arm_sqrt_f32(1.0f + R00 - R11 - R22,&doubler); // invs=4*qx
+        doubler = 2.0f*doubler;
+        s = 1.0f / doubler;
+
+        q1 = vmulq_n_f32(q1,s);
+        q2 = vmulq_n_f32(q2,s);
+
+        q[0] = R21 - R12;
+        q[1] = 0.25f * doubler;
+        q[2] = R01 + R10;
+        q[3] = R02 + R20;
+      }
+      else if (R11 > R22)
+      {
+        (void)arm_sqrt_f32(1.0f + R11 - R00 - R22,&doubler); // invs=4*qy
+        doubler = 2.0f*doubler;
+        s = 1.0f / doubler;
+
+        q1 = vmulq_n_f32(q1,s);
+        q2 = vmulq_n_f32(q2,s);
+
+        q[0] = R02 - R20;
+        q[1] = R01 + R10;
+        q[2] = 0.25f * doubler;
+        q[3] = R12 + R21;
+      }
+      else
+      {
+        (void)arm_sqrt_f32(1.0f + R22 - R00 - R11,&doubler); // invs=4*qz
+        doubler = 2.0f*doubler;
+        s = 1.0f / doubler;
+
+        q1 = vmulq_n_f32(q1,s);
+        q2 = vmulq_n_f32(q2,s);
+
+        q[0] = R10 - R01;
+        q[1] = R02 + R20;
+        q[2] = R12 + R21;
+        q[3] = 0.25f * doubler;
+      }
+
+      vst1q(pOutputQuaternions, q);
+      pOutputQuaternions += 4;
+
+   }
+}
+
+#else
+void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 
+    float32_t *pOutputQuaternions,  
+    uint32_t nbQuaternions)
+{
+   uint32_t nb;
+   for(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.0f)
+      {
+        doubler = sqrtf(trace + 1.0f) * 2.0f; // invs=4*qw
+        s = 1.0f / doubler;
+        q[0] = 0.25f * 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.0f + RI(0,0) - RI(1,1) - RI(2,2)) * 2.0f; // invs=4*qx
+        s = 1.0f / doubler;
+        q[0] = (RI(2,1) - RI(1,2)) * s;
+        q[1] = 0.25f * 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.0f + RI(1,1) - RI(0,0) - RI(2,2)) * 2.0f; // invs=4*qy
+        s = 1.0f / doubler;
+        q[0] = (RI(0,2) - RI(2,0)) * s;
+        q[1] = (RI(0,1) + RI(1,0)) * s;
+        q[2] = 0.25f * doubler;
+        q[3] = (RI(1,2) + RI(2,1)) * s;
+      }
+      else
+      {
+        doubler = sqrtf(1.0f + RI(2,2) - RI(0,0) - RI(1,1)) * 2.0f; // invs=4*qz
+        s = 1.0f / 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.25f * doubler;
+      }
+
+    }
+}
+#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
+
+/**
+  @} end of RotQuat group
+ */