dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Deleted old cruft

This commit is contained in:
Lorenz Meier 2012-10-26 12:45:07 +02:00
parent 5f01688490
commit 67e4584407
29 changed files with 0 additions and 2939 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
apps/attitude_estimator_ekf/attitudeKalmanfilter.m
View File

@ -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];

270
apps/attitude_estimator_ekf/attitudeKalmanfilter.prj
View File

@ -1,270 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project>
<configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="attitudeKalmanfilter" location="F:\codegenerationMatlabAttFilter" file="F:\codegenerationMatlabAttFilter\attitudeKalmanfilter.prj" build-checksum="2344418414">
<param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
<param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
<param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
<param.mex.general.EnableBLAS>true</param.mex.general.EnableBLAS>
<param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
<param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
<param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
<param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
<param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
<param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
<param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
<param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
<param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
<param.mex.paths.working.specified />
<param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
<param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
<param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
<param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomSourceCode />
<param.mex.customcode.CustomHeaderCode />
<param.mex.customcode.CustomInitializer />
<param.mex.customcode.CustomTerminator />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
<param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
<param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
<param.mex.InlineThreshold>10</param.mex.InlineThreshold>
<param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
<param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
<param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
<param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
<param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
<param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
<param.grt.CCompilerOptimization>option.CCompilerOptimization.On</param.grt.CCompilerOptimization>
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.GenerateMakefile>true</param.grt.general.GenerateMakefile>
<param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
<param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
<param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
<param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
<param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
<param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
<param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
<param.grt.paths.working.specified />
<param.grt.paths.build>option.paths.build.project</param.grt.paths.build>
<param.grt.paths.build.specified />
<param.grt.paths.search />
<param.grt.report.GenerateReport>true</param.grt.report.GenerateReport>
<param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
<param.grt.GenerateComments>true</param.grt.GenerateComments>
<param.grt.MATLABSourceComments>true</param.grt.MATLABSourceComments>
<param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
<param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
<param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
<param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
<param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
<param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
<param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
<param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
<param.grt.MaxIdLength>32</param.grt.MaxIdLength>
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomSourceCode />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose>false</param.grt.Verbose>
<param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
<param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
<param.ert.TargetFunctionLibrary>C99 (ISO)</param.ert.TargetFunctionLibrary>
<param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
<param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
<param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
<param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
<param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
<param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
<param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
<param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
<param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
<param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
<param.grt.InlineThreshold>10</param.grt.InlineThreshold>
<param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
<param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
<param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
<param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<param.mex.outputfile>attitudeKalmanfilter_mex</param.mex.outputfile>
<param.grt.outputfile>attitudeKalmanfilter</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.version>R2011a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml />
<param.grt.mainhtml>F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html</param.grt.mainhtml>
<unset>
<param.mex.general.TargetLang />
<param.mex.general.IntegrityChecks />
<param.mex.general.ResponsivenessChecks />
<param.mex.general.EnableBLAS />
<param.mex.general.ExtrinsicCalls />
<param.mex.general.EchoExpressions />
<param.mex.general.EnableDebugging />
<param.mex.general.SaturateOnIntegerOverflow />
<param.mex.general.FilePartitionMethod />
<param.mex.general.GlobalDataSyncMethod />
<param.mex.general.EnableVariableSizing />
<param.mex.general.DynamicMemoryAllocation />
<param.mex.paths.working />
<param.mex.paths.working.specified />
<param.mex.paths.build />
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport />
<param.mex.report.LaunchReport />
<param.mex.comments.GenerateComments />
<param.mex.comments.MATLABSourceComments />
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomSourceCode />
<param.mex.customcode.CustomHeaderCode />
<param.mex.customcode.CustomInitializer />
<param.mex.customcode.CustomTerminator />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy />
<param.mex.MemcpyThreshold />
<param.mex.InitFltsAndDblsToZero />
<param.mex.InlineThreshold />
<param.mex.InlineThresholdMax />
<param.mex.InlineStackLimit />
<param.mex.StackUsageMax />
<param.mex.ConstantFoldingTimeout />
<param.grt.general.TargetLang />
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.GenerateMakefile />
<param.grt.general.MakeCommand />
<param.grt.general.TemplateMakefile />
<param.grt.general.SaturateOnIntegerOverflow />
<param.grt.general.FilePartitionMethod />
<param.grt.general.EnableVariableSizing />
<param.grt.general.DynamicMemoryAllocation />
<param.grt.paths.working />
<param.grt.paths.working.specified />
<param.grt.paths.build />
<param.grt.paths.build.specified />
<param.grt.paths.search />
<param.grt.report.GenerateReport />
<param.grt.report.LaunchReport />
<param.grt.GenerateComments />
<param.ert.MATLABFcnDesc />
<param.ert.CustomSymbolStrGlobalVar />
<param.ert.CustomSymbolStrType />
<param.ert.CustomSymbolStrField />
<param.ert.CustomSymbolStrFcn />
<param.ert.CustomSymbolStrTmpVar />
<param.ert.CustomSymbolStrMacro />
<param.ert.CustomSymbolStrEMXArray />
<param.grt.MaxIdLength />
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose />
<param.grt.SupportNonFinite />
<param.ert.PurelyIntegerCode />
<param.ert.SupportNonFinite />
<param.ert.IncludeTerminateFcn />
<param.ert.MultiInstanceCode />
<param.ert.ParenthesesLevel />
<param.ert.ConvertIfToSwitch />
<param.ert.PreserveExternInFcnDecls />
<param.grt.EnableMemcpy />
<param.grt.MemcpyThreshold />
<param.grt.InitFltsAndDblsToZero />
<param.grt.InlineThreshold />
<param.grt.InlineThresholdMax />
<param.grt.InlineStackLimit />
<param.grt.StackUsageMax />
<param.grt.ConstantFoldingTimeout />
<param.UseECoderFeatures />
<param.mex.outputfile />
<param.grt.outputfile />
<param.mex.GenCodeOnly />
<param.version />
<param.HasECoderFeatures />
<param.mex.mainhtml />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}\attitudeKalmanfilter.m" custom-data-expanded="true">
<Inputs fileName="attitudeKalmanfilter.m" functionName="attitudeKalmanfilter">
<Input Name="dt">
<Class>single</Class>
<Size>1 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="z_k">
<Class>single</Class>
<Size>9 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="x_aposteriori_k">
<Class>single</Class>
<Size>12 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="P_aposteriori_k">
<Class>single</Class>
<Size>12 x 12</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="knownConst">
<Class>single</Class>
<Size>7 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.package />
<build-deliverables />
<matlab>
<root>C:\Program Files\MATLAB\R2011a</root>
</matlab>
<platform>
<unix>false</unix>
<mac>false</mac>
<windows>true</windows>
<win2k>false</win2k>
<winxp>false</winxp>
<vista>false</vista>
<linux>false</linux>
<solaris>false</solaris>
<osver>6.1</osver>
<os32>false</os32>
<os64>true</os64>
<arch>win64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>

261
apps/position_estimator/codegen/position_estimator.c
View File

@ -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) */

32
apps/position_estimator/codegen/position_estimator.h
View File

@ -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 <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "position_estimator_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]);
#endif
/* End of code generation (position_estimator.h) */

31
apps/position_estimator/codegen/position_estimator_initialize.c
View File

@ -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) */

32
apps/position_estimator/codegen/position_estimator_initialize.h
View File

@ -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 <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "position_estimator_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void position_estimator_initialize(void);
#endif
/* End of code generation (position_estimator_initialize.h) */

31
apps/position_estimator/codegen/position_estimator_terminate.c
View File

@ -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) */

32
apps/position_estimator/codegen/position_estimator_terminate.h
View File

@ -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 <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "position_estimator_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void position_estimator_terminate(void);
#endif
/* End of code generation (position_estimator_terminate.h) */

16
apps/position_estimator/codegen/position_estimator_types.h
View File

@ -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) */

139
apps/position_estimator/codegen/rtGetInf.c
View File

@ -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) */

23
apps/position_estimator/codegen/rtGetInf.h
View File

@ -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 <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetInf(void);
extern real32_T rtGetInfF(void);
extern real_T rtGetMinusInf(void);
extern real32_T rtGetMinusInfF(void);
#endif
/* End of code generation (rtGetInf.h) */

96
apps/position_estimator/codegen/rtGetNaN.c
View File

@ -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) */

21
apps/position_estimator/codegen/rtGetNaN.h
View File

@ -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 <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#endif
/* End of code generation (rtGetNaN.h) */

87
apps/position_estimator/codegen/rt_nonfinite.c
View File

@ -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) */

53
apps/position_estimator/codegen/rt_nonfinite.h
View File

@ -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 <float.h>
#endif
#include <stddef.h>
#include "rtwtypes.h"
extern real_T rtInf;
extern real_T rtMinusInf;
extern real_T rtNaN;
extern real32_T rtInfF;
extern real32_T rtMinusInfF;
extern real32_T rtNaNF;
extern void rt_InitInfAndNaN(size_t realSize);
extern boolean_T rtIsInf(real_T value);
extern boolean_T rtIsInfF(real32_T value);
extern boolean_T rtIsNaN(real_T value);
extern boolean_T rtIsNaNF(real32_T value);
typedef struct {
struct {
uint32_T wordH;
uint32_T wordL;
} words;
} BigEndianIEEEDouble;
typedef struct {
struct {
uint32_T wordL;
uint32_T wordH;
} words;
} LittleEndianIEEEDouble;
typedef struct {
union {
real32_T wordLreal;
uint32_T wordLuint;
} wordL;
} IEEESingle;
#endif
/* End of code generation (rt_nonfinite.h) */

175
apps/position_estimator/codegen/rtwtypes.h
View File

@ -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 <limits.h>
/*=======================================================================*
* Target hardware information
* Device type: Generic->MATLAB Host Computer
* Number of bits: char: 8 short: 16 int: 32
* long: 64 native word size: 64
* Byte ordering: LittleEndian
* Signed integer division rounds to: Zero
* Shift right on a signed integer as arithmetic shift: on
*=======================================================================*/
/*=======================================================================*
* Fixed width word size data types: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
* real32_T, real64_T - 32 and 64 bit floating point numbers *
*=======================================================================*/
typedef signed char int8_T;
typedef unsigned char uint8_T;
typedef short int16_T;
typedef unsigned short uint16_T;
typedef int int32_T;
typedef unsigned int uint32_T;
typedef long int64_T;
typedef unsigned long uint64_T;
typedef float real32_T;
typedef double real64_T;
/*===========================================================================*
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
* ulong_T, char_T and byte_T. *
*===========================================================================*/
typedef double real_T;
typedef double time_T;
typedef unsigned char boolean_T;
typedef int int_T;
typedef unsigned uint_T;
typedef unsigned long ulong_T;
typedef char char_T;
typedef char_T byte_T;
/*===========================================================================*
* Complex number type definitions *
*===========================================================================*/
#define CREAL_T
typedef struct {
real32_T re;
real32_T im;
} creal32_T;
typedef struct {
real64_T re;
real64_T im;
} creal64_T;
typedef struct {
real_T re;
real_T im;
} creal_T;
typedef struct {
int8_T re;
int8_T im;
} cint8_T;
typedef struct {
uint8_T re;
uint8_T im;
} cuint8_T;
typedef struct {
int16_T re;
int16_T im;
} cint16_T;
typedef struct {
uint16_T re;
uint16_T im;
} cuint16_T;
typedef struct {
int32_T re;
int32_T im;
} cint32_T;
typedef struct {
uint32_T re;
uint32_T im;
} cuint32_T;
typedef struct {
int64_T re;
int64_T im;
} cint64_T;
typedef struct {
uint64_T re;
uint64_T im;
} cuint64_T;
/*=======================================================================*
* Min and Max: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
*=======================================================================*/
#define MAX_int8_T ((int8_T)(127))
#define MIN_int8_T ((int8_T)(-128))
#define MAX_uint8_T ((uint8_T)(255))
#define MIN_uint8_T ((uint8_T)(0))
#define MAX_int16_T ((int16_T)(32767))
#define MIN_int16_T ((int16_T)(-32768))
#define MAX_uint16_T ((uint16_T)(65535))
#define MIN_uint16_T ((uint16_T)(0))
#define MAX_int32_T ((int32_T)(2147483647))
#define MIN_int32_T ((int32_T)(-2147483647-1))
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
#define MIN_uint32_T ((uint32_T)(0))
#define MAX_int64_T ((int64_T)(9223372036854775807L))
#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L))
#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
#define MIN_uint64_T ((uint64_T)(0UL))
/* Logical type definitions */
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
# ifndef false
# define false (0U)
# endif
# ifndef true
# define true (1U)
# endif
#endif
/*
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
* for signed integer values.
*/
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
#error "This code must be compiled using a 2's complement representation for signed integer values"
#endif
/*
* Maximum length of a MATLAB identifier (function/variable)
* including the null-termination character. Referenced by
* rt_logging.c and rt_matrx.c.
*/
#define TMW_NAME_LENGTH_MAX 64
#endif
#endif
/* End of code generation (rtwtypes.h) */

62
apps/position_estimator/position_estimator.m
View File

@ -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

269
apps/position_estimator/position_estimator.prj
View File

@ -1,269 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project>
<configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="position_estimator" location="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator" file="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator/position_estimator.prj" build-checksum="2425236060">
<param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
<param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
<param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
<param.mex.general.EnableBLAS>false</param.mex.general.EnableBLAS>
<param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
<param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
<param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
<param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
<param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
<param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
<param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
<param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
<param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
<param.mex.paths.working.specified />
<param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
<param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
<param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
<param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomSourceCode />
<param.mex.customcode.CustomHeaderCode />
<param.mex.customcode.CustomInitializer />
<param.mex.customcode.CustomTerminator />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
<param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
<param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
<param.mex.InlineThreshold>10</param.mex.InlineThreshold>
<param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
<param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
<param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
<param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
<param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
<param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
<param.grt.CCompilerOptimization>option.CCompilerOptimization.Off</param.grt.CCompilerOptimization>
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.GenerateMakefile>false</param.grt.general.GenerateMakefile>
<param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
<param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
<param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
<param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
<param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
<param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
<param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
<param.grt.paths.working.specified />
<param.grt.paths.build>option.paths.build.specified</param.grt.paths.build>
<param.grt.paths.build.specified>./codegen</param.grt.paths.build.specified>
<param.grt.paths.search />
<param.grt.report.GenerateReport>false</param.grt.report.GenerateReport>
<param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
<param.grt.GenerateComments>true</param.grt.GenerateComments>
<param.grt.MATLABSourceComments>false</param.grt.MATLABSourceComments>
<param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
<param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
<param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
<param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
<param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
<param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
<param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
<param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
<param.grt.MaxIdLength>32</param.grt.MaxIdLength>
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomSourceCode />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose>false</param.grt.Verbose>
<param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
<param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
<param.ert.TargetFunctionLibrary>C89/C90 (ANSI)</param.ert.TargetFunctionLibrary>
<param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
<param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
<param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
<param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
<param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
<param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
<param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
<param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
<param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
<param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
<param.grt.InlineThreshold>10</param.grt.InlineThreshold>
<param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
<param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
<param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
<param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<param.mex.outputfile>position_estimator_mex</param.mex.outputfile>
<param.grt.outputfile>position_estimator</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.mex.GenCodeOnly>true</param.mex.GenCodeOnly>
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.version>R2011a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml>/home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html</param.mex.mainhtml>
<param.grt.mainhtml />
<unset>
<param.mex.general.TargetLang />
<param.mex.general.IntegrityChecks />
<param.mex.general.ResponsivenessChecks />
<param.mex.general.ExtrinsicCalls />
<param.mex.general.EchoExpressions />
<param.mex.general.EnableDebugging />
<param.mex.general.SaturateOnIntegerOverflow />
<param.mex.general.FilePartitionMethod />
<param.mex.general.GlobalDataSyncMethod />
<param.mex.general.EnableVariableSizing />
<param.mex.general.DynamicMemoryAllocation />
<param.mex.paths.working />
<param.mex.paths.working.specified />
<param.mex.paths.build />
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport />
<param.mex.report.LaunchReport />
<param.mex.comments.GenerateComments />
<param.mex.comments.MATLABSourceComments />
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy />
<param.mex.MemcpyThreshold />
<param.mex.InitFltsAndDblsToZero />
<param.mex.InlineThreshold />
<param.mex.InlineThresholdMax />
<param.mex.InlineStackLimit />
<param.mex.StackUsageMax />
<param.mex.ConstantFoldingTimeout />
<param.grt.general.TargetLang />
<param.grt.CCompilerOptimization />
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.MakeCommand />
<param.grt.general.TemplateMakefile />
<param.grt.general.SaturateOnIntegerOverflow />
<param.grt.general.FilePartitionMethod />
<param.grt.general.EnableVariableSizing />
<param.grt.general.DynamicMemoryAllocation />
<param.grt.paths.working />
<param.grt.paths.working.specified />
<param.grt.paths.search />
<param.grt.report.LaunchReport />
<param.grt.GenerateComments />
<param.grt.MATLABSourceComments />
<param.ert.MATLABFcnDesc />
<param.ert.CustomSymbolStrGlobalVar />
<param.ert.CustomSymbolStrType />
<param.ert.CustomSymbolStrField />
<param.ert.CustomSymbolStrFcn />
<param.ert.CustomSymbolStrTmpVar />
<param.ert.CustomSymbolStrMacro />
<param.ert.CustomSymbolStrEMXArray />
<param.grt.MaxIdLength />
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose />
<param.grt.SupportNonFinite />
<param.ert.PurelyIntegerCode />
<param.ert.SupportNonFinite />
<param.ert.IncludeTerminateFcn />
<param.ert.MultiInstanceCode />
<param.ert.ParenthesesLevel />
<param.ert.ConvertIfToSwitch />
<param.ert.PreserveExternInFcnDecls />
<param.grt.EnableMemcpy />
<param.grt.MemcpyThreshold />
<param.grt.InitFltsAndDblsToZero />
<param.grt.InlineThreshold />
<param.grt.InlineThresholdMax />
<param.grt.InlineStackLimit />
<param.grt.StackUsageMax />
<param.grt.ConstantFoldingTimeout />
<param.UseECoderFeatures />
<param.mex.outputfile />
<param.grt.outputfile />
<param.version />
<param.HasECoderFeatures />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}/position_estimator.m" custom-data-expanded="true">
<Inputs fileName="position_estimator.m" functionName="position_estimator">
<Input Name="u">
<Class>single</Class>
<Size>2 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="z">
<Class>single</Class>
<Size>3 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="xapo">
<Class>single</Class>
<Size>6 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="Papo">
<Class>single</Class>
<Size>6 x 6</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="gps_covariance">
<Class>single</Class>
<Size>3 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="predict_only">
<Class>uint8</Class>
<Size>1 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.package />
<build-deliverables />
<workflow />
<matlab>
<root>/home/thomasgubler/tools/matlab</root>
</matlab>
<platform>
<unix>true</unix>
<mac>false</mac>
<windows>false</windows>
<win2k>false</win2k>
<winxp>false</winxp>
<vista>false</vista>
<linux>true</linux>
<solaris>false</solaris>
<osver>3.2.0-25-generic</osver>
<os32>false</os32>
<os64>true</os64>
<arch>glnxa64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>

0
apps/px4/attitude_estimator_bm/.context
View File

42
apps/px4/attitude_estimator_bm/Makefile
View File

@ -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

322
apps/px4/attitude_estimator_bm/attitude_bm.c
View File

@ -1,322 +0,0 @@
/*
* attitude_bm.c
*
* Created on: 21.12.2010
* Author: Laurens Mackay, Tobias Naegeli
*/
#include <math.h>
#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);
}

24
apps/px4/attitude_estimator_bm/attitude_bm.h
View File

@ -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_ */

283
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
View File

@ -1,283 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file attitude_estimator_bm.c
* Black Magic Attitude Estimator
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/time.h>
#include <stdbool.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <string.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <math.h>
#include <errno.h>
#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;
}

115
apps/px4/attitude_estimator_bm/kalman.c
View File

@ -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);
}

35
apps/px4/attitude_estimator_bm/kalman.h
View File

@ -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_ */

156
apps/px4/attitude_estimator_bm/matrix.h
View File

@ -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_ */

42
apps/px4/ground_estimator/Makefile
View File

@ -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

180
apps/px4/ground_estimator/ground_estimator.c
View File

@ -1,180 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* 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 <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <stdlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/debug_key_value.h>
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 <additional params>]\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);
}

2
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -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