diff options
Diffstat (limited to 'apps/position_estimator')
19 files changed, 1837 insertions, 0 deletions
diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/position_estimator/.context diff --git a/apps/position_estimator/Makefile b/apps/position_estimator/Makefile new file mode 100644 index 000000000..a766f1666 --- /dev/null +++ b/apps/position_estimator/Makefile @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the position estimator +# + +APPNAME = position_estimator +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_main.c \ + codegen/position_estimator.c \ + codegen/position_estimator_initialize.c \ + codegen/position_estimator_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c new file mode 100644 index 000000000..731ae03e3 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator.c @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator.h b/apps/position_estimator/codegen/position_estimator.h new file mode 100644 index 000000000..5e2c9acd8 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator.h @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.c b/apps/position_estimator/codegen/position_estimator_initialize.c new file mode 100644 index 000000000..862381d3b --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_initialize.c @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.h b/apps/position_estimator/codegen/position_estimator_initialize.h new file mode 100644 index 000000000..71013b390 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_initialize.h @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.c b/apps/position_estimator/codegen/position_estimator_terminate.c new file mode 100644 index 000000000..b25aabf40 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_terminate.c @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.h b/apps/position_estimator/codegen/position_estimator_terminate.h new file mode 100644 index 000000000..d9fcf838b --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_terminate.h @@ -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) */ diff --git a/apps/position_estimator/codegen/position_estimator_types.h b/apps/position_estimator/codegen/position_estimator_types.h new file mode 100644 index 000000000..cb01c7426 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_types.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) */ diff --git a/apps/position_estimator/codegen/rtGetInf.c b/apps/position_estimator/codegen/rtGetInf.c new file mode 100644 index 000000000..20a64117c --- /dev/null +++ b/apps/position_estimator/codegen/rtGetInf.c @@ -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) */ diff --git a/apps/position_estimator/codegen/rtGetInf.h b/apps/position_estimator/codegen/rtGetInf.h new file mode 100644 index 000000000..c74f1fc28 --- /dev/null +++ b/apps/position_estimator/codegen/rtGetInf.h @@ -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) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.c b/apps/position_estimator/codegen/rtGetNaN.c new file mode 100644 index 000000000..6aa2f4639 --- /dev/null +++ b/apps/position_estimator/codegen/rtGetNaN.c @@ -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) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.h b/apps/position_estimator/codegen/rtGetNaN.h new file mode 100644 index 000000000..d3ec61c9e --- /dev/null +++ b/apps/position_estimator/codegen/rtGetNaN.h @@ -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) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.c b/apps/position_estimator/codegen/rt_nonfinite.c new file mode 100644 index 000000000..40e15fd22 --- /dev/null +++ b/apps/position_estimator/codegen/rt_nonfinite.c @@ -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) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.h b/apps/position_estimator/codegen/rt_nonfinite.h new file mode 100644 index 000000000..ac9660124 --- /dev/null +++ b/apps/position_estimator/codegen/rt_nonfinite.h @@ -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) */ diff --git a/apps/position_estimator/codegen/rtwtypes.h b/apps/position_estimator/codegen/rtwtypes.h new file mode 100644 index 000000000..e87ae1fdc --- /dev/null +++ b/apps/position_estimator/codegen/rtwtypes.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) */ diff --git a/apps/position_estimator/position_estimator.m b/apps/position_estimator/position_estimator.m new file mode 100644 index 000000000..2ef4d8b06 --- /dev/null +++ b/apps/position_estimator/position_estimator.m @@ -0,0 +1,62 @@ +function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available +%#codegen +%%initialization +%use model F=m*a x''=F/m +% 250Hz---> dT = 0.004s +%u=[phi;theta] +%x=[px;vx;py;vy]; +%%------------------------------------------ +dT=0.004; +%%------------------------------------------------ + +%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; + + +F=[ 1, 0.004, 0, 0, 0, 0; + 0, 1, 0, 0, 0, 0; + 0, 0, 1, 0.004, 0, 0; + 0, 0, 0, 1, 0, 0; + 0, 0, 0, 0, 1, 0.004; + 0, 0, 0, 0, 0, 1]; + +B=[ 0, -0.1744; + 0, -87.2; + 0.1744, 0; + 87.2, 0; + 0, 0; + 0, 0]; + +H=[1,0,0,0,0,0; + 0,0,1,0,0,0; + 0,0,0,0,1,0]; + + + + Q=[1e-007 ,0 ,0 ,0 ,0 ,0; + 0 ,1 ,0 ,0 ,0 ,0; + 0 ,0 ,1e-007 ,0 ,0 ,0; + 0 ,0 ,0 ,1 ,0 ,0 + 0 ,0 ,0 ,0 ,1e-007 ,0; + 0 ,0 ,0 ,0 ,0 ,1]; %process Covariance Matrix + + +R=[gps_covariance(1), 0, 0; + 0, gps_covariance(2), 0; + 0, 0, gps_covariance(3)]; %measurement Covariance Matrix + +%%prediction + +xapri=F*xapo+B*u; +Papri=F*Papo*F'+Q; + +if 1 ~= predict_only + %update + yr=z-H*xapri; + S=H*Papri*H'+R; + K=(Papri*H')/S; + xapo1=xapri+K*yr; + Papo1=(eye(6)-K*H)*Papri; +else + Papo1=Papri; + xapo1=xapri; +end
\ No newline at end of file diff --git a/apps/position_estimator/position_estimator.prj b/apps/position_estimator/position_estimator.prj new file mode 100644 index 000000000..d754f36cc --- /dev/null +++ b/apps/position_estimator/position_estimator.prj @@ -0,0 +1,269 @@ +<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project>
+ <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="position_estimator" location="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator" file="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator/position_estimator.prj" build-checksum="2425236060">
+ <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
+ <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
+ <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
+ <param.mex.general.EnableBLAS>false</param.mex.general.EnableBLAS>
+ <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
+ <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
+ <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
+ <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
+ <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
+ <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
+ <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
+ <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
+ <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
+ <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
+ <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
+ <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomSourceCode />
+ <param.mex.customcode.CustomHeaderCode />
+ <param.mex.customcode.CustomInitializer />
+ <param.mex.customcode.CustomTerminator />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
+ <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
+ <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
+ <param.mex.InlineThreshold>10</param.mex.InlineThreshold>
+ <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
+ <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
+ <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
+ <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
+ <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
+ <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
+ <param.grt.CCompilerOptimization>option.CCompilerOptimization.Off</param.grt.CCompilerOptimization>
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.GenerateMakefile>false</param.grt.general.GenerateMakefile>
+ <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
+ <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
+ <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
+ <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
+ <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
+ <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
+ <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
+ <param.grt.paths.working.specified />
+ <param.grt.paths.build>option.paths.build.specified</param.grt.paths.build>
+ <param.grt.paths.build.specified>./codegen</param.grt.paths.build.specified>
+ <param.grt.paths.search />
+ <param.grt.report.GenerateReport>false</param.grt.report.GenerateReport>
+ <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
+ <param.grt.GenerateComments>true</param.grt.GenerateComments>
+ <param.grt.MATLABSourceComments>false</param.grt.MATLABSourceComments>
+ <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
+ <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
+ <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
+ <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
+ <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
+ <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
+ <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
+ <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
+ <param.grt.MaxIdLength>32</param.grt.MaxIdLength>
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomSourceCode />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose>false</param.grt.Verbose>
+ <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
+ <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
+ <param.ert.TargetFunctionLibrary>C89/C90 (ANSI)</param.ert.TargetFunctionLibrary>
+ <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
+ <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
+ <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
+ <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
+ <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
+ <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
+ <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
+ <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
+ <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
+ <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
+ <param.grt.InlineThreshold>10</param.grt.InlineThreshold>
+ <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
+ <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
+ <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
+ <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
+ <param.UseECoderFeatures>true</param.UseECoderFeatures>
+ <param.mex.outputfile>position_estimator_mex</param.mex.outputfile>
+ <param.grt.outputfile>position_estimator</param.grt.outputfile>
+ <param.artifact>option.target.artifact.lib</param.artifact>
+ <param.mex.GenCodeOnly>true</param.mex.GenCodeOnly>
+ <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+ <param.version>R2011a</param.version>
+ <param.HasECoderFeatures>true</param.HasECoderFeatures>
+ <param.mex.mainhtml>/home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html</param.mex.mainhtml>
+ <param.grt.mainhtml />
+ <unset>
+ <param.mex.general.TargetLang />
+ <param.mex.general.IntegrityChecks />
+ <param.mex.general.ResponsivenessChecks />
+ <param.mex.general.ExtrinsicCalls />
+ <param.mex.general.EchoExpressions />
+ <param.mex.general.EnableDebugging />
+ <param.mex.general.SaturateOnIntegerOverflow />
+ <param.mex.general.FilePartitionMethod />
+ <param.mex.general.GlobalDataSyncMethod />
+ <param.mex.general.EnableVariableSizing />
+ <param.mex.general.DynamicMemoryAllocation />
+ <param.mex.paths.working />
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build />
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport />
+ <param.mex.report.LaunchReport />
+ <param.mex.comments.GenerateComments />
+ <param.mex.comments.MATLABSourceComments />
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy />
+ <param.mex.MemcpyThreshold />
+ <param.mex.InitFltsAndDblsToZero />
+ <param.mex.InlineThreshold />
+ <param.mex.InlineThresholdMax />
+ <param.mex.InlineStackLimit />
+ <param.mex.StackUsageMax />
+ <param.mex.ConstantFoldingTimeout />
+ <param.grt.general.TargetLang />
+ <param.grt.CCompilerOptimization />
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.MakeCommand />
+ <param.grt.general.TemplateMakefile />
+ <param.grt.general.SaturateOnIntegerOverflow />
+ <param.grt.general.FilePartitionMethod />
+ <param.grt.general.EnableVariableSizing />
+ <param.grt.general.DynamicMemoryAllocation />
+ <param.grt.paths.working />
+ <param.grt.paths.working.specified />
+ <param.grt.paths.search />
+ <param.grt.report.LaunchReport />
+ <param.grt.GenerateComments />
+ <param.grt.MATLABSourceComments />
+ <param.ert.MATLABFcnDesc />
+ <param.ert.CustomSymbolStrGlobalVar />
+ <param.ert.CustomSymbolStrType />
+ <param.ert.CustomSymbolStrField />
+ <param.ert.CustomSymbolStrFcn />
+ <param.ert.CustomSymbolStrTmpVar />
+ <param.ert.CustomSymbolStrMacro />
+ <param.ert.CustomSymbolStrEMXArray />
+ <param.grt.MaxIdLength />
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose />
+ <param.grt.SupportNonFinite />
+ <param.ert.PurelyIntegerCode />
+ <param.ert.SupportNonFinite />
+ <param.ert.IncludeTerminateFcn />
+ <param.ert.MultiInstanceCode />
+ <param.ert.ParenthesesLevel />
+ <param.ert.ConvertIfToSwitch />
+ <param.ert.PreserveExternInFcnDecls />
+ <param.grt.EnableMemcpy />
+ <param.grt.MemcpyThreshold />
+ <param.grt.InitFltsAndDblsToZero />
+ <param.grt.InlineThreshold />
+ <param.grt.InlineThresholdMax />
+ <param.grt.InlineStackLimit />
+ <param.grt.StackUsageMax />
+ <param.grt.ConstantFoldingTimeout />
+ <param.UseECoderFeatures />
+ <param.mex.outputfile />
+ <param.grt.outputfile />
+ <param.version />
+ <param.HasECoderFeatures />
+ </unset>
+ <fileset.entrypoints>
+ <file value="${PROJECT_ROOT}/position_estimator.m" custom-data-expanded="true">
+ <Inputs fileName="position_estimator.m" functionName="position_estimator">
+ <Input Name="u">
+ <Class>single</Class>
+ <Size>2 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="z">
+ <Class>single</Class>
+ <Size>3 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="xapo">
+ <Class>single</Class>
+ <Size>6 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="Papo">
+ <Class>single</Class>
+ <Size>6 x 6</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="gps_covariance">
+ <Class>single</Class>
+ <Size>3 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="predict_only">
+ <Class>uint8</Class>
+ <Size>1 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ </Inputs>
+ </file>
+ </fileset.entrypoints>
+ <fileset.package />
+ <build-deliverables />
+ <workflow />
+ <matlab>
+ <root>/home/thomasgubler/tools/matlab</root>
+ </matlab>
+ <platform>
+ <unix>true</unix>
+ <mac>false</mac>
+ <windows>false</windows>
+ <win2k>false</win2k>
+ <winxp>false</winxp>
+ <vista>false</vista>
+ <linux>true</linux>
+ <solaris>false</solaris>
+ <osver>3.2.0-25-generic</osver>
+ <os32>false</os32>
+ <os64>true</os64>
+ <arch>glnxa64</arch>
+ <matlab>true</matlab>
+ </platform>
+ </configuration>
+</deployment-project>
+
diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c new file mode 100644 index 000000000..773cd87ff --- /dev/null +++ b/apps/position_estimator/position_estimator_main.c @@ -0,0 +1,425 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Model-identification based position estimator for multirotors + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <v1.0/common/mavlink.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <poll.h> + +#include "codegen/position_estimator.h" + +#define N_STATES 6 +#define ERROR_COVARIANCE_INIT 3 +#define R_EARTH 6371000.0 + +#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 +#define REPROJECTION_COUNTER_LIMIT 125 + +__EXPORT int position_estimator_main(int argc, char *argv[]); + +static uint16_t position_estimator_counter_position_information; + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + +/**************************************************************************** + * main + ****************************************************************************/ + +int position_estimator_main(int argc, char *argv[]) +{ + + /* welcome user */ + printf("[multirotor position_estimator] started\n"); + + /* initialize values */ + static float u[2] = {0, 0}; + static float z[3] = {0, 0, 0}; + static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; + static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 + }; + + static float xapo1[N_STATES]; + static float Papo1[36]; + + static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; + + static uint16_t counter = 0; + position_estimator_counter_position_information = 0; + + uint8_t predict_only = 1; + + bool gps_valid = false; + + bool new_initialization = true; + + static double lat_current = 0;//[°]] --> 47.0 + static double lon_current = 0; //[°]] -->8.5 + + + //TODO: handle flight without gps but with estimator + + /* subscribe to vehicle status, attitude, gps */ + struct vehicle_gps_position_s gps; + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + + int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude at 100 Hz */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* wait until gps signal turns valid, only then can we initialize the projection */ + while (!gps_valid) { + struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds, 1, 5000)) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + /* Read wether the vehicle status changed */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + gps_valid = vstatus.gps_valid; + } + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; + + /* publish global position messages only after first GPS message */ + struct vehicle_global_position_s global_pos = { + .lat = lat_current * 1e7, + .lon = lon_current * 1e7, + .alt = gps.alt + }; + int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + + while (1) { + + /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ + struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; + + if (poll(fds, 1, 5000) <= 0) { + /* error / timeout */ + } else { + + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* got attitude, updating pos as well */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /*copy attitude */ + u[0] = att.roll; + u[1] = att.pitch; + + /* initialize map projection with the last estimate (not at full rate) */ + if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) { + map_projection_init(lat_current, lon_current); + new_initialization = true; + + } else { + new_initialization = false; + } + + /*check if new gps values are available */ + gps_valid = vstatus.gps_valid; + + + if (gps_valid) { //we are safe to use the gps signal (it has good quality) + + predict_only = 0; + /* Project gps lat lon (Geographic coordinate system) to plane*/ + map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1])); + + /* copy altitude */ + z[2] = (gps.alt) * 1e-3; + + gps_covariance[0] = gps.eph; //TODO: needs scaling + gps_covariance[1] = gps.eph; + gps_covariance[2] = gps.epv; + + } else { + /* we can not use the gps signal (it is of low quality) */ + predict_only = 1; + } + + // predict_only = 0; //TODO: only for testing, removeme, XXX + // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX + // usleep(100000); //TODO: only for testing, removeme, XXX + + + /*Get new estimation (this is calculated in the plane) */ + //TODO: if new_initialization == true: use 0,0,0, else use xapo + if (true == new_initialization) { //TODO,XXX: uncomment! + xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane + xapo[2] = 0; + xapo[4] = 0; + position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + + } else { + position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + } + + + + /* Copy values from xapo1 to xapo */ + int i; + + for (i = 0; i < N_STATES; i++) { + xapo[i] = xapo1[i]; + } + + if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { + /* Reproject from plane to geographic coordinate system */ + // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! + map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme + // //DEBUG + // if(counter%500 == 0) + // { + // printf("phi_1: %.10f\n", phi_1); + // printf("lambda_0: %.10f\n", lambda_0); + // printf("lat_estimated: %.10f\n", lat_current); + // printf("lon_estimated: %.10f\n", lon_current); + // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); + // fflush(stdout); + // + // } + + // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) + // { + /* send out */ + + global_pos.lat = lat_current; + global_pos.lon = lon_current; + global_pos.alt = xapo1[4]; + global_pos.vx = xapo1[1]; + global_pos.vy = xapo1[3]; + global_pos.vz = xapo1[5]; + + /* publish current estimate */ + orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); + // } + // else + // { + // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); + // fflush(stdout); + // } + + } + + counter++; + } + + } + + return 0; +} + + |