You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
62 lines
1.1 KiB
62 lines
1.1 KiB
/* |
|
* norm.c |
|
* |
|
* Code generation for function 'norm' |
|
* |
|
* C source code generated on: Wed Jul 11 08:38:35 2012 |
|
* |
|
*/ |
|
|
|
/* Include files */ |
|
#include "rt_nonfinite.h" |
|
#include "attitudeKalmanfilter.h" |
|
#include "norm.h" |
|
|
|
/* Type Definitions */ |
|
|
|
/* Named Constants */ |
|
|
|
/* Variable Declarations */ |
|
|
|
/* Variable Definitions */ |
|
|
|
/* Function Declarations */ |
|
|
|
/* Function Definitions */ |
|
|
|
/* |
|
* |
|
*/ |
|
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; |
|
for (k = 0; k < 3; k++) { |
|
if (x[k] != 0.0F) { |
|
absxk = 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; |
|
} |
|
} |
|
} |
|
|
|
return scale * sqrtf(y); |
|
} |
|
|
|
/* End of code generation (norm.c) */
|
|
|