TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-25 11:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

修复bmi088读取零飘错误,为instask增加初始化四元数以防止零飘发散，修复cancomm顺序

Browse Source
This commit is contained in:
NeoZng
2023-04-13 10:31:06 +08:00
parent 95fca40700
commit 6523bbd6a9
8 changed files with 82 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
modules/algorithm/user_lib.c
View File

@@ -162,4 +162,28 @@ int float_rounding(float raw)
if (decimal > 0.5f)
integer++;
return integer;
}
// 三维向量归一化
float* Norm3d(float* v)
{
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len;
v[1] /= len;
v[2] /= len;
return v;
}
// 三维向量叉乘v1 x v2
void Cross3d(float* v1, float* v2,float* res)
{
res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2];
res[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
// 三维向量点乘
float Dot3d(float* v1, float* v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}