fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-21 23:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged new EKF version

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-16 11:10:09 +02:00
parent a720bfff5e
commit fef4362e79
41 changed files with 709 additions and 722 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/codegen/norm.c
+11 -19
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -31,32 +31,24 @@ real32_T norm(const real32_T x[3])
{
real32_T y;
real32_T scale;
boolean_T firstNonZero;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
scale = 0.0F;
firstNonZero = TRUE;
scale = 1.17549435E-38F;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
absxk = (real32_T)fabsf(x[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
firstNonZero = FALSE;
} else if (scale < absxk) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
absxk = (real32_T)fabs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * (real32_T)sqrtf(y);
return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */