From 482cada59ba87bc1ac538f7f165a57880e7fbeda Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Mar 2013 22:39:54 +0100 Subject: Butchered position estimator from Damian Aregger into shape, publishes now global position estimate as well. Compiling, needs HIL testing --- apps/position_estimator_mc/Makefile | 64 +++ apps/position_estimator_mc/codegen/kalman_dlqe1.c | 58 +++ apps/position_estimator_mc/codegen/kalman_dlqe1.h | 30 ++ .../codegen/kalman_dlqe1_initialize.c | 31 ++ .../codegen/kalman_dlqe1_initialize.h | 30 ++ .../codegen/kalman_dlqe1_terminate.c | 31 ++ .../codegen/kalman_dlqe1_terminate.h | 30 ++ .../codegen/kalman_dlqe1_types.h | 16 + apps/position_estimator_mc/codegen/kalman_dlqe2.c | 119 +++++ apps/position_estimator_mc/codegen/kalman_dlqe2.h | 32 ++ .../codegen/kalman_dlqe2_initialize.c | 31 ++ .../codegen/kalman_dlqe2_initialize.h | 32 ++ .../codegen/kalman_dlqe2_terminate.c | 31 ++ .../codegen/kalman_dlqe2_terminate.h | 32 ++ .../codegen/kalman_dlqe2_types.h | 16 + apps/position_estimator_mc/codegen/kalman_dlqe3.c | 137 ++++++ apps/position_estimator_mc/codegen/kalman_dlqe3.h | 33 ++ .../codegen/kalman_dlqe3_data.c | 32 ++ .../codegen/kalman_dlqe3_data.h | 38 ++ .../codegen/kalman_dlqe3_initialize.c | 47 ++ .../codegen/kalman_dlqe3_initialize.h | 33 ++ .../codegen/kalman_dlqe3_terminate.c | 31 ++ .../codegen/kalman_dlqe3_terminate.h | 33 ++ .../codegen/kalman_dlqe3_types.h | 16 + .../codegen/positionKalmanFilter1D.c | 136 ++++++ .../codegen/positionKalmanFilter1D.h | 31 ++ .../codegen/positionKalmanFilter1D_dT.c | 157 ++++++ .../codegen/positionKalmanFilter1D_dT.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_initialize.c | 31 ++ .../codegen/positionKalmanFilter1D_dT_initialize.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_terminate.c | 31 ++ .../codegen/positionKalmanFilter1D_dT_terminate.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_types.h | 16 + .../codegen/positionKalmanFilter1D_initialize.c | 31 ++ .../codegen/positionKalmanFilter1D_initialize.h | 31 ++ .../codegen/positionKalmanFilter1D_terminate.c | 31 ++ .../codegen/positionKalmanFilter1D_terminate.h | 31 ++ .../codegen/positionKalmanFilter1D_types.h | 16 + apps/position_estimator_mc/codegen/randn.c | 524 +++++++++++++++++++++ apps/position_estimator_mc/codegen/randn.h | 33 ++ apps/position_estimator_mc/codegen/rtGetInf.c | 139 ++++++ apps/position_estimator_mc/codegen/rtGetInf.h | 23 + apps/position_estimator_mc/codegen/rtGetNaN.c | 96 ++++ apps/position_estimator_mc/codegen/rtGetNaN.h | 21 + apps/position_estimator_mc/codegen/rt_nonfinite.c | 87 ++++ apps/position_estimator_mc/codegen/rt_nonfinite.h | 53 +++ apps/position_estimator_mc/codegen/rtwtypes.h | 159 +++++++ apps/position_estimator_mc/kalman_dlqe1.m | 3 + apps/position_estimator_mc/kalman_dlqe2.m | 9 + apps/position_estimator_mc/kalman_dlqe3.m | 17 + .../position_estimator_mc/positionKalmanFilter1D.m | 19 + .../positionKalmanFilter1D_dT.m | 26 + .../position_estimator_mc_main.c | 510 ++++++++++++++++++++ .../position_estimator_mc_params.c | 68 +++ .../position_estimator_mc_params.h | 69 +++ nuttx/configs/px4fmu/nsh/appconfig | 2 +- 56 files changed, 3475 insertions(+), 1 deletion(-) create mode 100644 apps/position_estimator_mc/Makefile create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe1_types.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe2_types.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_data.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_data.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h create mode 100755 apps/position_estimator_mc/codegen/kalman_dlqe3_types.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h create mode 100755 apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h create mode 100755 apps/position_estimator_mc/codegen/randn.c create mode 100755 apps/position_estimator_mc/codegen/randn.h create mode 100755 apps/position_estimator_mc/codegen/rtGetInf.c create mode 100755 apps/position_estimator_mc/codegen/rtGetInf.h create mode 100755 apps/position_estimator_mc/codegen/rtGetNaN.c create mode 100755 apps/position_estimator_mc/codegen/rtGetNaN.h create mode 100755 apps/position_estimator_mc/codegen/rt_nonfinite.c create mode 100755 apps/position_estimator_mc/codegen/rt_nonfinite.h create mode 100755 apps/position_estimator_mc/codegen/rtwtypes.h create mode 100755 apps/position_estimator_mc/kalman_dlqe1.m create mode 100755 apps/position_estimator_mc/kalman_dlqe2.m create mode 100755 apps/position_estimator_mc/kalman_dlqe3.m create mode 100755 apps/position_estimator_mc/positionKalmanFilter1D.m create mode 100755 apps/position_estimator_mc/positionKalmanFilter1D_dT.m create mode 100755 apps/position_estimator_mc/position_estimator_mc_main.c create mode 100755 apps/position_estimator_mc/position_estimator_mc_params.c create mode 100755 apps/position_estimator_mc/position_estimator_mc_params.h diff --git a/apps/position_estimator_mc/Makefile b/apps/position_estimator_mc/Makefile new file mode 100644 index 000000000..8f1100158 --- /dev/null +++ b/apps/position_estimator_mc/Makefile @@ -0,0 +1,64 @@ +############################################################################ +# +# 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_mc +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_mc_main.c \ + position_estimator_mc_params.c \ + codegen/positionKalmanFilter1D_initialize.c \ + codegen/positionKalmanFilter1D_terminate.c \ + codegen/positionKalmanFilter1D.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/positionKalmanFilter1D_dT_initialize.c \ + codegen/positionKalmanFilter1D_dT_terminate.c \ + codegen/kalman_dlqe1.c \ + codegen/kalman_dlqe1_initialize.c \ + codegen/kalman_dlqe1_terminate.c \ + codegen/kalman_dlqe2.c \ + codegen/kalman_dlqe2_initialize.c \ + codegen/kalman_dlqe2_terminate.c \ + codegen/kalman_dlqe3.c \ + codegen/kalman_dlqe3_initialize.c \ + codegen/kalman_dlqe3_terminate.c \ + codegen/kalman_dlqe3_data.c \ + codegen/randn.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1.c b/apps/position_estimator_mc/codegen/kalman_dlqe1.c new file mode 100755 index 000000000..977565b8e --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1.c @@ -0,0 +1,58 @@ +/* + * kalman_dlqe1.c + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], + const real32_T x_aposteriori_k[3], real32_T z, real32_T + x_aposteriori[3]) +{ + printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z); + printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2])); + real32_T y; + int32_T i0; + real32_T b_y[3]; + int32_T i1; + real32_T f0; + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + b_y[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_y[i0] += C[i1] * A[i1 + 3 * i0]; + } + + y += b_y[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + K[i0] * y; + } + //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2])); +} + +/* End of code generation (kalman_dlqe1.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1.h b/apps/position_estimator_mc/codegen/kalman_dlqe1.h new file mode 100755 index 000000000..2f5330563 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1.h + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +#ifndef __KALMAN_DLQE1_H__ +#define __KALMAN_DLQE1_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe1.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c new file mode 100755 index 000000000..6627f76e9 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe1_initialize.c + * + * Code generation for function 'kalman_dlqe1_initialize' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" +#include "kalman_dlqe1_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (kalman_dlqe1_initialize.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h new file mode 100755 index 000000000..a77eb5712 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1_initialize.h + * + * Code generation for function 'kalman_dlqe1_initialize' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +#ifndef __KALMAN_DLQE1_INITIALIZE_H__ +#define __KALMAN_DLQE1_INITIALIZE_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1_initialize(void); +#endif +/* End of code generation (kalman_dlqe1_initialize.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c new file mode 100755 index 000000000..a65536f79 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe1_terminate.c + * + * Code generation for function 'kalman_dlqe1_terminate' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" +#include "kalman_dlqe1_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe1_terminate.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h new file mode 100755 index 000000000..100c7f76c --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1_terminate.h + * + * Code generation for function 'kalman_dlqe1_terminate' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +#ifndef __KALMAN_DLQE1_TERMINATE_H__ +#define __KALMAN_DLQE1_TERMINATE_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1_terminate(void); +#endif +/* End of code generation (kalman_dlqe1_terminate.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h new file mode 100755 index 000000000..d4b2c2d61 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe1_types.h + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +#ifndef __KALMAN_DLQE1_TYPES_H__ +#define __KALMAN_DLQE1_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe1_types.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2.c b/apps/position_estimator_mc/codegen/kalman_dlqe2.c new file mode 100755 index 000000000..11b999064 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2.c @@ -0,0 +1,119 @@ +/* + * kalman_dlqe2.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1) +{ + real32_T y; + real32_T f1; + real32_T f2; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); + if (rtIsInfF(u1)) { + if (f1 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f1 > 1.0F) { + if (u1 > 0.0F) { + y = ((real32_T)rtInf); + } else { + y = 0.0F; + } + } else if (u1 > 0.0F) { + y = 0.0F; + } else { + y = ((real32_T)rtInf); + } + } else if (f2 == 0.0F) { + y = 1.0F; + } else if (f2 == 1.0F) { + if (u1 > 0.0F) { + y = u0; + } else { + y = 1.0F / u0; + } + } else if (u1 == 2.0F) { + y = u0 * u0; + } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)pow(u0, u1); + } + } + + return y; +} + +void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const + real32_T x_aposteriori_k[3], real32_T z, real32_T + x_aposteriori[3]) +{ + //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3)); + //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3)); + real32_T A[9]; + real32_T y; + int32_T i0; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real32_T b_k1[3]; + int32_T i1; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T f0; + A[0] = 1.0F; + A[3] = dt; + A[6] = 0.5F * rt_powf_snf(dt, 2.0F); + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = dt; + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + A[2 + 3 * i0] = (real32_T)iv0[i0]; + b_k1[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; + } + + y += b_k1[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + b_k1[0] = k1; + b_k1[1] = k2; + b_k1[2] = k3; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + b_k1[i0] * y; + } +} + +/* End of code generation (kalman_dlqe2.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2.h b/apps/position_estimator_mc/codegen/kalman_dlqe2.h new file mode 100755 index 000000000..30170ae22 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __KALMAN_DLQE2_H__ +#define __KALMAN_DLQE2_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe2.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c new file mode 100755 index 000000000..de5a1d8aa --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe2_initialize.c + * + * Code generation for function 'kalman_dlqe2_initialize' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" +#include "kalman_dlqe2_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe2_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (kalman_dlqe2_initialize.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h new file mode 100755 index 000000000..3d507ff31 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2_initialize.h + * + * Code generation for function 'kalman_dlqe2_initialize' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_INITIALIZE_H__ +#define __KALMAN_DLQE2_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2_initialize(void); +#endif +/* End of code generation (kalman_dlqe2_initialize.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c new file mode 100755 index 000000000..0757c878c --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe2_terminate.c + * + * Code generation for function 'kalman_dlqe2_terminate' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" +#include "kalman_dlqe2_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe2_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe2_terminate.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h new file mode 100755 index 000000000..23995020b --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2_terminate.h + * + * Code generation for function 'kalman_dlqe2_terminate' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_TERMINATE_H__ +#define __KALMAN_DLQE2_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2_terminate(void); +#endif +/* End of code generation (kalman_dlqe2_terminate.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h new file mode 100755 index 000000000..f7a04d908 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe2_types.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_TYPES_H__ +#define __KALMAN_DLQE2_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe2_types.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3.c b/apps/position_estimator_mc/codegen/kalman_dlqe3.c new file mode 100755 index 000000000..9efe2ea7a --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3.c @@ -0,0 +1,137 @@ +/* + * kalman_dlqe3.c + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "randn.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1) +{ + real32_T y; + real32_T f1; + real32_T f2; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); + if (rtIsInfF(u1)) { + if (f1 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f1 > 1.0F) { + if (u1 > 0.0F) { + y = ((real32_T)rtInf); + } else { + y = 0.0F; + } + } else if (u1 > 0.0F) { + y = 0.0F; + } else { + y = ((real32_T)rtInf); + } + } else if (f2 == 0.0F) { + y = 1.0F; + } else if (f2 == 1.0F) { + if (u1 > 0.0F) { + y = u0; + } else { + y = 1.0F / u0; + } + } else if (u1 == 2.0F) { + y = u0 * u0; + } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)pow(u0, u1); + } + } + + return y; +} + +void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const + real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, + real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]) +{ + real32_T A[9]; + int32_T i0; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real_T b; + real32_T y; + real32_T b_y[3]; + int32_T i1; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T b_k1[3]; + real32_T f0; + A[0] = 1.0F; + A[3] = dt; + A[6] = 0.5F * rt_powf_snf(dt, 2.0F); + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = dt; + for (i0 = 0; i0 < 3; i0++) { + A[2 + 3 * i0] = (real32_T)iv0[i0]; + } + + if (addNoise == 1.0F) { + b = randn(); + z += sigma * (real32_T)b; + } + + if (posUpdate != 0.0F) { + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + b_y[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; + } + + y += b_y[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + b_k1[0] = k1; + b_k1[1] = k2; + b_k1[2] = k3; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + b_k1[i0] * y; + } + } else { + for (i0 = 0; i0 < 3; i0++) { + x_aposteriori[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + } + } +} + +/* End of code generation (kalman_dlqe3.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3.h b/apps/position_estimator_mc/codegen/kalman_dlqe3.h new file mode 100755 index 000000000..9bbffbbb3 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3.h + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +#ifndef __KALMAN_DLQE3_H__ +#define __KALMAN_DLQE3_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe3.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c new file mode 100755 index 000000000..8f2275c13 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c @@ -0,0 +1,32 @@ +/* + * kalman_dlqe3_data.c + * + * Code generation for function 'kalman_dlqe3_data' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ +uint32_T method; +uint32_T state[2]; +uint32_T b_method; +uint32_T b_state; +uint32_T c_state[2]; +boolean_T state_not_empty; + +/* Function Declarations */ + +/* Function Definitions */ +/* End of code generation (kalman_dlqe3_data.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h new file mode 100755 index 000000000..952eb7b89 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h @@ -0,0 +1,38 @@ +/* + * kalman_dlqe3_data.h + * + * Code generation for function 'kalman_dlqe3_data' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_DATA_H__ +#define __KALMAN_DLQE3_DATA_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ +extern uint32_T method; +extern uint32_T state[2]; +extern uint32_T b_method; +extern uint32_T b_state; +extern uint32_T c_state[2]; +extern boolean_T state_not_empty; + +/* Variable Definitions */ + +/* Function Declarations */ +#endif +/* End of code generation (kalman_dlqe3_data.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c new file mode 100755 index 000000000..b87d604c4 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c @@ -0,0 +1,47 @@ +/* + * kalman_dlqe3_initialize.c + * + * Code generation for function 'kalman_dlqe3_initialize' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_initialize.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe3_initialize(void) +{ + int32_T i; + static const uint32_T uv0[2] = { 362436069U, 0U }; + + rt_InitInfAndNaN(8U); + state_not_empty = FALSE; + b_state = 1144108930U; + b_method = 7U; + method = 0U; + for (i = 0; i < 2; i++) { + c_state[i] = 362436069U + 158852560U * (uint32_T)i; + state[i] = uv0[i]; + } + + if (state[1] == 0U) { + state[1] = 521288629U; + } +} + +/* End of code generation (kalman_dlqe3_initialize.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h new file mode 100755 index 000000000..9dee90f9e --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3_initialize.h + * + * Code generation for function 'kalman_dlqe3_initialize' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_INITIALIZE_H__ +#define __KALMAN_DLQE3_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3_initialize(void); +#endif +/* End of code generation (kalman_dlqe3_initialize.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c new file mode 100755 index 000000000..b00858205 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe3_terminate.c + * + * Code generation for function 'kalman_dlqe3_terminate' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe3_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe3_terminate.c) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h new file mode 100755 index 000000000..69cc85c76 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3_terminate.h + * + * Code generation for function 'kalman_dlqe3_terminate' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_TERMINATE_H__ +#define __KALMAN_DLQE3_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3_terminate(void); +#endif +/* End of code generation (kalman_dlqe3_terminate.h) */ diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h new file mode 100755 index 000000000..f36ea4557 --- /dev/null +++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe3_types.h + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:30 2013 + * + */ + +#ifndef __KALMAN_DLQE3_TYPES_H__ +#define __KALMAN_DLQE3_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe3_types.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c new file mode 100755 index 000000000..5139848bc --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c @@ -0,0 +1,136 @@ +/* + * positionKalmanFilter1D.c + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const + real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T + P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T + Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], + real32_T P_aposteriori[9]) +{ + int32_T i0; + real32_T f0; + int32_T k; + real32_T b_A[9]; + int32_T i1; + real32_T P_apriori[9]; + real32_T y; + real32_T K[3]; + real32_T S; + int8_T I[9]; + + /* prediction */ + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (k = 0; k < 3; k++) { + f0 += A[i0 + 3 * k] * x_aposteriori_k[k]; + } + + x_aposteriori[i0] = f0 + B[i0] * u; + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + b_A[i0 + 3 * k] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1]; + } + + P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k]; + } + } + + if ((real32_T)fabs(u) < thresh) { + x_aposteriori[1] *= decay; + } + + /* update */ + if (gps_update == 1) { + y = 0.0F; + for (k = 0; k < 3; k++) { + y += C[k] * x_aposteriori[k]; + K[k] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + K[k] += C[i0] * P_apriori[i0 + 3 * k]; + } + } + + y = z - y; + S = 0.0F; + for (k = 0; k < 3; k++) { + S += K[k] * C[k]; + } + + S += R; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (k = 0; k < 3; k++) { + f0 += P_apriori[i0 + 3 * k] * C[k]; + } + + K[i0] = f0 / S; + } + + for (i0 = 0; i0 < 3; i0++) { + x_aposteriori[i0] += K[i0] * y; + } + + for (i0 = 0; i0 < 9; i0++) { + I[i0] = 0; + } + + for (k = 0; k < 3; k++) { + I[k + 3 * k] = 1; + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0]; + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + P_aposteriori[i0 + 3 * k] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k]; + } + } + } + } else { + for (i0 = 0; i0 < 9; i0++) { + P_aposteriori[i0] = P_apriori[i0]; + } + } +} + +/* End of code generation (positionKalmanFilter1D.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h new file mode 100755 index 000000000..205c8eb4e --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D.h + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_H__ +#define __POSITIONKALMANFILTER1D_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); +#endif +/* End of code generation (positionKalmanFilter1D.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c new file mode 100755 index 000000000..4c535618a --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c @@ -0,0 +1,157 @@ +/* + * positionKalmanFilter1D_dT.c + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], + const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, + const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T + x_aposteriori[3], real32_T P_aposteriori[9]) +{ + real32_T A[9]; + int32_T i; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real32_T K[3]; + real32_T f0; + int32_T i0; + real32_T b_A[9]; + int32_T i1; + real32_T P_apriori[9]; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T fv0[3]; + real32_T y; + static const int8_T iv2[3] = { 1, 0, 0 }; + + real32_T S; + int8_T I[9]; + + /* dynamics */ + A[0] = 1.0F; + A[3] = dT; + A[6] = -0.5F * dT * dT; + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = -dT; + for (i = 0; i < 3; i++) { + A[2 + 3 * i] = (real32_T)iv0[i]; + } + + /* prediction */ + K[0] = 0.5F * dT * dT; + K[1] = dT; + K[2] = 0.0F; + for (i = 0; i < 3; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + f0 += A[i + 3 * i0] * x_aposteriori_k[i0]; + } + + x_aposteriori[i] = f0 + K[i] * u; + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1]; + } + + P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0]; + } + } + + if ((real32_T)fabs(u) < thresh) { + x_aposteriori[1] *= decay; + } + + /* update */ + if (gps_update == 1) { + f0 = 0.0F; + for (i = 0; i < 3; i++) { + f0 += (real32_T)iv1[i] * x_aposteriori[i]; + fv0[i] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i]; + } + } + + y = z - f0; + f0 = 0.0F; + for (i = 0; i < 3; i++) { + f0 += fv0[i] * (real32_T)iv2[i]; + } + + S = f0 + (real32_T)R; + for (i = 0; i < 3; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0]; + } + + K[i] = f0 / S; + } + + for (i = 0; i < 3; i++) { + x_aposteriori[i] += K[i] * y; + } + + for (i = 0; i < 9; i++) { + I[i] = 0; + } + + for (i = 0; i < 3; i++) { + I[i + 3 * i] = 1; + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + P_aposteriori[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0]; + } + } + } + } else { + for (i = 0; i < 9; i++) { + P_aposteriori[i] = P_apriori[i]; + } + } +} + +/* End of code generation (positionKalmanFilter1D_dT.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h new file mode 100755 index 000000000..94cbe2ce6 --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT.h + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_H__ +#define __POSITIONKALMANFILTER1D_DT_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); +#endif +/* End of code generation (positionKalmanFilter1D_dT.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c new file mode 100755 index 000000000..aa89f8a9d --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_initialize.c + * + * Code generation for function 'positionKalmanFilter1D_dT_initialize' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" +#include "positionKalmanFilter1D_dT_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h new file mode 100755 index 000000000..8d358a9a3 --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_initialize.h + * + * Code generation for function 'positionKalmanFilter1D_dT_initialize' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ +#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT_initialize(void); +#endif +/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c new file mode 100755 index 000000000..20ed2edbb --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_terminate.c + * + * Code generation for function 'positionKalmanFilter1D_dT_terminate' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" +#include "positionKalmanFilter1D_dT_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h new file mode 100755 index 000000000..5eb5807a0 --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_terminate.h + * + * Code generation for function 'positionKalmanFilter1D_dT_terminate' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ +#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT_terminate(void); +#endif +/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h new file mode 100755 index 000000000..43e5f016c --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h @@ -0,0 +1,16 @@ +/* + * positionKalmanFilter1D_dT_types.h + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__ +#define __POSITIONKALMANFILTER1D_DT_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (positionKalmanFilter1D_dT_types.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c new file mode 100755 index 000000000..5bd87c390 --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_initialize.c + * + * Code generation for function 'positionKalmanFilter1D_initialize' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" +#include "positionKalmanFilter1D_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (positionKalmanFilter1D_initialize.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h new file mode 100755 index 000000000..44bce472f --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_initialize.h + * + * Code generation for function 'positionKalmanFilter1D_initialize' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__ +#define __POSITIONKALMANFILTER1D_INITIALIZE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_initialize(void); +#endif +/* End of code generation (positionKalmanFilter1D_initialize.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c new file mode 100755 index 000000000..41e11936f --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_terminate.c + * + * Code generation for function 'positionKalmanFilter1D_terminate' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" +#include "positionKalmanFilter1D_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (positionKalmanFilter1D_terminate.c) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h new file mode 100755 index 000000000..e84ea01bc --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_terminate.h + * + * Code generation for function 'positionKalmanFilter1D_terminate' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__ +#define __POSITIONKALMANFILTER1D_TERMINATE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_terminate(void); +#endif +/* End of code generation (positionKalmanFilter1D_terminate.h) */ diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h new file mode 100755 index 000000000..4b473f56f --- /dev/null +++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h @@ -0,0 +1,16 @@ +/* + * positionKalmanFilter1D_types.h + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_TYPES_H__ +#define __POSITIONKALMANFILTER1D_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (positionKalmanFilter1D_types.h) */ diff --git a/apps/position_estimator_mc/codegen/randn.c b/apps/position_estimator_mc/codegen/randn.c new file mode 100755 index 000000000..51aef7b76 --- /dev/null +++ b/apps/position_estimator_mc/codegen/randn.c @@ -0,0 +1,524 @@ +/* + * randn.c + * + * Code generation for function 'randn' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "randn.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ +static uint32_T d_state[625]; + +/* Function Declarations */ +static real_T b_genrandu(uint32_T mt[625]); +static real_T eml_rand_mt19937ar(uint32_T e_state[625]); +static real_T eml_rand_shr3cong(uint32_T e_state[2]); +static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]); +static void genrandu(uint32_T s, uint32_T *e_state, real_T *r); +static void twister_state_vector(uint32_T mt[625], real_T seed); + +/* Function Definitions */ +static real_T b_genrandu(uint32_T mt[625]) +{ + real_T r; + int32_T exitg1; + uint32_T u[2]; + boolean_T isvalid; + int32_T k; + boolean_T exitg2; + + /* This is a uniform (0,1) pseudorandom number generator based on: */ + /* */ + /* A C-program for MT19937, with initialization improved 2002/1/26. */ + /* Coded by Takuji Nishimura and Makoto Matsumoto. */ + /* */ + /* Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */ + /* 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. The names of its contributors may not 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. */ + do { + exitg1 = 0; + genrand_uint32_vector(mt, u); + r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T) + (u[1] >> 6U)); + if (r == 0.0) { + if ((mt[624] >= 1U) && (mt[624] < 625U)) { + isvalid = TRUE; + } else { + isvalid = FALSE; + } + + if (isvalid) { + isvalid = FALSE; + k = 1; + exitg2 = FALSE; + while ((exitg2 == FALSE) && (k < 625)) { + if (mt[k - 1] == 0U) { + k++; + } else { + isvalid = TRUE; + exitg2 = TRUE; + } + } + } + + if (!isvalid) { + twister_state_vector(mt, 5489.0); + } + } else { + exitg1 = 1; + } + } while (exitg1 == 0); + + return r; +} + +static real_T eml_rand_mt19937ar(uint32_T e_state[625]) +{ + real_T r; + int32_T exitg1; + uint32_T u32[2]; + int32_T i; + static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068, + 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787, + 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557, + 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991, + 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419, + 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975, + 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107, + 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997, + 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153, + 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165, + 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959, + 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086, + 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062, + 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548, + 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093, + 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034, + 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357, + 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815, + 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083, + 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944, + 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653, + 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949, + 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382, + 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073, + 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936, + 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328, + 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998, + 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547, + 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872, + 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808, + 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049, + 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761, + 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417, + 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591, + 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609, + 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487, + 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818, + 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719, + 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782, + 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172, + 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452, + 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082, + 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281, + 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127, + 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043, + 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569, + 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366, + 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053, + 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753, + 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983, + 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952, + 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431, + 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416, + 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379, + 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137, + 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729, + 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715, + 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105, + 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184, + 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674, + 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999, + 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341, + 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214, + 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143, + 3.65415288536101, 3.91075795952492 }; + + real_T u; + static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108, + 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131, + 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857, + 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884, + 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964, + 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187, + 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157, + 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863, + 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512, + 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774, + 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697, + 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624, + 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914, + 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987, + 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354, + 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766, + 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952, + 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322, + 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468, + 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458, + 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773, + 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716, + 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804, + 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022, + 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188, + 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832, + 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393, + 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032, + 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077, + 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522, + 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601, + 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337, + 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803, + 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183, + 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668, + 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261, + 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942, + 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501, + 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681, + 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331, + 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881, + 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135, + 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268, + 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839, + 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366, + 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263, + 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514, + 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669, + 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597, + 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839, + 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487, + 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009, + 0.093365311912691, 0.0911136480663738, 0.0888735920682759, + 0.0866451944505581, 0.0844285095703535, 0.082223595813203, + 0.0800305158146631, 0.0778493367020961, 0.0756801303589272, + 0.0735229737139814, 0.0713779490588905, 0.0692451443970068, + 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954, + 0.0587679529209339, 0.0567106901062031, 0.0546664613248891, + 0.0526354182767924, 0.0506177238609479, 0.0486135532158687, + 0.0466230949019305, 0.0446465522512946, 0.0426841449164746, + 0.0407361106559411, 0.0388027074045262, 0.0368842156885674, + 0.0349809414617162, 0.0330932194585786, 0.0312214171919203, + 0.0293659397581334, 0.0275272356696031, 0.0257058040085489, + 0.0239022033057959, 0.0221170627073089, 0.0203510962300445, + 0.0186051212757247, 0.0168800831525432, 0.0151770883079353, + 0.0134974506017399, 0.0118427578579079, 0.0102149714397015, + 0.00861658276939875, 0.00705087547137324, 0.00552240329925101, + 0.00403797259336304, 0.00260907274610216, 0.0012602859304986, + 0.000477467764609386 }; + + real_T x; + do { + exitg1 = 0; + genrand_uint32_vector(e_state, u32); + i = (int32_T)((u32[1] >> 24U) + 1U); + r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] & + 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i]; + if (fabs(r) <= dv1[i - 1]) { + exitg1 = 1; + } else if (i < 256) { + u = b_genrandu(e_state); + if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) { + exitg1 = 1; + } + } else { + do { + u = b_genrandu(e_state); + x = log(u) * 0.273661237329758; + u = b_genrandu(e_state); + } while (!(-2.0 * log(u) > x * x)); + + if (r < 0.0) { + r = x - 3.65415288536101; + } else { + r = 3.65415288536101 - x; + } + + exitg1 = 1; + } + } while (exitg1 == 0); + + return r; +} + +static real_T eml_rand_shr3cong(uint32_T e_state[2]) +{ + real_T r; + uint32_T icng; + uint32_T jsr; + uint32_T ui; + int32_T j; + static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427, + 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948, + 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738, + 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008, + 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122, + 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503, + 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831, + 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713, + 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658, + 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 }; + + real_T x; + real_T y; + real_T s; + icng = 69069U * e_state[0] + 1234567U; + jsr = e_state[1] ^ e_state[1] << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + ui = icng + jsr; + j = (int32_T)(ui & 63U); + r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1]; + if (fabs(r) <= dv0[j]) { + } else { + x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]); + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10; + s = x + (0.5 + y); + if (s > 1.301198) { + if (r < 0.0) { + r = 0.4878992 * x - 0.4878992; + } else { + r = 0.4878992 - 0.4878992 * x; + } + } else if (s <= 0.9689279) { + } else { + s = 0.4878992 * x; + x = 0.4878992 - 0.4878992 * x; + if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) { + if (r < 0.0) { + r = -(0.4878992 - s); + } else { + r = 0.4878992 - s; + } + } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 / + dv0[j + 1] <= exp(-0.5 * r * r)) { + } else { + do { + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) / + 2.776994; + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) * + 2.328306436538696E-10) > x * x)); + + if (r < 0.0) { + r = x - 2.776994; + } else { + r = 2.776994 - x; + } + } + } + } + + e_state[0] = icng; + e_state[1] = jsr; + return r; +} + +static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]) +{ + int32_T i; + uint32_T mti; + int32_T kk; + uint32_T y; + uint32_T b_y; + uint32_T c_y; + uint32_T d_y; + for (i = 0; i < 2; i++) { + u[i] = 0U; + } + + for (i = 0; i < 2; i++) { + mti = mt[624] + 1U; + if (mti >= 625U) { + for (kk = 0; kk < 227; kk++) { + y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + b_y = y >> 1U; + } else { + b_y = y >> 1U ^ 2567483615U; + } + + mt[kk] = mt[397 + kk] ^ b_y; + } + + for (kk = 0; kk < 396; kk++) { + y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + c_y = y >> 1U; + } else { + c_y = y >> 1U ^ 2567483615U; + } + + mt[227 + kk] = mt[kk] ^ c_y; + } + + y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + d_y = y >> 1U; + } else { + d_y = y >> 1U ^ 2567483615U; + } + + mt[623] = mt[396] ^ d_y; + mti = 1U; + } + + y = mt[(int32_T)mti - 1]; + mt[624] = mti; + y ^= y >> 11U; + y ^= y << 7U & 2636928640U; + y ^= y << 15U & 4022730752U; + y ^= y >> 18U; + u[i] = y; + } +} + +static void genrandu(uint32_T s, uint32_T *e_state, real_T *r) +{ + int32_T hi; + uint32_T test1; + uint32_T test2; + hi = (int32_T)(s / 127773U); + test1 = 16807U * (s - (uint32_T)hi * 127773U); + test2 = 2836U * (uint32_T)hi; + if (test1 < test2) { + *e_state = (test1 - test2) + 2147483647U; + } else { + *e_state = test1 - test2; + } + + *r = (real_T)*e_state * 4.6566128752457969E-10; +} + +static void twister_state_vector(uint32_T mt[625], real_T seed) +{ + uint32_T r; + int32_T mti; + if (seed < 4.294967296E+9) { + if (seed >= 0.0) { + r = (uint32_T)seed; + } else { + r = 0U; + } + } else if (seed >= 4.294967296E+9) { + r = MAX_uint32_T; + } else { + r = 0U; + } + + mt[0] = r; + for (mti = 0; mti < 623; mti++) { + r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti); + mt[1 + mti] = r; + } + + mt[624] = 624U; +} + +real_T randn(void) +{ + real_T r; + uint32_T e_state; + real_T t; + real_T b_r; + uint32_T f_state; + if (method == 0U) { + if (b_method == 4U) { + do { + genrandu(b_state, &e_state, &r); + genrandu(e_state, &b_state, &t); + b_r = 2.0 * r - 1.0; + t = 2.0 * t - 1.0; + t = t * t + b_r * b_r; + } while (!(t <= 1.0)); + + r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); + } else if (b_method == 5U) { + r = eml_rand_shr3cong(c_state); + } else { + if (!state_not_empty) { + memset(&d_state[0], 0, 625U * sizeof(uint32_T)); + twister_state_vector(d_state, 5489.0); + state_not_empty = TRUE; + } + + r = eml_rand_mt19937ar(d_state); + } + } else if (method == 4U) { + e_state = state[0]; + do { + genrandu(e_state, &f_state, &r); + genrandu(f_state, &e_state, &t); + b_r = 2.0 * r - 1.0; + t = 2.0 * t - 1.0; + t = t * t + b_r * b_r; + } while (!(t <= 1.0)); + + state[0] = e_state; + r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); + } else { + r = eml_rand_shr3cong(state); + } + + return r; +} + +/* End of code generation (randn.c) */ diff --git a/apps/position_estimator_mc/codegen/randn.h b/apps/position_estimator_mc/codegen/randn.h new file mode 100755 index 000000000..8a2aa9277 --- /dev/null +++ b/apps/position_estimator_mc/codegen/randn.h @@ -0,0 +1,33 @@ +/* + * randn.h + * + * Code generation for function 'randn' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +#ifndef __RANDN_H__ +#define __RANDN_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern real_T randn(void); +#endif +/* End of code generation (randn.h) */ diff --git a/apps/position_estimator_mc/codegen/rtGetInf.c b/apps/position_estimator_mc/codegen/rtGetInf.c new file mode 100755 index 000000000..c6fa7884e --- /dev/null +++ b/apps/position_estimator_mc/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * rtGetInf.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * 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_mc/codegen/rtGetInf.h b/apps/position_estimator_mc/codegen/rtGetInf.h new file mode 100755 index 000000000..e7b2a2d1c --- /dev/null +++ b/apps/position_estimator_mc/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * rtGetInf.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTGETINF_H__ +#define __RTGETINF_H__ + +#include +#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_mc/codegen/rtGetNaN.c b/apps/position_estimator_mc/codegen/rtGetNaN.c new file mode 100755 index 000000000..552770149 --- /dev/null +++ b/apps/position_estimator_mc/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * rtGetNaN.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * 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_mc/codegen/rtGetNaN.h b/apps/position_estimator_mc/codegen/rtGetNaN.h new file mode 100755 index 000000000..5acdd9790 --- /dev/null +++ b/apps/position_estimator_mc/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * rtGetNaN.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTGETNAN_H__ +#define __RTGETNAN_H__ + +#include +#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_mc/codegen/rt_nonfinite.c b/apps/position_estimator_mc/codegen/rt_nonfinite.c new file mode 100755 index 000000000..de121c4a0 --- /dev/null +++ b/apps/position_estimator_mc/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * rt_nonfinite.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * 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_mc/codegen/rt_nonfinite.h b/apps/position_estimator_mc/codegen/rt_nonfinite.h new file mode 100755 index 000000000..3bbcfd087 --- /dev/null +++ b/apps/position_estimator_mc/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * rt_nonfinite.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RT_NONFINITE_H__ +#define __RT_NONFINITE_H__ + +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif +#include +#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_mc/codegen/rtwtypes.h b/apps/position_estimator_mc/codegen/rtwtypes.h new file mode 100755 index 000000000..8916e8572 --- /dev/null +++ b/apps/position_estimator_mc/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * rtwtypes.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: Generic->MATLAB Host Computer + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: off + *=======================================================================*/ + +/*=======================================================================* + * 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 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 int 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; + + +/*=======================================================================* + * 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)) + +/* 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_mc/kalman_dlqe1.m b/apps/position_estimator_mc/kalman_dlqe1.m new file mode 100755 index 000000000..ff939d029 --- /dev/null +++ b/apps/position_estimator_mc/kalman_dlqe1.m @@ -0,0 +1,3 @@ +function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z) + x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); +end \ No newline at end of file diff --git a/apps/position_estimator_mc/kalman_dlqe2.m b/apps/position_estimator_mc/kalman_dlqe2.m new file mode 100755 index 000000000..2a50164ef --- /dev/null +++ b/apps/position_estimator_mc/kalman_dlqe2.m @@ -0,0 +1,9 @@ +function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z) + st = 1/2*dt^2; + A = [1,dt,st; + 0,1,dt; + 0,0,1]; + C=[1,0,0]; + K = [k1;k2;k3]; + x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); +end \ No newline at end of file diff --git a/apps/position_estimator_mc/kalman_dlqe3.m b/apps/position_estimator_mc/kalman_dlqe3.m new file mode 100755 index 000000000..4c6421b7f --- /dev/null +++ b/apps/position_estimator_mc/kalman_dlqe3.m @@ -0,0 +1,17 @@ +function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma) + st = 1/2*dt^2; + A = [1,dt,st; + 0,1,dt; + 0,0,1]; + C=[1,0,0]; + K = [k1;k2;k3]; + if addNoise==1 + noise = sigma*randn(1,1); + z = z + noise; + end + if(posUpdate) + x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); + else + x_aposteriori=A*x_aposteriori_k; + end +end \ No newline at end of file diff --git a/apps/position_estimator_mc/positionKalmanFilter1D.m b/apps/position_estimator_mc/positionKalmanFilter1D.m new file mode 100755 index 000000000..144ff8c7c --- /dev/null +++ b/apps/position_estimator_mc/positionKalmanFilter1D.m @@ -0,0 +1,19 @@ +function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) +%prediction + x_apriori=A*x_aposteriori_k+B*u; + P_apriori=A*P_aposteriori_k*A'+Q; + if abs(u) + * Tobias Naegeli +* Lorenz Meier + * + * 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 position_estimator_main.c + * Model-identification based position estimator for multirotors + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "position_estimator_mc_params.h" +//#include +#include "codegen/kalman_dlqe2.h" +#include "codegen/kalman_dlqe2_initialize.h" +#include "codegen/kalman_dlqe3.h" +#include "codegen/kalman_dlqe3_initialize.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_mc_task; /**< Handle of deamon task / thread */ + +__EXPORT int position_estimator_mc_main(int argc, char *argv[]); + +int position_estimator_mc_thread_main(int argc, char *argv[]); +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + warnx("%s\n", reason); + warnx("usage: position_estimator_mc {start|stop|status}"); + exit(1); +} + +/** + * The position_estimator_mc_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int position_estimator_mc_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("position_estimator_mc already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + position_estimator_mc_task = task_spawn("position_estimator_mc", + SCHED_RR, + SCHED_PRIORITY_MAX - 5, + 4096, + position_estimator_mc_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("position_estimator_mc is running"); + } else { + warnx("position_estimator_mc not started"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_mc_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + warnx("[position_estimator_mc] started"); + int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[position_estimator_mc] started"); + + /* initialize values */ + float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */ + // float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f}; + float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; + float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; + float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; + float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f}; + float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f}; + float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f}; + + // XXX this is terribly wrong and should actual dT instead + const float dT_const_50 = 1.0f/50.0f; + + float addNoise = 0.0f; + float sigma = 0.0f; + //computed from dlqe in matlab + const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f}; + // XXX implement baro filter + const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f}; + float K[3] = {0.0f, 0.0f, 0.0f}; + int baro_loop_cnt = 0; + int baro_loop_end = 70; /* measurement for 1 second */ + float p0_Pa = 0.0f; /* to determin while start up */ + float rho0 = 1.293f; /* standard pressure */ + const float const_earth_gravity = 9.81f; + + float posX = 0.0f; + float posY = 0.0f; + float posZ = 0.0f; + + double lat_current; + double lon_current; + float alt_current; + + float gps_origin_altitude = 0.0f; + + /* Initialize filter */ + kalman_dlqe2_initialize(); + kalman_dlqe3_initialize(); + + /* declare and safely initialize all structs */ + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + struct vehicle_vicon_position_s vicon_pos; + memset(&vicon_pos, 0, sizeof(vicon_pos)); + struct actuator_controls_effective_s act_eff; + memset(&act_eff, 0, sizeof(act_eff)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_local_position_s local_pos_est; + memset(&local_pos_est, 0, sizeof(local_pos_est)); + struct vehicle_global_position_s global_pos_est; + memset(&global_pos_est, 0, sizeof(global_pos_est)); + + /* subscribe */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0)); + int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise */ + orb_advert_t local_pos_est_pub = 0; + orb_advert_t global_pos_est_pub = 0; + + struct position_estimator_mc_params pos1D_params; + struct position_estimator_mc_param_handles pos1D_param_handles; + /* initialize parameter handles */ + parameters_init(&pos1D_param_handles); + + bool flag_use_gps = false; + bool flag_use_baro = false; + bool flag_baro_initialized = false; /* in any case disable baroINITdone */ + /* FIRST PARAMETER READ at START UP*/ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */ + /* FIRST PARAMETER UPDATE */ + parameters_update(&pos1D_param_handles, &pos1D_params); + flag_use_baro = pos1D_params.baro; + sigma = pos1D_params.sigma; + addNoise = pos1D_params.addNoise; + /* END FIRST PARAMETER UPDATE */ + + /* try to grab a vicon message - if it fails, go for GPS. */ + + /* make sure the next orb_check() can't return true unless we get a timely update */ + orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); + /* allow 200 ms for vicon to come in */ + usleep(200000); + /* check if we got vicon */ + bool update_check; + orb_check(vicon_pos_sub, &update_check); + /* if no update was available, use GPS */ + flag_use_gps = !update_check; + + if (flag_use_gps) { + mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked"); + /* wait until gps signal turns valid, only then can we initialize the projection */ + + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + while (!(gps.fix_type == 3 + && (gps.eph_m < hdop_threshold_m) + && (gps.epv_m < vdop_threshold_m) + && (hrt_absolute_time() - gps.timestamp_position < 2000000))) { + + struct pollfd fds1[2] = { + { .fd = vehicle_gps_sub, .events = POLLIN }, + { .fd = sub_params, .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(fds1, 2, 5000)) { + if (fds1[0].revents & POLLIN){ + /* Read gps position */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + } + if (fds1[1].revents & POLLIN){ + /* Read out parameters to check for an update there, e.g. useGPS variable */ + /* read from param to clear updated flag */ + struct parameter_update_s updated; + orb_copy(ORB_ID(parameter_update), sub_params, &updated); + /* update parameters */ + parameters_update(&pos1D_param_handles, &pos1D_params); + } + } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + warnx("[pos_est_mc] wait for GPS fix"); + } + printcounter++; + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + lat_current = ((double)(gps.lat)) * 1e-7d; + lon_current = ((double)(gps.lon)) * 1e-7d; + alt_current = gps.alt * 1e-3f; + gps_origin_altitude = alt_current; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + + } else { + mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); + /* onboard calculated position estimations */ + } + thread_running = true; + + struct pollfd fds2[3] = { + { .fd = vehicle_gps_sub, .events = POLLIN }, + { .fd = vicon_pos_sub, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN }, + }; + + bool vicon_updated = false; + bool gps_updated = false; + + /**< main_loop */ + while (!thread_should_exit) { + int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate + if (ret < 0) { + /* poll error */ + } else { + if (fds2[2].revents & POLLIN){ + /* new parameter */ + /* read from param to clear updated flag */ + struct parameter_update_s updated; + orb_copy(ORB_ID(parameter_update), sub_params, &updated); + /* update parameters */ + parameters_update(&pos1D_param_handles, &pos1D_params); + flag_use_baro = pos1D_params.baro; + sigma = pos1D_params.sigma; + addNoise = pos1D_params.addNoise; + } + vicon_updated = false; /* default is no vicon_updated */ + if (fds2[1].revents & POLLIN) { + /* new vicon position */ + orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); + posX = vicon_pos.x; + posY = vicon_pos.y; + posZ = vicon_pos.z; + vicon_updated = true; /* set flag for vicon update */ + } /* end of poll call for vicon updates */ + gps_updated = false; + if (fds2[0].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + /* Project gps lat lon (Geographic coordinate system) to plane*/ + map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1])); + posX = z[0]; + posY = z[1]; + posZ = (float)(gps.alt * 1e-3f); + gps_updated = true; + } + + /* Main estimator loop */ + orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff); + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor); + // barometric pressure estimation at start up + if (!flag_baro_initialized){ + // mean calculation over several measurements + if (baro_loop_cnt 2) { + /* x-y-position/velocity estimation in earth frame = gps frame */ + kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori); + memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); + kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori); + memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); + /* z-position/velocity estimation in earth frame = vicon frame */ + float z_est = 0.0f; + if (flag_baro_initialized && flag_use_baro) { + z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity); + K[0] = K_vicon_50Hz[0]; + K[1] = K_vicon_50Hz[1]; + K[2] = K_vicon_50Hz[2]; + gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */ + } else { + z_est = posZ; + K[0] = K_vicon_50Hz[0]; + K[1] = K_vicon_50Hz[1]; + K[2] = K_vicon_50Hz[2]; + } + + kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori); + memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); + local_pos_est.x = x_x_aposteriori_k[0]; + local_pos_est.vx = x_x_aposteriori_k[1]; + local_pos_est.y = x_y_aposteriori_k[0]; + local_pos_est.vy = x_y_aposteriori_k[1]; + local_pos_est.z = x_z_aposteriori_k[0]; + local_pos_est.vz = x_z_aposteriori_k[1]; + local_pos_est.timestamp = hrt_absolute_time(); + if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) { + /* publish local position estimate */ + if (local_pos_est_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + } else { + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + } + /* publish on GPS updates */ + if (gps_updated) { + + double lat, lon; + float alt = z_est + gps_origin_altitude; + + map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon); + + global_pos_est.lat = lat; + global_pos_est.lon = lon; + global_pos_est.alt = alt; + + if (global_pos_est_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est); + } else { + global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est); + } + } + } + } + } else { + /* x-y-position/velocity estimation in earth frame = vicon frame */ + kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori); + memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); + kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori); + memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); + /* z-position/velocity estimation in earth frame = vicon frame */ + float z_est = 0.0f; + float local_sigma = 0.0f; + if (flag_baro_initialized && flag_use_baro) { + z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity); + K[0] = K_vicon_50Hz[0]; + K[1] = K_vicon_50Hz[1]; + K[2] = K_vicon_50Hz[2]; + vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */ + local_sigma = 0.0f; /* don't add noise on barometer in any case */ + } else { + z_est = posZ; + K[0] = K_vicon_50Hz[0]; + K[1] = K_vicon_50Hz[1]; + K[2] = K_vicon_50Hz[2]; + local_sigma = sigma; + } + kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori); + memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); + local_pos_est.x = x_x_aposteriori_k[0]; + local_pos_est.vx = x_x_aposteriori_k[1]; + local_pos_est.y = x_y_aposteriori_k[0]; + local_pos_est.vy = x_y_aposteriori_k[1]; + local_pos_est.z = x_z_aposteriori_k[0]; + local_pos_est.vz = x_z_aposteriori_k[1]; + local_pos_est.timestamp = hrt_absolute_time(); + if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + } + } + } /* end of poll return value check */ + } + + printf("[pos_est_mc] exiting.\n"); + mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting"); + thread_running = false; + return 0; +} diff --git a/apps/position_estimator_mc/position_estimator_mc_params.c b/apps/position_estimator_mc/position_estimator_mc_params.c new file mode 100755 index 000000000..72e5bc73b --- /dev/null +++ b/apps/position_estimator_mc/position_estimator_mc_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli +* Lorenz Meier + * + * 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 position_estimator_mc_params.c + * + * Parameters for position_estimator_mc + */ + +#include "position_estimator_mc_params.h" + +/* Kalman Filter covariances */ +/* gps measurement noise standard deviation */ +PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f); +PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f); +PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); +PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f); + +int parameters_init(struct position_estimator_mc_param_handles *h) +{ + h->addNoise = param_find("POS_EST_ADDN"); + h->sigma = param_find("POS_EST_SIGMA"); + h->r = param_find("POS_EST_R"); + h->baro_param_handle = param_find("POS_EST_BARO"); + return OK; +} + +int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p) +{ + param_get(h->addNoise, &(p->addNoise)); + param_get(h->sigma, &(p->sigma)); + param_get(h->r, &(p->R)); + param_get(h->baro_param_handle, &(p->baro)); + return OK; +} diff --git a/apps/position_estimator_mc/position_estimator_mc_params.h b/apps/position_estimator_mc/position_estimator_mc_params.h new file mode 100755 index 000000000..c4c95f7b4 --- /dev/null +++ b/apps/position_estimator_mc/position_estimator_mc_params.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * Lorenz Meier + * + * 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 position_estimator_mc_params.h + * + * Parameters for Position Estimator + */ + +#include + +struct position_estimator_mc_params { + float addNoise; + float sigma; + float R; + int baro; /* consider barometer */ +}; + +struct position_estimator_mc_param_handles { + param_t addNoise; + param_t sigma; + param_t r; + param_t baro_param_handle; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct position_estimator_mc_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312..9e28ff485 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -97,7 +97,7 @@ ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -#CONFIGURED_APPS += fixedwing_control +CONFIGURED_APPS += position_estimator_mc CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator -- cgit v1.2.3 From 6fb2496c49d2e6e380207df4acdfa9f79f2c0c5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Mar 2013 18:38:40 +0100 Subject: Improved HIL startup script, added highres HIL receive routine --- ROMFS/scripts/rc.hil | 11 +++++++ apps/mavlink/mavlink_receiver.c | 68 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+) diff --git a/ROMFS/scripts/rc.hil b/ROMFS/scripts/rc.hil index 3b37ac26b..980b78edd 100644 --- a/ROMFS/scripts/rc.hil +++ b/ROMFS/scripts/rc.hil @@ -35,6 +35,17 @@ param set MAV_TYPE 1 # commander start +# +# Check if we got an IO +# +if [ px4io start ] +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +end + # # Start the sensors (depends on orb, px4io) # diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 4d9cd964d..28b0c50c9 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -376,6 +376,74 @@ handle_message(mavlink_message_t *msg) } } + if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { + + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = imu.time_usec; + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; + hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; + hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; + hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; + hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { mavlink_gps_raw_int_t gps; -- cgit v1.2.3 From a1c8d19c343454c5464ae21be1ec4456b0559996 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Apr 2013 18:43:19 +0200 Subject: Added generation of pressure altitude in highres IMU message mode --- apps/mavlink/mavlink_receiver.c | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 28b0c50c9..798e509e0 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -308,6 +308,14 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); + /* TODO, set ground_press/ temp during calib */ + static const float ground_press = 1013.25f; // mbar + static const float ground_tempC = 21.0f; + static const float ground_alt = 0.0f; + static const float T0 = 273.15; + static const float R = 287.05f; + static const float g = 9.806f; + if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { mavlink_raw_imu_t imu; @@ -429,6 +437,15 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.baro_pres_mbar = imu.abs_pressure; + + float tempC = imu.temperature; + float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; + float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); + + hil_sensors.baro_alt_meter = h; + hil_sensors.baro_temp_celcius = imu.temperature; + /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); @@ -503,13 +520,6 @@ handle_message(mavlink_message_t *msg) hil_sensors.timestamp = press.time_usec; /* baro */ - /* TODO, set ground_press/ temp during calib */ - static const float ground_press = 1013.25f; // mbar - static const float ground_tempC = 21.0f; - static const float ground_alt = 0.0f; - static const float T0 = 273.15; - static const float R = 287.05f; - static const float g = 9.806f; float tempC = press.temperature / 100.0f; float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; -- cgit v1.2.3 From 8c70f4412d7e55f22cffa60e0ea953751780f462 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:29:24 -0700 Subject: Fixed axis in L3GD20 driver --- apps/drivers/l3gd20/l3gd20.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6227df72a..c7f433dd4 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -684,9 +684,10 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -- cgit v1.2.3 From d7178fb8339d94ce119a190a076eb931b8a265e0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 10:10:08 -0700 Subject: Play SOS if the SD card can't be mounted --- ROMFS/scripts/rcS | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 89a767879..c0a70f7dd 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -21,9 +21,9 @@ set MODE autostart set USB autoconnect # -# Start playing the startup tune + # -tone_alarm start + # # Try to mount the microSD card. @@ -32,8 +32,12 @@ echo "[init] looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start else echo "[init] no microSD card found" + # Play SOS + tone_alarm 2 fi # -- cgit v1.2.3 From d7d0d9ea513ea146d42bd5f6397abcd5943ca73b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 15:53:25 +0200 Subject: Fixed startup scripts for default platforms, needs integration with wiki to avoid similar issues --- ROMFS/scripts/rc.PX4IO | 57 +++++++++++++++++++++++++++++++++++------------- ROMFS/scripts/rc.PX4IOAR | 27 ++++++++++++----------- 2 files changed, 56 insertions(+), 28 deletions(-) diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 1e3963b9a..625f23bdd 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -5,7 +5,7 @@ set USB no set MODE camflyer # -# Start the ORB +# Start the ORB (first app to start) # uorb start @@ -27,38 +27,65 @@ fi param set MAV_TYPE 1 # -# Start the sensors. +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) # -sh /etc/init.d/rc.sensors +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi # -# Start MAVLink +# Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 # -# Start the commander. +# Start the commander (depends on orb, mavlink) # commander start # -# Start GPS interface +# Start PX4IO interface (depends on orb, commander) # -gps start +px4io start # -# Start the attitude estimator +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + # -kalman_demo start +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors # -# Start PX4IO interface +# Start GPS interface (depends on orb) # -px4io start +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start # -# Load mixer and start controllers +# Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start @@ -66,8 +93,8 @@ control_demo start # # Start logging # -sdlog start -s 10 - +#sdlog start -s 4 + # # Start system state # @@ -77,4 +104,4 @@ then blinkm systemstate else echo "no BlinkM found, OK." -fi +fi \ No newline at end of file diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 72df68e35..6af91992e 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -2,10 +2,10 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - + # Disable the USB interface set USB no - + # Disable autostarting other apps set MODE ardrone @@ -25,13 +25,18 @@ if [ -f /fs/microsd/parameters ] then param load /fs/microsd/parameters fi - + # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor # see https://pixhawk.ethz.ch/mavlink/ # param set MAV_TYPE 2 + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial # # Start the sensors. @@ -54,11 +59,6 @@ commander start # attitude_estimator_ekf start -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - # # Fire up the multi rotor attitude controller # @@ -68,17 +68,17 @@ multirotor_att_control start # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start GPS capture -# -gps start # # Start logging # sdlog start -s 10 +# +# Start GPS capture +# +gps start + # # Start system state # @@ -95,4 +95,5 @@ fi # use the same UART for telemetry # echo "[init] startup done" + exit \ No newline at end of file -- cgit v1.2.3 From 492e0174755740799ac095345d96baaa370e20cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Apr 2013 14:58:07 +0200 Subject: New, fancy log conversion script by Philipp Oettershagen. Offers a few convenience functions and plots more than previously --- ROMFS/logging/logconv.m | 810 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 586 insertions(+), 224 deletions(-) mode change 100755 => 100644 ROMFS/logging/logconv.m diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m old mode 100755 new mode 100644 index 92ee01413..3750ddae2 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -1,224 +1,586 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -% Clear everything -clc -clear all -close all - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% Work around a Matlab bug (not related to PX4) -% where timestamps from 1.1.1970 do not allow to -% read the file's size -if ismac - system('touch -t 201212121212.12 sysvector.bin'); -end - -%%%%%%%%%%%%%%%%%%%%%%% -% SYSTEM VECTOR -% -% //All measurements in NED frame -% -% uint64_t timestamp; //[us] -% float gyro[3]; //[rad/s] -% float accel[3]; //[m/s^2] -% float mag[3]; //[gauss] -% float baro; //pressure [millibar] -% float baro_alt; //altitude above MSL [meter] -% float baro_temp; //[degree celcius] -% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] -% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) -% float vbat; //battery voltage in [volt] -% float bat_current - current drawn from battery at this time instant -% float bat_discharged - discharged energy in mAh -% float adc[4]; //ADC ports [volt] -% float local_position[3]; //tangent plane mapping into x,y,z [m] -% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] -% float attitude[3]; //pitch, roll, yaw [rad] -% float rotMatrix[9]; //unitvectors -% float actuator_control[4]; //unitvector -% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] -% float diff_pressure; - pressure difference in millibar -% float ind_airspeed; -% float true_airspeed; - -% Definition of the logged values -logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); -logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); -logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - -% First get length of one line -columns = length(logFormat); -lineLength = 0; - -for i=1:columns - lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; -end - - -if exist(filePath, 'file') - - fileInfo = dir(filePath); - fileSize = fileInfo.bytes; - - elements = int64(fileSize./(lineLength)); - - fid = fopen(filePath, 'r'); - offset = 0; - for i=1:columns - % using fread with a skip speeds up the import drastically, do not - % import the values one after the other - sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... - fid, ... - [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... - lineLength - logFormat{i}.bytes*logFormat{i}.array, ... - logFormat{i}.machineformat) ... - ); - offset = offset + logFormat{i}.bytes*logFormat{i}.array; - fseek(fid, offset,'bof'); - end - - % shot the flight time - time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6; - time_m = time_s/60; - - % close the logfile - fclose(fid); - - disp(['end log2matlab conversion' char(10)]); -else - disp(['file: ' filePath ' does not exist' char(10)]); -end - -%% Plot GPS RAW measurements - -% Only plot GPS data if available -if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 - figure('units','normalized','outerposition',[0 0 1 1]) - plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); -end - - -%% Plot optical flow trajectory - -flow_sz = size(sysvector.timestamp); -flow_elements = flow_sz(1); - -xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] - - -%calc dt -dt = zeros(flow_elements,1); -for i = 1:flow_elements-1 - dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] -end -dt(1,1) = mean(dt); - - -global_speed = zeros(flow_elements,3); - -%calc global speed (with rot matrix) -for i = 1:flow_elements - rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; - speedX = sysvector.optical_flow(i,3); - speedY = sysvector.optical_flow(i,4); - - relSpeed = [-speedY,speedX,0]; - global_speed(i,:) = relSpeed * rotM; -end - - - -px = zeros(flow_elements,1); -py = zeros(flow_elements,1); -distance = 0; - -last_vx = 0; -last_vy = 0; -elem_cnt = 0; - -% Very basic accumulation, stops on bad flow quality -for i = 1:flow_elements - if sysvector.optical_flow(i,6) > 5 - px(i,1) = global_speed(i,1)*dt(i,1); - py(i,1) = global_speed(i,2)*dt(i,1); - distance = distance + norm([px(i,1) py(i,1)]); - last_vx = px(i,1); - last_vy = py(i,1); - else - px(i,1) = last_vx; - py(i,1) = last_vy; - last_vx = last_vx*0.95; - last_vy = last_vy*0.95; - end -end - -px_sum = cumsum(px); -py_sum = cumsum(py); -time = cumsum(dt); - -figure() -set(gca, 'Units','normal'); - -plot(py_sum, px_sum, '-blue', 'LineWidth',2); -axis equal; -% set title and axis captions -xlabel('X position (meters)','fontsize',14) -ylabel('Y position (meters)','fontsize',14) -% mark begin and end -hold on -plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','g',... -'MarkerSize',10) -hold on -plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','b',... -'MarkerSize',10) -% add total length as annotation -set(gca,'fontsize',13); -legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); -title('Optical Flow Position Integration', 'fontsize', 15); - -figure() -plot(time, sysvector.optical_flow(:,5), 'blue'); -axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); -xlabel('seconds','fontsize',14); -ylabel('m','fontsize',14); -set(gca,'fontsize',13); -title('Ultrasound Altitude', 'fontsize', 15); - - -figure() -plot(time, global_speed(:,2), 'red'); -hold on; -plot(time, global_speed(:,1), 'blue'); -legend('y velocity (m/s)', 'x velocity (m/s)'); -xlabel('seconds','fontsize',14); -ylabel('m/s','fontsize',14); -set(gca,'fontsize',13); -title('Optical Flow Velocity', 'fontsize', 15); +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +%% ************************************************************************ +% PX4LOG_PLOTSCRIPT: Main function +% ************************************************************************ +function PX4Log_Plotscript + +% Clear everything +clc +clear all +close all + +% ************************************************************************ +% SETTINGS +% ************************************************************************ + +% Set the path to your sysvector.bin file here +filePath = 'sysvector.bin'; + +% Set the minimum and maximum times to plot here [in seconds] +mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] +maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] + +%Determine which data to plot. Not completely implemented yet. +bDisplayGPS=true; + +%conversion factors +fconv_gpsalt=1E-3; %[mm] to [m] +fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg] +fconv_timestamp=1E-6; % [microseconds] to [seconds] + +% ************************************************************************ +% Import the PX4 logs +% ************************************************************************ +ImportPX4LogData(); + +%Translate min and max plot times to indices +time=double(sysvector.timestamp) .*fconv_timestamp; +mintime_log=time(1); %The minimum time/timestamp found in the log +maxtime_log=time(end); %The maximum time/timestamp found in the log +CurTime=mintime_log; %The current time at which to draw the aircraft position + +[imintime,imaxtime]=FindMinMaxTimeIndices(); + +% ************************************************************************ +% PLOT & GUI SETUP +% ************************************************************************ +NrFigures=5; +NrAxes=10; +h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively +h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively +h.pathpoints=[]; % Temporary initiliazation of path points + +% Setup the GUI to control the plots +InitControlGUI(); +% Setup the plotting-GUI (figures, axes) itself. +InitPlotGUI(); + +% ************************************************************************ +% DRAW EVERYTHING +% ************************************************************************ +DrawRawData(); +DrawCurrentAircraftState(); + +%% ************************************************************************ +% *** END OF MAIN SCRIPT *** +% NESTED FUNCTION DEFINTIONS FROM HERE ON +% ************************************************************************ + + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +function ImportPX4LogData() + % Work around a Matlab bug (not related to PX4) + % where timestamps from 1.1.1970 do not allow to + % read the file's size + if ismac + system('touch -t 201212121212.12 sysvector.bin'); + end + + % ************************************************************************ + % RETRIEVE SYSTEM VECTOR + % ************************************************************************* + % //All measurements in NED frame + % + % uint64_t timestamp; //[us] + % float gyro[3]; //[rad/s] + % float accel[3]; //[m/s^2] + % float mag[3]; //[gauss] + % float baro; //pressure [millibar] + % float baro_alt; //altitude above MSL [meter] + % float baro_temp; //[degree celcius] + % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] + % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) + % float vbat; //battery voltage in [volt] + % float bat_current - current drawn from battery at this time instant + % float bat_discharged - discharged energy in mAh + % float adc[4]; //remaining auxiliary ADC ports [volt] + % float local_position[3]; //tangent plane mapping into x,y,z [m] + % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] + % float attitude[3]; //pitch, roll, yaw [rad] + % float rotMatrix[9]; //unitvectors + % float actuator_control[4]; //unitvector + % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] + % float diff_pressure; - pressure difference in millibar + % float ind_airspeed; + % float true_airspeed; + + % Definition of the logged values + logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); + logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); + logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + + % First get length of one line + columns = length(logFormat); + lineLength = 0; + + for i=1:columns + lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; + end + + + if exist(filePath, 'file') + + fileInfo = dir(filePath); + fileSize = fileInfo.bytes; + + elements = int64(fileSize./(lineLength)); + + fid = fopen(filePath, 'r'); + offset = 0; + for i=1:columns + % using fread with a skip speeds up the import drastically, do not + % import the values one after the other + sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... + fid, ... + [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... + lineLength - logFormat{i}.bytes*logFormat{i}.array, ... + logFormat{i}.machineformat) ... + ); + offset = offset + logFormat{i}.bytes*logFormat{i}.array; + fseek(fid, offset,'bof'); + end + + % shot the flight time + time_us = sysvector.timestamp(end) - sysvector.timestamp(1); + time_s = time_us*1e-6; + time_m = time_s/60; + + % close the logfile + fclose(fid); + + disp(['end log2matlab conversion' char(10)]); + else + disp(['file: ' filePath ' does not exist' char(10)]); + end +end + +%% ************************************************************************ +% INITCONTROLGUI (nested function) +% ************************************************************************ +%Setup central control GUI components to control current time where data is shown +function InitControlGUI() + %********************************************************************** + % GUI size definitions + %********************************************************************** + dxy=5; %margins + %Panel: Plotctrl + dlabels=120; + dsliders=200; + dedits=80; + hslider=20; + + hpanel1=40; %panel1 + hpanel2=220;%panel2 + hpanel3=3*hslider+4*dxy+3*dxy;%panel3. + + width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width + height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height + + %********************************************************************** + % Create GUI + %********************************************************************** + h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); + h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); + h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); + h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); + + %%Control GUI-elements + %Slider: Current time + h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... + 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); + h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... + 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); + + %Slider: MaxTime + h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %Slider: MinTime + h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %%Current data/state GUI-elements (Multiline-edit-box) + h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... + 'HorizontalAlignment','left','parent',h.aircraftstatepanel); + + h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... + 'HorizontalAlignment','left','parent',h.guistatepanel); + +end + +%% ************************************************************************ +% INITPLOTGUI (nested function) +% ************************************************************************ +function InitPlotGUI() + + % Setup handles to lines and text + h.markertext=[]; + templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array + h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively + h.markerline(1:NrAxes)=0.0; + + % Setup all other figures and axes for plotting + % PLOT WINDOW 1: GPS POSITION + h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); + h.axes(1)=axes(); + set(h.axes(1),'Parent',h.figures(2)); + + % PLOT WINDOW 2: IMU, baro altitude + h.figures(3)=figure('Name', 'IMU / Baro Altitude'); + h.axes(2)=subplot(4,1,1); + h.axes(3)=subplot(4,1,2); + h.axes(4)=subplot(4,1,3); + h.axes(5)=subplot(4,1,4); + set(h.axes(2:5),'Parent',h.figures(3)); + + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); + h.axes(6)=subplot(4,1,1); + h.axes(7)=subplot(4,1,2); + h.axes(8)=subplot(4,1,3); + h.axes(9)=subplot(4,1,4); + set(h.axes(6:9),'Parent',h.figures(4)); + + % PLOT WINDOW 4: LOG STATS + h.figures(5) = figure('Name', 'Log Statistics'); + h.axes(10)=subplot(1,1,1); + set(h.axes(10:10),'Parent',h.figures(5)); + +end + +%% ************************************************************************ +% DRAWRAWDATA (nested function) +% ************************************************************************ +%Draws the raw data from the sysvector, but does not add any +%marker-lines or so +function DrawRawData() + % ************************************************************************ + % PLOT WINDOW 1: GPS POSITION & GUI + % ************************************************************************ + figure(h.figures(2)); + % Only plot GPS data if available + if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS) + %Draw data + plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.'); + title(h.axes(1),'GPS Position Data(if available)'); + xlabel(h.axes(1),'Latitude [deg]'); + ylabel(h.axes(1),'Longitude [deg]'); + zlabel(h.axes(1),'Altitude above MSL [m]'); + grid on + + %Reset path + h.pathpoints=0; + end + + % ************************************************************************ + % PLOT WINDOW 2: IMU, baro altitude + % ************************************************************************ + figure(h.figures(3)); + plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:)); + title(h.axes(2),'Magnetometers [Gauss]'); + legend(h.axes(2),'x','y','z'); + plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:)); + title(h.axes(3),'Accelerometers [m/s²]'); + legend(h.axes(3),'x','y','z'); + plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:)); + title(h.axes(4),'Gyroscopes [rad/s]'); + legend(h.axes(4),'x','y','z'); + plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue'); + if(bDisplayGPS) + hold on; + plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red'); + hold off + legend('Barometric Altitude [m]','GPS Altitude [m]'); + else + legend('Barometric Altitude [m]'); + end + title(h.axes(5),'Altitude above MSL [m]'); + + % ************************************************************************ + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + % ************************************************************************ + figure(h.figures(4)); + %Attitude Estimate + plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159); + title(h.axes(6),'Estimated attitude [deg]'); + legend(h.axes(6),'roll','pitch','yaw'); + %Actuator Controls + plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:)); + title(h.axes(7),'Actuator control [-]'); + legend(h.axes(7),'0','1','2','3'); + %Actuator Controls + plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8)); + title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); + legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); + set(h.axes(8), 'YLim',[800 2200]); + %Airspeeds + plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime)); + hold on + plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime)); + hold off + %add GPS total airspeed here + title(h.axes(9),'Airspeed [m/s]'); + legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); + %calculate time differences and plot them + intervals = zeros(0,imaxtime - imintime); + for k = imintime+1:imaxtime + intervals(k) = time(k) - time(k-1); + end + plot(h.axes(10), time(imintime:imaxtime), intervals); + + %Set same timescale for all plots + for i=2:NrAxes + set(h.axes(i),'XLim',[mintime maxtime]); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% DRAWCURRENTAIRCRAFTSTATE(nested function) +% ************************************************************************ +function DrawCurrentAircraftState() + %find current data index + i=find(time>=CurTime,1,'first'); + + %********************************************************************** + % Current aircraft state label update + %********************************************************************** + acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',... + 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',... + 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; + acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),... + ', y=',num2str(sysvector.mag(i,2)),... + ', z=',num2str(sysvector.mag(i,3)),']']; + acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),... + ', y=',num2str(sysvector.accel(i,2)),... + ', z=',num2str(sysvector.accel(i,3)),']']; + acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),... + ', y=',num2str(sysvector.gyro(i,2)),... + ', z=',num2str(sysvector.gyro(i,3)),']']; + acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; + acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),... + ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),... + ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']']; + acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); + for j=1:4 + acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),',']; + end + acstate{7,:}=[acstate{7,:},']']; + acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); + for j=1:8 + acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),',']; + end + acstate{8,:}=[acstate{8,:},']']; + acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']']; + + set(h.edits.AircraftState,'String',acstate); + + %********************************************************************** + % GPS Plot Update + %********************************************************************** + %Plot traveled path, and and time. + figure(h.figures(2)); + hold on; + if(CurTime>mintime+1) %the +1 is only a small bugfix + h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); + end; + hold off + %Plot current position + newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt]; + if(numel(h.pathpoints)<=3) %empty path + h.pathpoints(1,1:3)=newpoint; + else %Not empty, append new point + h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; + end + axes(h.axes(1)); + line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); + + + % Plot current time (small label next to current gps position) + textdesc=strcat(' t=',num2str(time(i)),'s'); + if(isvalidhandle(h.markertext)) + delete(h.markertext); %delete old text + end + h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,... + double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc); + set(h.edits.CurTime,'String',CurTime); + + %********************************************************************** + % Plot the lines showing the current time in the 2-d plots + %********************************************************************** + for i=2:NrAxes + if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end + ylims=get(h.axes(i),'YLim'); + h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); + set(h.markerline(i),'parent',h.axes(i)); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% MINMAXTIME CALLBACK (nested function) +% ************************************************************************ +function minmaxtime_callback(hObj,event) %#ok + new_mintime=get(h.sliders.MinTime,'Value'); + new_maxtime=get(h.sliders.MaxTime,'Value'); + + %Safety checks: + bErr=false; + %1: mintime must be < maxtime + if((new_mintime>maxtime) || (new_maxtimeCurTime) + set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); + mintime=new_mintime; + CurTime=mintime; + bErr=true; + end + %3: MaxTime must be >CurTime + if(new_maxtime + %find current time + if(hObj==h.sliders.CurTime) + CurTime=get(h.sliders.CurTime,'Value'); + elseif (hObj==h.edits.CurTime) + temp=str2num(get(h.edits.CurTime,'String')); + if(tempmintime) + CurTime=temp; + else + %Error + set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); + end + else + %Error + set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); + end + + set(h.sliders.CurTime,'Value',CurTime); + set(h.edits.CurTime,'String',num2str(CurTime)); + + %Redraw time markers, but don't have to redraw the whole raw data + DrawCurrentAircraftState(); +end + +%% ************************************************************************ +% FINDMINMAXINDICES (nested function) +% ************************************************************************ +function [idxmin,idxmax] = FindMinMaxTimeIndices() + for i=1:size(sysvector.timestamp,1) + if time(i)>=mintime; idxmin=i; break; end + end + for i=1:size(sysvector.timestamp,1) + if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end + if time(i)>=maxtime; idxmax=i; break; end + end + mintime=time(idxmin); + maxtime=time(idxmax); +end + +%% ************************************************************************ +% ISVALIDHANDLE (nested function) +% ************************************************************************ +function isvalid = isvalidhandle(handle) + if(exist(varname(handle))>0 && length(ishandle(handle))>0) + if(ishandle(handle)>0) + if(handle>0.0) + isvalid=true; + return; + end + end + end + isvalid=false; +end + +%% ************************************************************************ +% JUST SOME SMALL HELPER FUNCTIONS (nested function) +% ************************************************************************ +function out = varname(var) + out = inputname(1); +end + +%This is the end of the matlab file / the main function +end -- cgit v1.2.3 From edc8e9aa5dc858d8177adacb64e1d7843017fac4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Apr 2013 13:43:28 +0200 Subject: Small compile warning fix for HoTT test --- apps/px4/tests/test_hott_telemetry.c | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/px4/tests/test_hott_telemetry.c b/apps/px4/tests/test_hott_telemetry.c index 74aa0e614..270dc3857 100644 --- a/apps/px4/tests/test_hott_telemetry.c +++ b/apps/px4/tests/test_hott_telemetry.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From 82d2ab677ea6bfccdc211a6e0c77a7904175e0c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Apr 2013 13:47:45 +0200 Subject: Fixes to MAVLink HIL --- apps/mavlink/mavlink_receiver.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 798e509e0..588b3e07e 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -395,7 +395,7 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* sensors general */ - hil_sensors.timestamp = imu.time_usec; + hil_sensors.timestamp = hrt_absolute_time(); /* hil gyro */ static const float mrad2rad = 1.0e-3f; @@ -446,12 +446,16 @@ handle_message(mavlink_message_t *msg) hil_sensors.baro_alt_meter = h; hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.accelerometer_counter = hil_counter; + /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); // increment counters - hil_counter += 1 ; - hil_frames += 1 ; + hil_counter++; + hil_frames++; // output if ((timestamp - old_timestamp) > 10000000) { -- cgit v1.2.3 From a817dedf11f60cafee262e81531208a260b7f13a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Apr 2013 14:39:49 +0200 Subject: Fixed bug in MAVLink HIL interface, now consistent with in-flight results in the field --- apps/mavlink/mavlink_receiver.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 588b3e07e..14631d503 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -485,13 +485,17 @@ handle_message(mavlink_message_t *msg) // hil_gps.counter_pos_valid = hil_counter++; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m - hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance? - hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.s_variance_m_s = 5.0f; + hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + + /* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F; + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); hil_gps.vel_d_m_s = 0.0f; - hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad + /* COG (course over ground) is speced as 0..360 degrees (compass) */ + hil_gps.cog_rad = cog_rad + M_PI_F; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; -- cgit v1.2.3 From 2b2c3f135dd13dda0ab9664efd6fa7ac86ee9563 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Apr 2013 14:56:13 +0200 Subject: Hotfix: Build fix --- apps/mavlink/mavlink_receiver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 14631d503..a30f0bf3c 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -495,7 +495,7 @@ handle_message(mavlink_message_t *msg) hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); hil_gps.vel_d_m_s = 0.0f; /* COG (course over ground) is speced as 0..360 degrees (compass) */ - hil_gps.cog_rad = cog_rad + M_PI_F; // from deg*100 to rad + hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; -- cgit v1.2.3 From 873b0f40b5934194f83f360f7193f48762da177d Mon Sep 17 00:00:00 2001 From: yvestroxler Date: Mon, 22 Apr 2013 19:44:40 +0300 Subject: Update ubx.cpp GPS didn't work before this change was made --- apps/drivers/gps/ubx.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 74cbc5aaf..0d4894b8d 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -113,13 +113,15 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ + receive(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* no ack is ecpected here, keep going configuring */ - /* send a CFT-RATE message to define update rate */ type_gps_bin_cfg_rate_packet_t cfg_rate_packet; memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); -- cgit v1.2.3 From ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Apr 2013 17:25:42 +0200 Subject: HIL bugfixing --- apps/drivers/gps/ubx.cpp | 18 ++++++++++----- apps/examples/kalman_demo/KalmanNav.cpp | 2 ++ apps/mavlink/mavlink_receiver.c | 14 +++++++----- apps/uORB/topics/vehicle_gps_position.h | 39 +++++++++++++++++---------------- 4 files changed, 43 insertions(+), 30 deletions(-) diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 0d4894b8d..c150f5271 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -33,7 +33,14 @@ * ****************************************************************************/ -/* @file U-Blox protocol implementation */ +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + */ #include #include @@ -633,16 +640,17 @@ UBX::handle_message() } case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); if (!_waiting_for_ack) { + /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); } diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 6df454a55..4ef150da1 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt) float LDot = vN / R; float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; + + // XXX position prediction using speed float vNDot = fN - vE * rotRate * sinL + vD * LDot; float vDDot = fD - vE * rotRate * cosL - diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index a30f0bf3c..22c2fcdad 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -477,25 +477,27 @@ handle_message(mavlink_message_t *msg) /* gps */ hil_gps.timestamp_position = gps.time_usec; -// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; -// hil_gps.counter_pos_valid = hil_counter++; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - /* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */ - float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F; + /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ + if (heading_rad > M_PI_F) + heading_rad -= 2.0f * M_PI_F; hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); hil_gps.vel_d_m_s = 0.0f; - /* COG (course over ground) is speced as 0..360 degrees (compass) */ - hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad + hil_gps.vel_ned_valid = true; + /* COG (course over ground) is speced as -PI..+PI */ + hil_gps.cog_rad = heading_rad; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index 5463a460d..0a7fb4e9d 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,38 +55,39 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + float p_variance_m; /**< position accuracy estimate m */ + float c_variance_rad; /**< course accuracy estimate rad */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ uint8_t satellite_prn[20]; /**< Global satellite ID */ uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** -- cgit v1.2.3 From afbb4d55b3fc4e0e2a1fc1a1b052e9f4fb034d51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 15:13:47 +0200 Subject: Finished conversion to C++ --- apps/attitude_estimator_ekf/Makefile | 5 +- .../attitude_estimator_ekf_main.c | 465 -------------------- .../attitude_estimator_ekf_main.cpp | 472 +++++++++++++++++++++ 3 files changed, 475 insertions(+), 467 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c create mode 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 734af7cc1..46a54c660 100755 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -35,8 +35,9 @@ APPNAME = attitude_estimator_ekf PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 -CSRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c deleted file mode 100755 index bd972f03f..000000000 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * - * 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 attitude_estimator_ekf_main.c - * - * Extended Kalman Filter for Attitude Estimation. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" -#include "attitude_estimator_ekf_params.h" - -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ - -/** - * Mainloop of attitude_estimator_ekf. - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The attitude_estimator_ekf app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int attitude_estimator_ekf_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("attitude_estimator_ekf already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12400, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); - - } else { - printf("\tattitude_estimator_ekf app not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/* - * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - -/* - * EKF Attitude Estimator main function. - * - * Estimates the attitude recursively once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]) -{ - -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ - float x_aposteriori_k[12]; /**< states */ - float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - - float x_aposteriori[12]; - float P_aposteriori[144]; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* Initialize filter */ - attitudeKalmanfilter_initialize(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - - - int loopcounter = 0; - int printcounter = 0; - - thread_running = true; - - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - - /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_ekf_params ekf_params; - - struct attitude_estimator_ekf_param_handles ekf_param_handles; - - /* initialize parameter handles */ - parameters_init(&ekf_param_handles); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); - - if (!state.flag_hil_enabled) { - fprintf(stderr, - "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); - } - - } else { - - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&ekf_param_handles, &ekf_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } - - } else { - - perf_begin(ekf_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - uint64_t timing_start = hrt_absolute_time(); - - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - continue; - } - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; - - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; - att.rollacc = x_aposteriori[3]; - att.pitchacc = x_aposteriori[4]; - att.yawacc = x_aposteriori[5]; - - //att.yawspeed =z_k[2] ; - /* copy offsets */ - memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); - att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - } - - perf_end(ekf_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp new file mode 100755 index 000000000..1a50dde0f --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -0,0 +1,472 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Lorenz Meier + * + * 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 attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of attitude_estimator_ekf. + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The attitude_estimator_ekf app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_ekf already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12400, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tattitude_estimator_ekf app is running\n"); + + } else { + printf("\tattitude_estimator_ekf app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + + } else { + + perf_begin(ekf_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(ekf_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} -- cgit v1.2.3 From 556a017444b809c18e2ce495a2fd00380960e0f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 17:41:46 +0200 Subject: Hotfix: GPS MAVLink transmission fixes --- apps/mavlink/orb_listener.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 1dd3fc2d8..5f15facf8 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l) /* copy gps data into local buffer */ orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) + cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; + + /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps.timestamp_position, @@ -236,13 +243,14 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - (uint16_t)(gps.eph_m * 1e2f), // from m to cm - (uint16_t)(gps.epv_m * 1e2f), // from m to cm - (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s - (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 + gps.eph_m * 1e2f, // from m to cm + gps.epv_m * 1e2f, // from m to cm + gps.vel_m_s * 1e2f, // from m/s to cm/s + cog_deg * 1e2f, // from rad to deg * 100 gps.satellites_visible); - if (gps.satellite_info_available && (gps_counter % 4 == 0)) { + /* update SAT info every 10 seconds */ + if (gps.satellite_info_available && (gps_counter % 50 == 0)) { mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.satellites_visible, gps.satellite_prn, -- cgit v1.2.3