mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 12:53:33 +08:00
Fresh import of the PX4 firmware sources.
This commit is contained in:
px4dev
@@ -0,0 +1,261 @@
|
||||
/*
|
||||
* position_estimator.c
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "position_estimator.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T
|
||||
xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T
|
||||
predict_only, real32_T xapo1[6], real32_T Papo1[36])
|
||||
{
|
||||
real32_T fv0[6];
|
||||
real32_T fv1[6];
|
||||
real32_T I[36];
|
||||
real32_T xapri[6];
|
||||
int32_T i;
|
||||
int32_T r1;
|
||||
static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F,
|
||||
1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
|
||||
0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 0.004F, 1.0F };
|
||||
|
||||
static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F,
|
||||
-0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F };
|
||||
|
||||
int32_T r2;
|
||||
real32_T Papri[36];
|
||||
real32_T maxval;
|
||||
static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
|
||||
1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F,
|
||||
0.0F, 0.0F, 0.0F, 0.0F, 1.0F };
|
||||
|
||||
static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
|
||||
1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 0.0F, 0.0F, 1.0F };
|
||||
|
||||
real32_T K[18];
|
||||
static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
|
||||
0, 0 };
|
||||
|
||||
real32_T fv6[9];
|
||||
static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0 };
|
||||
|
||||
real32_T b_gps_covariance[9];
|
||||
real32_T A[9];
|
||||
real32_T B[18];
|
||||
int32_T r3;
|
||||
real32_T a21;
|
||||
real32_T Y[18];
|
||||
real32_T b_z[3];
|
||||
int8_T b_I[36];
|
||||
|
||||
/* if predit_onli == 1: no update step: use this when no new gps data is available */
|
||||
/* %initialization */
|
||||
/* use model F=m*a x''=F/m */
|
||||
/* 250Hz---> dT = 0.004s */
|
||||
/* u=[phi;theta] */
|
||||
/* x=[px;vx;py;vy]; */
|
||||
/* %------------------------------------------ */
|
||||
/* %------------------------------------------------ */
|
||||
/* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */
|
||||
/* process Covariance Matrix */
|
||||
/* measurement Covariance Matrix */
|
||||
/* %prediction */
|
||||
for (i = 0; i < 6; i++) {
|
||||
fv0[i] = 0.0F;
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
fv0[i] += fv2[i + 6 * r1] * xapo[r1];
|
||||
}
|
||||
|
||||
fv1[i] = 0.0F;
|
||||
for (r1 = 0; r1 < 2; r1++) {
|
||||
fv1[i] += fv3[i + 6 * r1] * u[r1];
|
||||
}
|
||||
|
||||
xapri[i] = fv0[i] + fv1[i];
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
I[i + 6 * r1] = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 6; i++) {
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
maxval = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1];
|
||||
}
|
||||
|
||||
Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1];
|
||||
}
|
||||
}
|
||||
|
||||
if (1 != predict_only) {
|
||||
/* update */
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
K[i + 3 * r1] = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (r1 = 0; r1 < 3; r1++) {
|
||||
fv6[i + 3 * r1] = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b_gps_covariance[0] = gps_covariance[0];
|
||||
b_gps_covariance[1] = 0.0F;
|
||||
b_gps_covariance[2] = 0.0F;
|
||||
b_gps_covariance[3] = 0.0F;
|
||||
b_gps_covariance[4] = gps_covariance[1];
|
||||
b_gps_covariance[5] = 0.0F;
|
||||
b_gps_covariance[6] = 0.0F;
|
||||
b_gps_covariance[7] = 0.0F;
|
||||
b_gps_covariance[8] = gps_covariance[2];
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (r1 = 0; r1 < 3; r1++) {
|
||||
A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i];
|
||||
}
|
||||
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
B[i + 3 * r1] = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
r1 = 0;
|
||||
r2 = 1;
|
||||
r3 = 2;
|
||||
maxval = (real32_T)fabs(A[0]);
|
||||
a21 = (real32_T)fabs(A[1]);
|
||||
if (a21 > maxval) {
|
||||
maxval = a21;
|
||||
r1 = 1;
|
||||
r2 = 0;
|
||||
}
|
||||
|
||||
if ((real32_T)fabs(A[2]) > maxval) {
|
||||
r1 = 2;
|
||||
r2 = 1;
|
||||
r3 = 0;
|
||||
}
|
||||
|
||||
A[r2] /= A[r1];
|
||||
A[r3] /= A[r1];
|
||||
A[3 + r2] -= A[r2] * A[3 + r1];
|
||||
A[3 + r3] -= A[r3] * A[3 + r1];
|
||||
A[6 + r2] -= A[r2] * A[6 + r1];
|
||||
A[6 + r3] -= A[r3] * A[6 + r1];
|
||||
if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) {
|
||||
i = r2;
|
||||
r2 = r3;
|
||||
r3 = i;
|
||||
}
|
||||
|
||||
A[3 + r3] /= A[3 + r2];
|
||||
A[6 + r3] -= A[3 + r3] * A[6 + r2];
|
||||
for (i = 0; i < 6; i++) {
|
||||
Y[3 * i] = B[r1 + 3 * i];
|
||||
Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2];
|
||||
Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 +
|
||||
r3];
|
||||
Y[2 + 3 * i] /= A[6 + r3];
|
||||
Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1];
|
||||
Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2];
|
||||
Y[1 + 3 * i] /= A[3 + r2];
|
||||
Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1];
|
||||
Y[3 * i] /= A[r1];
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
K[r1 + 6 * i] = Y[i + 3 * r1];
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
maxval = 0.0F;
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1];
|
||||
}
|
||||
|
||||
b_z[i] = z[i] - maxval;
|
||||
}
|
||||
|
||||
for (i = 0; i < 6; i++) {
|
||||
maxval = 0.0F;
|
||||
for (r1 = 0; r1 < 3; r1++) {
|
||||
maxval += K[i + 6 * r1] * b_z[r1];
|
||||
}
|
||||
|
||||
xapo1[i] = xapri[i] + maxval;
|
||||
}
|
||||
|
||||
for (i = 0; i < 36; i++) {
|
||||
b_I[i] = 0;
|
||||
}
|
||||
|
||||
for (i = 0; i < 6; i++) {
|
||||
b_I[i + 6 * i] = 1;
|
||||
}
|
||||
|
||||
for (i = 0; i < 6; i++) {
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
maxval = 0.0F;
|
||||
for (r2 = 0; r2 < 3; r2++) {
|
||||
maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1];
|
||||
}
|
||||
|
||||
I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 6; i++) {
|
||||
for (r1 = 0; r1 < 6; r1++) {
|
||||
Papo1[i + 6 * r1] = 0.0F;
|
||||
for (r2 = 0; r2 < 6; r2++) {
|
||||
Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1];
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T));
|
||||
for (i = 0; i < 6; i++) {
|
||||
xapo1[i] = xapri[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (position_estimator.c) */
|
||||
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
* position_estimator.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __POSITION_ESTIMATOR_H__
|
||||
#define __POSITION_ESTIMATOR_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "position_estimator_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]);
|
||||
#endif
|
||||
/* End of code generation (position_estimator.h) */
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* position_estimator_initialize.c
|
||||
*
|
||||
* Code generation for function 'position_estimator_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "position_estimator.h"
|
||||
#include "position_estimator_initialize.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
void position_estimator_initialize(void)
|
||||
{
|
||||
rt_InitInfAndNaN(8U);
|
||||
}
|
||||
|
||||
/* End of code generation (position_estimator_initialize.c) */
|
||||
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
* position_estimator_initialize.h
|
||||
*
|
||||
* Code generation for function 'position_estimator_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __POSITION_ESTIMATOR_INITIALIZE_H__
|
||||
#define __POSITION_ESTIMATOR_INITIALIZE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "position_estimator_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void position_estimator_initialize(void);
|
||||
#endif
|
||||
/* End of code generation (position_estimator_initialize.h) */
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* position_estimator_terminate.c
|
||||
*
|
||||
* Code generation for function 'position_estimator_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "position_estimator.h"
|
||||
#include "position_estimator_terminate.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
void position_estimator_terminate(void)
|
||||
{
|
||||
/* (no terminate code required) */
|
||||
}
|
||||
|
||||
/* End of code generation (position_estimator_terminate.c) */
|
||||
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
* position_estimator_terminate.h
|
||||
*
|
||||
* Code generation for function 'position_estimator_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __POSITION_ESTIMATOR_TERMINATE_H__
|
||||
#define __POSITION_ESTIMATOR_TERMINATE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "position_estimator_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void position_estimator_terminate(void);
|
||||
#endif
|
||||
/* End of code generation (position_estimator_terminate.h) */
|
||||
@@ -0,0 +1,16 @@
|
||||
/*
|
||||
* position_estimator_types.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __POSITION_ESTIMATOR_TYPES_H__
|
||||
#define __POSITION_ESTIMATOR_TYPES_H__
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
#endif
|
||||
/* End of code generation (position_estimator_types.h) */
|
||||
@@ -0,0 +1,139 @@
|
||||
/*
|
||||
* rtGetInf.c
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finite, Inf and MinusInf
|
||||
*/
|
||||
#include "rtGetInf.h"
|
||||
#define NumBitsPerChar 8U
|
||||
|
||||
/* Function: rtGetInf ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtInf needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetInf(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T inf = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
inf = rtGetInfF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
inf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
inf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return inf;
|
||||
}
|
||||
|
||||
/* Function: rtGetInfF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtInfF needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetInfF(void)
|
||||
{
|
||||
IEEESingle infF;
|
||||
infF.wordL.wordLuint = 0x7F800000U;
|
||||
return infF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* Function: rtGetMinusInf ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtMinusInf needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetMinusInf(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T minf = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
minf = rtGetMinusInfF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
minf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
minf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return minf;
|
||||
}
|
||||
|
||||
/* Function: rtGetMinusInfF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtMinusInfF needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetMinusInfF(void)
|
||||
{
|
||||
IEEESingle minfF;
|
||||
minfF.wordL.wordLuint = 0xFF800000U;
|
||||
return minfF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* End of code generation (rtGetInf.c) */
|
||||
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* rtGetInf.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTGETINF_H__
|
||||
#define __RTGETINF_H__
|
||||
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
extern real_T rtGetInf(void);
|
||||
extern real32_T rtGetInfF(void);
|
||||
extern real_T rtGetMinusInf(void);
|
||||
extern real32_T rtGetMinusInfF(void);
|
||||
|
||||
#endif
|
||||
/* End of code generation (rtGetInf.h) */
|
||||
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* rtGetNaN.c
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finite, NaN
|
||||
*/
|
||||
#include "rtGetNaN.h"
|
||||
#define NumBitsPerChar 8U
|
||||
|
||||
/* Function: rtGetNaN ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtNaN needed by the generated code.
|
||||
* NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetNaN(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T nan = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
nan = rtGetNaNF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF80000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
nan = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
|
||||
tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
|
||||
nan = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return nan;
|
||||
}
|
||||
|
||||
/* Function: rtGetNaNF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtNaNF needed by the generated code.
|
||||
* NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetNaNF(void)
|
||||
{
|
||||
IEEESingle nanF = { { 0 } };
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
nanF.wordL.wordLuint = 0xFFC00000U;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
nanF.wordL.wordLuint = 0x7FFFFFFFU;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return nanF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* End of code generation (rtGetNaN.c) */
|
||||
@@ -0,0 +1,21 @@
|
||||
/*
|
||||
* rtGetNaN.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTGETNAN_H__
|
||||
#define __RTGETNAN_H__
|
||||
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
extern real_T rtGetNaN(void);
|
||||
extern real32_T rtGetNaNF(void);
|
||||
|
||||
#endif
|
||||
/* End of code generation (rtGetNaN.h) */
|
||||
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* rt_nonfinite.c
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finites,
|
||||
* (Inf, NaN and -Inf).
|
||||
*/
|
||||
#include "rt_nonfinite.h"
|
||||
#include "rtGetNaN.h"
|
||||
#include "rtGetInf.h"
|
||||
|
||||
real_T rtInf;
|
||||
real_T rtMinusInf;
|
||||
real_T rtNaN;
|
||||
real32_T rtInfF;
|
||||
real32_T rtMinusInfF;
|
||||
real32_T rtNaNF;
|
||||
|
||||
/* Function: rt_InitInfAndNaN ==================================================
|
||||
* Abstract:
|
||||
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
|
||||
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
void rt_InitInfAndNaN(size_t realSize)
|
||||
{
|
||||
(void) (realSize);
|
||||
rtNaN = rtGetNaN();
|
||||
rtNaNF = rtGetNaNF();
|
||||
rtInf = rtGetInf();
|
||||
rtInfF = rtGetInfF();
|
||||
rtMinusInf = rtGetMinusInf();
|
||||
rtMinusInfF = rtGetMinusInfF();
|
||||
}
|
||||
|
||||
/* Function: rtIsInf ==================================================
|
||||
* Abstract:
|
||||
* Test if value is infinite
|
||||
*/
|
||||
boolean_T rtIsInf(real_T value)
|
||||
{
|
||||
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
|
||||
}
|
||||
|
||||
/* Function: rtIsInfF =================================================
|
||||
* Abstract:
|
||||
* Test if single-precision value is infinite
|
||||
*/
|
||||
boolean_T rtIsInfF(real32_T value)
|
||||
{
|
||||
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
|
||||
}
|
||||
|
||||
/* Function: rtIsNaN ==================================================
|
||||
* Abstract:
|
||||
* Test if value is not a number
|
||||
*/
|
||||
boolean_T rtIsNaN(real_T value)
|
||||
{
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
return _isnan(value)? TRUE:FALSE;
|
||||
#else
|
||||
return (value!=value)? 1U:0U;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Function: rtIsNaNF =================================================
|
||||
* Abstract:
|
||||
* Test if single-precision value is not a number
|
||||
*/
|
||||
boolean_T rtIsNaNF(real32_T value)
|
||||
{
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
return _isnan((real_T)value)? true:false;
|
||||
#else
|
||||
return (value!=value)? 1U:0U;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/* End of code generation (rt_nonfinite.c) */
|
||||
@@ -0,0 +1,53 @@
|
||||
/*
|
||||
* rt_nonfinite.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RT_NONFINITE_H__
|
||||
#define __RT_NONFINITE_H__
|
||||
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
#include <float.h>
|
||||
#endif
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
|
||||
extern real_T rtInf;
|
||||
extern real_T rtMinusInf;
|
||||
extern real_T rtNaN;
|
||||
extern real32_T rtInfF;
|
||||
extern real32_T rtMinusInfF;
|
||||
extern real32_T rtNaNF;
|
||||
extern void rt_InitInfAndNaN(size_t realSize);
|
||||
extern boolean_T rtIsInf(real_T value);
|
||||
extern boolean_T rtIsInfF(real32_T value);
|
||||
extern boolean_T rtIsNaN(real_T value);
|
||||
extern boolean_T rtIsNaNF(real32_T value);
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordH;
|
||||
uint32_T wordL;
|
||||
} words;
|
||||
} BigEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordL;
|
||||
uint32_T wordH;
|
||||
} words;
|
||||
} LittleEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
union {
|
||||
real32_T wordLreal;
|
||||
uint32_T wordLuint;
|
||||
} wordL;
|
||||
} IEEESingle;
|
||||
|
||||
#endif
|
||||
/* End of code generation (rt_nonfinite.h) */
|
||||
@@ -0,0 +1,175 @@
|
||||
/*
|
||||
* rtwtypes.h
|
||||
*
|
||||
* Code generation for function 'position_estimator'
|
||||
*
|
||||
* C source code generated on: Fri Jun 8 13:31:21 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTWTYPES_H__
|
||||
#define __RTWTYPES_H__
|
||||
#ifndef TRUE
|
||||
# define TRUE (1U)
|
||||
#endif
|
||||
#ifndef FALSE
|
||||
# define FALSE (0U)
|
||||
#endif
|
||||
#ifndef __TMWTYPES__
|
||||
#define __TMWTYPES__
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
/*=======================================================================*
|
||||
* Target hardware information
|
||||
* Device type: Generic->MATLAB Host Computer
|
||||
* Number of bits: char: 8 short: 16 int: 32
|
||||
* long: 64 native word size: 64
|
||||
* Byte ordering: LittleEndian
|
||||
* Signed integer division rounds to: Zero
|
||||
* Shift right on a signed integer as arithmetic shift: on
|
||||
*=======================================================================*/
|
||||
|
||||
/*=======================================================================*
|
||||
* Fixed width word size data types: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
* real32_T, real64_T - 32 and 64 bit floating point numbers *
|
||||
*=======================================================================*/
|
||||
|
||||
typedef signed char int8_T;
|
||||
typedef unsigned char uint8_T;
|
||||
typedef short int16_T;
|
||||
typedef unsigned short uint16_T;
|
||||
typedef int int32_T;
|
||||
typedef unsigned int uint32_T;
|
||||
typedef long int64_T;
|
||||
typedef unsigned long uint64_T;
|
||||
typedef float real32_T;
|
||||
typedef double real64_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
|
||||
* ulong_T, char_T and byte_T. *
|
||||
*===========================================================================*/
|
||||
|
||||
typedef double real_T;
|
||||
typedef double time_T;
|
||||
typedef unsigned char boolean_T;
|
||||
typedef int int_T;
|
||||
typedef unsigned uint_T;
|
||||
typedef unsigned long ulong_T;
|
||||
typedef char char_T;
|
||||
typedef char_T byte_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Complex number type definitions *
|
||||
*===========================================================================*/
|
||||
#define CREAL_T
|
||||
typedef struct {
|
||||
real32_T re;
|
||||
real32_T im;
|
||||
} creal32_T;
|
||||
|
||||
typedef struct {
|
||||
real64_T re;
|
||||
real64_T im;
|
||||
} creal64_T;
|
||||
|
||||
typedef struct {
|
||||
real_T re;
|
||||
real_T im;
|
||||
} creal_T;
|
||||
|
||||
typedef struct {
|
||||
int8_T re;
|
||||
int8_T im;
|
||||
} cint8_T;
|
||||
|
||||
typedef struct {
|
||||
uint8_T re;
|
||||
uint8_T im;
|
||||
} cuint8_T;
|
||||
|
||||
typedef struct {
|
||||
int16_T re;
|
||||
int16_T im;
|
||||
} cint16_T;
|
||||
|
||||
typedef struct {
|
||||
uint16_T re;
|
||||
uint16_T im;
|
||||
} cuint16_T;
|
||||
|
||||
typedef struct {
|
||||
int32_T re;
|
||||
int32_T im;
|
||||
} cint32_T;
|
||||
|
||||
typedef struct {
|
||||
uint32_T re;
|
||||
uint32_T im;
|
||||
} cuint32_T;
|
||||
|
||||
typedef struct {
|
||||
int64_T re;
|
||||
int64_T im;
|
||||
} cint64_T;
|
||||
|
||||
typedef struct {
|
||||
uint64_T re;
|
||||
uint64_T im;
|
||||
} cuint64_T;
|
||||
|
||||
|
||||
/*=======================================================================*
|
||||
* Min and Max: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
*=======================================================================*/
|
||||
|
||||
#define MAX_int8_T ((int8_T)(127))
|
||||
#define MIN_int8_T ((int8_T)(-128))
|
||||
#define MAX_uint8_T ((uint8_T)(255))
|
||||
#define MIN_uint8_T ((uint8_T)(0))
|
||||
#define MAX_int16_T ((int16_T)(32767))
|
||||
#define MIN_int16_T ((int16_T)(-32768))
|
||||
#define MAX_uint16_T ((uint16_T)(65535))
|
||||
#define MIN_uint16_T ((uint16_T)(0))
|
||||
#define MAX_int32_T ((int32_T)(2147483647))
|
||||
#define MIN_int32_T ((int32_T)(-2147483647-1))
|
||||
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
|
||||
#define MIN_uint32_T ((uint32_T)(0))
|
||||
#define MAX_int64_T ((int64_T)(9223372036854775807L))
|
||||
#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L))
|
||||
#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
|
||||
#define MIN_uint64_T ((uint64_T)(0UL))
|
||||
|
||||
/* Logical type definitions */
|
||||
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
|
||||
# ifndef false
|
||||
# define false (0U)
|
||||
# endif
|
||||
# ifndef true
|
||||
# define true (1U)
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
|
||||
* for signed integer values.
|
||||
*/
|
||||
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
|
||||
#error "This code must be compiled using a 2's complement representation for signed integer values"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Maximum length of a MATLAB identifier (function/variable)
|
||||
* including the null-termination character. Referenced by
|
||||
* rt_logging.c and rt_matrx.c.
|
||||
*/
|
||||
#define TMW_NAME_LENGTH_MAX 64
|
||||
|
||||
#endif
|
||||
#endif
|
||||
/* End of code generation (rtwtypes.h) */
|
||||
Reference in New Issue
Block a user