GitHub-Mirror/bl_mcu_sdk
2
0
Fork 0
mirror of https://github.com/Fishwaldo/bl_mcu_sdk.git synced 2025-07-23 21:29:17 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

[feat][nmsis] add nmsis component and nn,dsp demo

Browse source
This commit is contained in:
jzlv 2021-09-26 13:38:51 +08:00
parent b2aada479b
commit 5d1126d0f0
989 changed files with 286224 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

35
examples/dsp/QuaternionMathFunctions_part1/quaternion2rotation.c Normal file
View file

@ -0,0 +1,35 @@
#include "ref.h"
void ref_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;
}
}