From 67e45844073717d4344e4772d7b838aea2b8a24f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Oct 2012 12:45:07 +0200 Subject: Deleted old cruft --- apps/attitude_estimator_ekf/attitudeKalmanfilter.m | 108 ------- .../attitudeKalmanfilter.prj | 270 ----------------- .../codegen/position_estimator.c | 261 ----------------- .../codegen/position_estimator.h | 32 -- .../codegen/position_estimator_initialize.c | 31 -- .../codegen/position_estimator_initialize.h | 32 -- .../codegen/position_estimator_terminate.c | 31 -- .../codegen/position_estimator_terminate.h | 32 -- .../codegen/position_estimator_types.h | 16 - apps/position_estimator/codegen/rtGetInf.c | 139 --------- apps/position_estimator/codegen/rtGetInf.h | 23 -- apps/position_estimator/codegen/rtGetNaN.c | 96 ------ apps/position_estimator/codegen/rtGetNaN.h | 21 -- apps/position_estimator/codegen/rt_nonfinite.c | 87 ------ apps/position_estimator/codegen/rt_nonfinite.h | 53 ---- apps/position_estimator/codegen/rtwtypes.h | 175 ----------- apps/position_estimator/position_estimator.m | 62 ---- apps/position_estimator/position_estimator.prj | 269 ----------------- apps/px4/attitude_estimator_bm/.context | 0 apps/px4/attitude_estimator_bm/Makefile | 42 --- apps/px4/attitude_estimator_bm/attitude_bm.c | 322 --------------------- apps/px4/attitude_estimator_bm/attitude_bm.h | 24 -- .../attitude_estimator_bm/attitude_estimator_bm.c | 283 ------------------ apps/px4/attitude_estimator_bm/kalman.c | 115 -------- apps/px4/attitude_estimator_bm/kalman.h | 35 --- apps/px4/attitude_estimator_bm/matrix.h | 156 ---------- apps/px4/ground_estimator/Makefile | 42 --- apps/px4/ground_estimator/ground_estimator.c | 180 ------------ nuttx/configs/px4fmu/nsh/appconfig | 2 - 29 files changed, 2939 deletions(-) delete mode 100644 apps/attitude_estimator_ekf/attitudeKalmanfilter.m delete mode 100644 apps/attitude_estimator_ekf/attitudeKalmanfilter.prj delete mode 100644 apps/position_estimator/codegen/position_estimator.c delete mode 100644 apps/position_estimator/codegen/position_estimator.h delete mode 100644 apps/position_estimator/codegen/position_estimator_initialize.c delete mode 100644 apps/position_estimator/codegen/position_estimator_initialize.h delete mode 100644 apps/position_estimator/codegen/position_estimator_terminate.c delete mode 100644 apps/position_estimator/codegen/position_estimator_terminate.h delete mode 100644 apps/position_estimator/codegen/position_estimator_types.h delete mode 100644 apps/position_estimator/codegen/rtGetInf.c delete mode 100644 apps/position_estimator/codegen/rtGetInf.h delete mode 100644 apps/position_estimator/codegen/rtGetNaN.c delete mode 100644 apps/position_estimator/codegen/rtGetNaN.h delete mode 100644 apps/position_estimator/codegen/rt_nonfinite.c delete mode 100644 apps/position_estimator/codegen/rt_nonfinite.h delete mode 100644 apps/position_estimator/codegen/rtwtypes.h delete mode 100644 apps/position_estimator/position_estimator.m delete mode 100644 apps/position_estimator/position_estimator.prj delete mode 100644 apps/px4/attitude_estimator_bm/.context delete mode 100644 apps/px4/attitude_estimator_bm/Makefile delete mode 100644 apps/px4/attitude_estimator_bm/attitude_bm.c delete mode 100644 apps/px4/attitude_estimator_bm/attitude_bm.h delete mode 100644 apps/px4/attitude_estimator_bm/attitude_estimator_bm.c delete mode 100644 apps/px4/attitude_estimator_bm/kalman.c delete mode 100644 apps/px4/attitude_estimator_bm/kalman.h delete mode 100644 apps/px4/attitude_estimator_bm/matrix.h delete mode 100644 apps/px4/ground_estimator/Makefile delete mode 100644 apps/px4/ground_estimator/ground_estimator.c diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m deleted file mode 100644 index 5fb4aa94f..000000000 --- a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m +++ /dev/null @@ -1,108 +0,0 @@ -function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) -%#codegen - - -%Extended Attitude Kalmanfilter -% - %state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' - %measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' - %knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] - % - %[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - % - %Example.... - % - % $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ - - - %%define the matrices - acc_ProcessNoise=knownConst(1); - mag_ProcessNoise=knownConst(2); - ratesOffset_ProcessNoise=knownConst(3); - rates_ProcessNoise=knownConst(4); - - - acc_MeasurementNoise=knownConst(5); - mag_MeasurementNoise=knownConst(6); - rates_MeasurementNoise=knownConst(7); - - %process noise covariance matrix - Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); - zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); - zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); - zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; - - %measurement noise covariance matrix - R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); - zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); - zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; - - - %observation matrix - H_k=[ eye(3), zeros(3), zeros(3), zeros(3); - zeros(3), eye(3), zeros(3), zeros(3); - zeros(3), zeros(3), eye(3), eye(3)]; - - %compute A(t,w) - - %x_aposteriori_k[10,11,12] should be [p,q,r] - %R_temp=[1,-r, q - % r, 1, -p - % -q, p, 1] - - R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); - dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); - -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; - - %strange, should not be transposed - A_pred=[R_temp', zeros(3), zeros(3), zeros(3); - zeros(3), R_temp', zeros(3), zeros(3); - zeros(3), zeros(3), eye(3), zeros(3); - zeros(3), zeros(3), zeros(3), eye(3)]; - - %%prediction step - x_apriori=A_pred*x_aposteriori_k; - - %linearization - acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); - -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); - dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; - - mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); - -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); - dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; - - A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; - zeros(3), R_temp', zeros(3), mag_temp_mat'; - zeros(3), zeros(3), eye(3), zeros(3); - zeros(3), zeros(3), zeros(3), eye(3)]; - - - P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; - - - %%update step - - y_k=z_k-H_k*x_apriori; - S_k=H_k*P_apriori*H_k'+R; - K_k=(P_apriori*H_k'/(S_k)); - - - x_aposteriori=x_apriori+K_k*y_k; - P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; - - - %%Rotation matrix generation - - earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); - earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); - earth_y=cross(earth_x,earth_z); - - Rot_matrix=[earth_x,earth_y,earth_z]; - - - - - - - diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj deleted file mode 100644 index 431ddb71e..000000000 --- a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj +++ /dev/null @@ -1,270 +0,0 @@ - - - - option.general.TargetLang.C - true - true - true - true - true - true - true - option.general.FilePartitionMethod.MapMFileToCFile - option.general.GlobalDataSyncMethod.SyncAlways - true - option.general.DynamicMemoryAllocation.Disabled - option.paths.working.project - - option.paths.build.project - - - true - false - true - true - - - - - - - - - - true - 64 - true - 10 - 200 - 4000 - 200000 - 10000 - option.general.TargetLang.C - MATLAB Embedded Coder Target - option.CCompilerOptimization.On - - true - make_rtw - default_tmf - true - option.general.FilePartitionMethod.MapMFileToCFile - true - option.general.DynamicMemoryAllocation.Disabled - option.paths.working.project - - option.paths.build.project - - - true - false - true - true - false - $M$N - $M$N - $M$N - $M$N - $M$N - $M$N - emxArray_$M$N - 32 - - - - - - - - - - false - C89/C90 (ANSI) - true - C99 (ISO) - false - true - true - false - option.ParenthesesLevel.Nominal - false - true - true - 64 - true - 10 - 200 - 4000 - 200000 - 10000 - true - attitudeKalmanfilter_mex - attitudeKalmanfilter - option.target.artifact.lib - false - true - R2011a - true - - F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - single - 1 x 1 - - - false - - - single - 9 x 1 - - - false - - - single - 12 x 1 - - - false - - - single - 12 x 12 - - - false - - - single - 7 x 1 - - - false - - - - - - - - C:\Program Files\MATLAB\R2011a - - - false - false - true - false - false - false - false - false - 6.1 - false - true - win64 - true - - - - diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c deleted file mode 100644 index 731ae03e3..000000000 --- a/apps/position_estimator/codegen/position_estimator.c +++ /dev/null @@ -1,261 +0,0 @@ -/* - * position_estimator.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T - xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T - predict_only, real32_T xapo1[6], real32_T Papo1[36]) -{ - real32_T fv0[6]; - real32_T fv1[6]; - real32_T I[36]; - real32_T xapri[6]; - int32_T i; - int32_T r1; - static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.004F, 1.0F }; - - static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F, - -0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F }; - - int32_T r2; - real32_T Papri[36]; - real32_T maxval; - static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - real32_T K[18]; - static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0 }; - - real32_T fv6[9]; - static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 1, 0 }; - - real32_T b_gps_covariance[9]; - real32_T A[9]; - real32_T B[18]; - int32_T r3; - real32_T a21; - real32_T Y[18]; - real32_T b_z[3]; - int8_T b_I[36]; - - /* if predit_onli == 1: no update step: use this when no new gps data is available */ - /* %initialization */ - /* use model F=m*a x''=F/m */ - /* 250Hz---> dT = 0.004s */ - /* u=[phi;theta] */ - /* x=[px;vx;py;vy]; */ - /* %------------------------------------------ */ - /* %------------------------------------------------ */ - /* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */ - /* process Covariance Matrix */ - /* measurement Covariance Matrix */ - /* %prediction */ - for (i = 0; i < 6; i++) { - fv0[i] = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - fv0[i] += fv2[i + 6 * r1] * xapo[r1]; - } - - fv1[i] = 0.0F; - for (r1 = 0; r1 < 2; r1++) { - fv1[i] += fv3[i + 6 * r1] * u[r1]; - } - - xapri[i] = fv0[i] + fv1[i]; - for (r1 = 0; r1 < 6; r1++) { - I[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1]; - } - - Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1]; - } - } - - if (1 != predict_only) { - /* update */ - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - fv6[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - b_gps_covariance[0] = gps_covariance[0]; - b_gps_covariance[1] = 0.0F; - b_gps_covariance[2] = 0.0F; - b_gps_covariance[3] = 0.0F; - b_gps_covariance[4] = gps_covariance[1]; - b_gps_covariance[5] = 0.0F; - b_gps_covariance[6] = 0.0F; - b_gps_covariance[7] = 0.0F; - b_gps_covariance[8] = gps_covariance[2]; - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i]; - } - - for (r1 = 0; r1 < 6; r1++) { - B[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(A[0]); - a21 = (real32_T)fabs(A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - A[r2] /= A[r1]; - A[r3] /= A[r1]; - A[3 + r2] -= A[r2] * A[3 + r1]; - A[3 + r3] -= A[r3] * A[3 + r1]; - A[6 + r2] -= A[r2] * A[6 + r1]; - A[6 + r3] -= A[r3] * A[6 + r1]; - if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) { - i = r2; - r2 = r3; - r3 = i; - } - - A[3 + r3] /= A[3 + r2]; - A[6 + r3] -= A[3 + r3] * A[6 + r2]; - for (i = 0; i < 6; i++) { - Y[3 * i] = B[r1 + 3 * i]; - Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2]; - Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 + - r3]; - Y[2 + 3 * i] /= A[6 + r3]; - Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1]; - Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2]; - Y[1 + 3 * i] /= A[3 + r2]; - Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1]; - Y[3 * i] /= A[r1]; - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[r1 + 6 * i] = Y[i + 3 * r1]; - } - } - - for (i = 0; i < 3; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1]; - } - - b_z[i] = z[i] - maxval; - } - - for (i = 0; i < 6; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - maxval += K[i + 6 * r1] * b_z[r1]; - } - - xapo1[i] = xapri[i] + maxval; - } - - for (i = 0; i < 36; i++) { - b_I[i] = 0; - } - - for (i = 0; i < 6; i++) { - b_I[i + 6 * i] = 1; - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 3; r2++) { - maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1]; - } - - I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval; - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - Papo1[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1]; - } - } - } - } else { - memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T)); - for (i = 0; i < 6; i++) { - xapo1[i] = xapri[i]; - } - } -} - -/* End of code generation (position_estimator.c) */ diff --git a/apps/position_estimator/codegen/position_estimator.h b/apps/position_estimator/codegen/position_estimator.h deleted file mode 100644 index 5e2c9acd8..000000000 --- a/apps/position_estimator/codegen/position_estimator.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_H__ -#define __POSITION_ESTIMATOR_H__ -/* Include files */ -#include -#include -#include -#include - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]); -#endif -/* End of code generation (position_estimator.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.c b/apps/position_estimator/codegen/position_estimator_initialize.c deleted file mode 100644 index 862381d3b..000000000 --- a/apps/position_estimator/codegen/position_estimator_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * position_estimator_initialize.c - * - * Code generation for function 'position_estimator_initialize' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" -#include "position_estimator_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (position_estimator_initialize.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.h b/apps/position_estimator/codegen/position_estimator_initialize.h deleted file mode 100644 index 71013b390..000000000 --- a/apps/position_estimator/codegen/position_estimator_initialize.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator_initialize.h - * - * Code generation for function 'position_estimator_initialize' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_INITIALIZE_H__ -#define __POSITION_ESTIMATOR_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator_initialize(void); -#endif -/* End of code generation (position_estimator_initialize.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.c b/apps/position_estimator/codegen/position_estimator_terminate.c deleted file mode 100644 index b25aabf40..000000000 --- a/apps/position_estimator/codegen/position_estimator_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * position_estimator_terminate.c - * - * Code generation for function 'position_estimator_terminate' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" -#include "position_estimator_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (position_estimator_terminate.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.h b/apps/position_estimator/codegen/position_estimator_terminate.h deleted file mode 100644 index d9fcf838b..000000000 --- a/apps/position_estimator/codegen/position_estimator_terminate.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator_terminate.h - * - * Code generation for function 'position_estimator_terminate' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_TERMINATE_H__ -#define __POSITION_ESTIMATOR_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator_terminate(void); -#endif -/* End of code generation (position_estimator_terminate.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_types.h b/apps/position_estimator/codegen/position_estimator_types.h deleted file mode 100644 index cb01c7426..000000000 --- a/apps/position_estimator/codegen/position_estimator_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * position_estimator_types.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_TYPES_H__ -#define __POSITION_ESTIMATOR_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (position_estimator_types.h) */ diff --git a/apps/position_estimator/codegen/rtGetInf.c b/apps/position_estimator/codegen/rtGetInf.c deleted file mode 100644 index 20a64117c..000000000 --- a/apps/position_estimator/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/apps/position_estimator/codegen/rtGetInf.h b/apps/position_estimator/codegen/rtGetInf.h deleted file mode 100644 index c74f1fc28..000000000 --- a/apps/position_estimator/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.c b/apps/position_estimator/codegen/rtGetNaN.c deleted file mode 100644 index 6aa2f4639..000000000 --- a/apps/position_estimator/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.h b/apps/position_estimator/codegen/rtGetNaN.h deleted file mode 100644 index d3ec61c9e..000000000 --- a/apps/position_estimator/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.c b/apps/position_estimator/codegen/rt_nonfinite.c deleted file mode 100644 index 40e15fd22..000000000 --- a/apps/position_estimator/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.h b/apps/position_estimator/codegen/rt_nonfinite.h deleted file mode 100644 index ac9660124..000000000 --- a/apps/position_estimator/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#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/codegen/rtwtypes.h b/apps/position_estimator/codegen/rtwtypes.h deleted file mode 100644 index e87ae1fdc..000000000 --- a/apps/position_estimator/codegen/rtwtypes.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 64 native word size: 64 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef long int64_T; -typedef unsigned long uint64_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - typedef struct { - int64_T re; - int64_T im; - } cint64_T; - - typedef struct { - uint64_T re; - uint64_T im; - } cuint64_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) -#define MAX_int64_T ((int64_T)(9223372036854775807L)) -#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L)) -#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL)) -#define MIN_uint64_T ((uint64_T)(0UL)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/apps/position_estimator/position_estimator.m b/apps/position_estimator/position_estimator.m deleted file mode 100644 index 2ef4d8b06..000000000 --- a/apps/position_estimator/position_estimator.m +++ /dev/null @@ -1,62 +0,0 @@ -function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available -%#codegen -%%initialization -%use model F=m*a x''=F/m -% 250Hz---> dT = 0.004s -%u=[phi;theta] -%x=[px;vx;py;vy]; -%%------------------------------------------ -dT=0.004; -%%------------------------------------------------ - -%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; - - -F=[ 1, 0.004, 0, 0, 0, 0; - 0, 1, 0, 0, 0, 0; - 0, 0, 1, 0.004, 0, 0; - 0, 0, 0, 1, 0, 0; - 0, 0, 0, 0, 1, 0.004; - 0, 0, 0, 0, 0, 1]; - -B=[ 0, -0.1744; - 0, -87.2; - 0.1744, 0; - 87.2, 0; - 0, 0; - 0, 0]; - -H=[1,0,0,0,0,0; - 0,0,1,0,0,0; - 0,0,0,0,1,0]; - - - - Q=[1e-007 ,0 ,0 ,0 ,0 ,0; - 0 ,1 ,0 ,0 ,0 ,0; - 0 ,0 ,1e-007 ,0 ,0 ,0; - 0 ,0 ,0 ,1 ,0 ,0 - 0 ,0 ,0 ,0 ,1e-007 ,0; - 0 ,0 ,0 ,0 ,0 ,1]; %process Covariance Matrix - - -R=[gps_covariance(1), 0, 0; - 0, gps_covariance(2), 0; - 0, 0, gps_covariance(3)]; %measurement Covariance Matrix - -%%prediction - -xapri=F*xapo+B*u; -Papri=F*Papo*F'+Q; - -if 1 ~= predict_only - %update - yr=z-H*xapri; - S=H*Papri*H'+R; - K=(Papri*H')/S; - xapo1=xapri+K*yr; - Papo1=(eye(6)-K*H)*Papri; -else - Papo1=Papri; - xapo1=xapri; -end \ No newline at end of file diff --git a/apps/position_estimator/position_estimator.prj b/apps/position_estimator/position_estimator.prj deleted file mode 100644 index d754f36cc..000000000 --- a/apps/position_estimator/position_estimator.prj +++ /dev/null @@ -1,269 +0,0 @@ - - - - option.general.TargetLang.C - true - true - false - true - true - true - true - option.general.FilePartitionMethod.MapMFileToCFile - option.general.GlobalDataSyncMethod.SyncAlways - true - option.general.DynamicMemoryAllocation.Disabled - option.paths.working.project - - option.paths.build.project - - - true - false - true - true - - - - - - - - - - true - 64 - true - 10 - 200 - 4000 - 200000 - 10000 - option.general.TargetLang.C - MATLAB Embedded Coder Target - option.CCompilerOptimization.Off - - false - make_rtw - default_tmf - true - option.general.FilePartitionMethod.MapMFileToCFile - true - option.general.DynamicMemoryAllocation.Disabled - option.paths.working.project - - option.paths.build.specified - ./codegen - - false - false - true - false - false - $M$N - $M$N - $M$N - $M$N - $M$N - $M$N - emxArray_$M$N - 32 - - - - - - - - - - false - C89/C90 (ANSI) - true - C89/C90 (ANSI) - false - true - true - false - option.ParenthesesLevel.Nominal - false - true - true - 64 - true - 10 - 200 - 4000 - 200000 - 10000 - true - position_estimator_mex - position_estimator - option.target.artifact.lib - true - true - R2011a - true - /home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - single - 2 x 1 - - - false - - - single - 3 x 1 - - - false - - - single - 6 x 1 - - - false - - - single - 6 x 6 - - - false - - - single - 3 x 1 - - - false - - - uint8 - 1 x 1 - - - false - - - - - - - - - /home/thomasgubler/tools/matlab - - - true - false - false - false - false - false - true - false - 3.2.0-25-generic - false - true - glnxa64 - true - - - - diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile deleted file mode 100644 index 2c1cfc510..000000000 --- a/apps/px4/attitude_estimator_bm/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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 black magic attitude estimator -# - -APPNAME = attitude_estimator_bm -PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 12000 - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c deleted file mode 100644 index 6921db375..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ /dev/null @@ -1,322 +0,0 @@ - -/* - * attitude_bm.c - * - * Created on: 21.12.2010 - * Author: Laurens Mackay, Tobias Naegeli - */ - -#include -#include "attitude_bm.h" -#include "kalman.h" - - -#define TIME_STEP (1.0f / 500.0f) - -static kalman_t attitude_blackmagic_kal; - -void vect_norm(float_vect3 *vect) -{ - float length = sqrtf( - vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); - - if (length != 0) { - vect->x /= length; - vect->y /= length; - vect->z /= length; - } -} - - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, - float_vect3 *c) -{ - c->x = a->y * b->z - a->z * b->y; - c->y = a->z * b->x - a->x * b->z; - c->z = a->x * b->y - a->y * b->x; -} - -void attitude_blackmagic_update_a(void) -{ - // for acc - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - // for mag - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - -} - -void attitude_blackmagic_init(void) -{ - //X Kalmanfilter - //initalize matrices - - static m_elem kal_a[12 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f - }; - - static m_elem kal_c[9 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f - }; - - - -#define FACTOR 0.5 -#define FACTORstart 1 - - -// static m_elem kal_gain[12 * 9] = -// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, -// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, -// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, -// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 -// }; - - static m_elem kal_gain[12 * 9] = { - 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, - 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, - -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, - 0, 0 , 0 , 0, 0, 0, 0, 0, 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f - }; - //offset update only correct if not upside down. - -#define K (10.0f*TIME_STEP) - - static m_elem kal_gain_start[12 * 9] = { - K, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, K, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, K, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, K, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, K, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, K, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, K, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, K, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, K, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0 - }; - - - - static m_elem kal_x_apriori[12 * 1] = - { }; - - - //---> initial states sind aposteriori!? ---> fehler - static m_elem kal_x_aposteriori[12 * 1] = - { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; - - kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, - kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); - -} - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) -{ - // Kalman Filter - - //Calculate new linearized A matrix - attitude_blackmagic_update_a(); - - kalman_predict(&attitude_blackmagic_kal); - - //correction update - - m_elem measurement[9] = - { }; - m_elem mask[9] = - { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; - - // XXX Hack - stop updating accel if upside down - - if (accel->z > 0) { - mask[0] = 0.0f; - mask[1] = 0.0f; - mask[2] = 0.0f; - } else { - mask[0] = 1.0f; - mask[1] = 1.0f; - mask[2] = 1.0f; - } - - measurement[0] = accel->x; - measurement[1] = accel->y; - measurement[2] = accel->z; - - measurement[3] = mag->x; - measurement[4] = mag->y; - measurement[5] = mag->z; - - measurement[6] = gyro->x; - measurement[7] = gyro->y; - measurement[8] = gyro->z; - - //Put measurements into filter - - -// static int j = 0; -// if (j >= 3) -// { -// j = 0; -// -// mask[3]=1; -// mask[4]=1; -// mask[5]=1; -// j=0; -// -// }else{ -// j++;} - - kalman_correct(&attitude_blackmagic_kal, measurement, mask); - -} -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - //debug - - // save outputs - float_vect3 kal_acc; - float_vect3 kal_mag; -// float_vect3 kal_w0, kal_w; - - kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); - kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); - kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); - - kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); - kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); - kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); - -// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); -// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); -// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); -// -// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); -// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); -// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); - - rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); - rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); - rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); - - - -// kal_w = kal_w; // XXX hack to silence compiler warning -// kal_w0 = kal_w0; // XXX hack to silence compiler warning - - - - //debug_vect("magn", mag); - - //float_vect3 x_n_b, y_n_b, z_n_b; - z_n_b->x = -kal_acc.x; - z_n_b->y = -kal_acc.y; - z_n_b->z = -kal_acc.z; - vect_norm(z_n_b); - vect_cross_product(z_n_b, &kal_mag, y_n_b); - vect_norm(y_n_b); - - vect_cross_product(y_n_b, z_n_b, x_n_b); - - - - //save euler angles - euler->x = atan2f(z_n_b->y, z_n_b->z); - euler->y = -asinf(z_n_b->x); - euler->z = atan2f(y_n_b->x, x_n_b->x); - -} diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h deleted file mode 100644 index c21b3d6f1..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * attitude_blackmagic.h - * - * Created on: May 31, 2011 - * Author: pixhawk - */ - -#ifndef attitude_blackmagic_H_ -#define attitude_blackmagic_H_ - -#include "matrix.h" - -void vect_norm(float_vect3 *vect); - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c); - -void attitude_blackmagic_update_a(void); - -void attitude_blackmagic_init(void); - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro); - -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); -#endif /* attitude_blackmagic_H_ */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c deleted file mode 100644 index 60737a654..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ /dev/null @@ -1,283 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Laurens Mackay - * - * 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_bm.c - * Black Magic Attitude Estimator - */ - - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "attitude_bm.h" - -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds - -__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * user_start - ****************************************************************************/ -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); - -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - float_vect3 gyro_values; - gyro_values.x = raw->gyro_rad_s[0]; - gyro_values.y = raw->gyro_rad_s[1]; - gyro_values.z = raw->gyro_rad_s[2]; - - float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; - - float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*1500.0f; - mag_values.y = raw->magnetometer_ga[1]*1500.0f; - mag_values.z = raw->magnetometer_ga[2]*1500.0f; - - // static int i = 0; - - // if (i == 500) { - // printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", - // gyro_values.x, gyro_values.y, gyro_values.z, - // accel_values.x, accel_values.y, accel_values.z, - // mag_values.x, mag_values.y, mag_values.z); - // i = 0; - // } - // i++; - - attitude_blackmagic(&accel_values, &mag_values, &gyro_values); - - /* read out values */ - attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); - - return OK; -} - - -int attitude_estimator_bm_main(int argc, char *argv[]) -{ - // print text - printf("Black Magic Attitude Estimator initialized..\n\n"); - fflush(stdout); - - /* data structures to read euler angles and rotation matrix back */ - float_vect3 euler = {.x = 0, .y = 0, .z = 0}; - float_vect3 rates; - float_vect3 x_n_b; - float_vect3 y_n_b; - float_vect3 z_n_b; - - int overloadcounter = 19; - - /* initialize */ - attitude_blackmagic_init(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; - struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, - .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, - .R = {0}, .timestamp = 0}; - - uint64_t last_data = 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, 5); - - bool hil_enabled = false; - bool publishing = false; - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - publishing = true; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - /* subscribe to system status */ - struct vehicle_status_s vstatus = {0}; - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - unsigned int loopcounter = 0; - - uint64_t last_checkstate_stamp = 0; - - /* Main loop*/ - while (true) { - - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - //#if 0 - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - overloadcounter = 0; - } - - overloadcounter++; - } - - //#endif - // now = hrt_absolute_time(); - /* filter values */ - attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); - - // time_elapsed = hrt_absolute_time() - now; - // if (blubb == 20) - // { - // printf("Estimator: %lu\n", time_elapsed); - // blubb = 0; - // } - // blubb++; - - // if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); - - // printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); - // last_data = sensor_combined_s_local.timestamp; - - /*correct yaw */ - // euler.z = euler.z + M_PI; - - /* send out */ - - att.timestamp = sensor_combined_s_local.timestamp; - att.roll = euler.x; - att.pitch = euler.y; - att.yaw = euler.z; - - if (att.yaw > M_PI_F) { - att.yaw -= 2.0f * M_PI_F; - } - - if (att.yaw < -M_PI_F) { - att.yaw += 2.0f * M_PI_F; - } - - att.rollspeed = rates.x; - att.pitchspeed = rates.y; - att.yawspeed = rates.z; - - att.R[0][0] = x_n_b.x; - att.R[0][1] = x_n_b.y; - att.R[0][2] = x_n_b.z; - - // Broadcast - if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - } - - - // XXX add remaining entries - - if (hrt_absolute_time() - last_checkstate_stamp > 500000) { - /* Check HIL state */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* switching from non-HIL to HIL mode */ - //printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !hil_enabled) { - hil_enabled = true; - publishing = false; - int ret = close(pub_att); - printf("Closing attitude: %i \n", ret); - - /* switching from HIL to non-HIL mode */ - - } else if (!publishing && !hil_enabled) { - /* advertise the topic and make the initial publication */ - pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - hil_enabled = false; - publishing = true; - } - last_checkstate_stamp = hrt_absolute_time(); - } - - loopcounter++; - } - - /* Should never reach here */ - return 0; -} - - diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c deleted file mode 100644 index e4ea7a417..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.c +++ /dev/null @@ -1,115 +0,0 @@ -/* - * kalman.c - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ -#include "kalman.h" -//#include "mavlink_debug.h" - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps) -{ - kalman->states = states; - kalman->measurements = measurements; - kalman->gainfactorsteps = gainfactorsteps; - kalman->gainfactor = 0; - - //Create all matrices that are persistent - kalman->a = matrix_create(states, states, a); - kalman->c = matrix_create(measurements, states, c); - kalman->gain_start = matrix_create(states, measurements, gain_start); - kalman->gain = matrix_create(states, measurements, gain); - kalman->x_apriori = matrix_create(states, 1, x_apriori); - kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori); -} - -void kalman_predict(kalman_t *kalman) -{ - matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori); -} - -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]) -{ - //create matrices from inputs - matrix_t measurement = - matrix_create(kalman->measurements, 1, measurement_a); - matrix_t mask = matrix_create(kalman->measurements, 1, mask_a); - - //create temporary matrices - m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_start_part = matrix_create(kalman->states, - kalman->measurements, gain_start_part_a); - - m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_part = matrix_create(kalman->states, kalman->measurements, - gain_part_a); - - m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements, - gain_sum_a); - - m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t error = matrix_create(kalman->measurements, 1, error_a); - - m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t measurement_estimate = matrix_create(kalman->measurements, 1, - measurement_estimate_a); - - m_elem x_update_a[KALMAN_MAX_STATES * 1] = - { }; - matrix_t x_update = matrix_create(kalman->states, 1, x_update_a); - - - - //x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori); - - - //est=C*xapriori; - matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate); - //error=(z-C*xapriori) = measurement-estimate - matrix_sub(measurement, measurement_estimate, error); - matrix_mult_element(error, mask, error); - - kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f - / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps; - - - - matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part); - - matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start, - gain_start_part); - - matrix_add(gain_start_part, gain_part, gain_sum); - - //gain*(z-C*xapriori) - matrix_mult(gain_sum, error, x_update); - - //xaposteriori = xapriori + update - - matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori); - - -// static int i=0; -// if(i++==4){ -// i=0; -// float_vect3 out_kal; -// out_kal.x = M(gain_sum,0,1); -//// out_kal_z.x = z_measurement[1]; -// out_kal.y = M(gain_sum,1,1); -// out_kal.z = M(gain_sum,2,1); -// debug_vect("out_kal", out_kal); -// } -} - -m_elem kalman_get_state(kalman_t *kalman, int state) -{ - return M(kalman->x_aposteriori, state, 0); -} diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h deleted file mode 100644 index 0a6a18505..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * kalman.h - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ - -#ifndef KALMAN_H_ -#define KALMAN_H_ - -#include "matrix.h" - -#define KALMAN_MAX_STATES 12 -#define KALMAN_MAX_MEASUREMENTS 9 -typedef struct { - int states; - int measurements; - matrix_t a; - matrix_t c; - matrix_t gain_start; - matrix_t gain; - matrix_t x_apriori; - matrix_t x_aposteriori; - float gainfactor; - int gainfactorsteps; -} kalman_t; - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps); -void kalman_predict(kalman_t *kalman); -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]); -m_elem kalman_get_state(kalman_t *kalman, int state); - -#endif /* KALMAN_H_ */ diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h deleted file mode 100644 index 613a2d081..000000000 --- a/apps/px4/attitude_estimator_bm/matrix.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * matrix.h - * - * Created on: 18.11.2010 - * Author: Laurens Mackay - */ - -#ifndef MATRIX_H_ -#define MATRIX_H_ - -typedef float m_elem; - -typedef struct { - int rows; - int cols; - m_elem *a; -} matrix_t; - -typedef struct { - float x; - float y; - float z; -} float_vect3; - -#define M(m,i,j) m.a[m.cols*i+j] - -///* This is the datatype used for the math and non-type specific ops. */ -// -//matrix_t matrix_create(const int rows, const int cols, m_elem * a); -///* matrix C = matrix A + matrix B , both of size m x n */ -//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A - matrix B , all of size m x n */ -//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ -//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); -// -//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); -// -//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = A*B'*/ -//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); - - -static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) -{ - matrix_t ret; - ret.rows = rows; - ret.cols = cols; - ret.a = a; - return ret; -} - -static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_add: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) + M(b, i, j); - } - - } -} - -static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_sub: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) - M(b, i, j); - } - - } -} - -static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, k, j); - } - } - - } -} - -static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) -{ - - if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, j, k); - } - } - - } - -} - -static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols) { - //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = f * M(a, i, j); - } - - } -} - - -static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_mult_element: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) * M(b, i, j); - } - - } -} - - - -#endif /* MATRIX_H_ */ diff --git a/apps/px4/ground_estimator/Makefile b/apps/px4/ground_estimator/Makefile deleted file mode 100644 index b44d871c6..000000000 --- a/apps/px4/ground_estimator/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Basic example application -# - -APPNAME = ground_estimator -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c deleted file mode 100644 index ccf9ee3ec..000000000 --- a/apps/px4/ground_estimator/ground_estimator.c +++ /dev/null @@ -1,180 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User - * - * 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 ground_estimator.c - * ground_estimator application example for PX4 autopilot - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - - -static bool thread_should_exit = false; /**< ground_estimator exit flag */ -static bool thread_running = false; /**< ground_estimator status flag */ -static int ground_estimator_task; /**< Handle of ground_estimator task / thread */ - -/** - * ground_estimator management function. - */ -__EXPORT int ground_estimator_main(int argc, char *argv[]); - -/** - * Mainloop of ground_estimator. - */ -int ground_estimator_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -int ground_estimator_thread_main(int argc, char *argv[]) { - - printf("[ground_estimator] starting\n"); - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - float position[3] = {}; - float velocity[3] = {}; - - uint64_t last_time = 0; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - struct sensor_combined_s s; - orb_copy(ORB_ID(sensor_combined), sub_raw, &s); - - float dt = ((float)(s.timestamp - last_time)) / 1000000.0f; - - /* Integration */ - position[0] += velocity[0] * dt; - position[1] += velocity[1] * dt; - position[2] += velocity[2] * dt; - - velocity[0] += velocity[0] * 0.01f + 0.99f * s.accelerometer_m_s2[0] * dt; - velocity[1] += velocity[1] * 0.01f + 0.99f * s.accelerometer_m_s2[1] * dt; - velocity[2] += velocity[2] * 0.01f + 0.99f * s.accelerometer_m_s2[2] * dt; - - dbg.value = position[0]; - - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - } - - printf("[ground_estimator] exiting.\n"); - - return 0; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ground_estimator {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The ground_estimator 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 ground_estimator_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("ground_estimator already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - ground_estimator_task = task_create("ground_estimator", SCHED_PRIORITY_DEFAULT, 4096, ground_estimator_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tground_estimator is running\n"); - } else { - printf("\tground_estimator not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index cfd86aac1..1c717e904 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -76,11 +76,9 @@ CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf -CONFIGURED_APPS += px4/ground_estimator # Hacking tools #CONFIGURED_APPS += system/i2c -- cgit v1.2.3