fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 16:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into indoor branch

Browse Source
This commit is contained in:
Lorenz Meier
2014-12-26 17:06:19 +01:00
parent f3a224e30d 5c51adf5f7
commit 407889ea2c
229 changed files with 27468 additions and 4247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/AttitudeEKF.m
+298
View File
@@ -0,0 +1,298 @@
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
%LQG Postion Estimator and Controller
% Observer:
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
% x[n+1|n] = Ax[n|n] + Bu[n]
%
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
%
%
% Arguments:
% approx_prediction: if 1 then the exponential map is approximated with a
% first order taylor approximation. has at the moment not a big influence
% (just 1st or 2nd order approximation) we should change it to rodriquez
% approximation.
% use_inertia_matrix: set to true if you have the inertia matrix J for your
% quadrotor
% xa_apo_k: old state vectotr
% zFlag: if sensor measurement is available [gyro, acc, mag]
% dt: dt in s
% z: measurements [gyro, acc, mag]
% q_rotSpeed: process noise gyro
% q_rotAcc: process noise gyro acceleration
% q_acc: process noise acceleration
% q_mag: process noise magnetometer
% r_gyro: measurement noise gyro
% r_accel: measurement noise accel
% r_mag: measurement noise mag
% J: moment of inertia matrix
% Output:
% xa_apo: updated state vectotr
% Pa_apo: updated state covariance matrix
% Rot_matrix: rotation matrix
% eulerAngles: euler angles
% debugOutput: not used
%% model specific parameters
% compute once the inverse of the Inertia
persistent Ji;
if isempty(Ji)
Ji=single(inv(J));
end
%% init
persistent x_apo
if(isempty(x_apo))
gyro_init=single([0;0;0]);
gyro_acc_init=single([0;0;0]);
acc_init=single([0;0;-9.81]);
mag_init=single([1;0;0]);
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
end
persistent P_apo
if(isempty(P_apo))
% P_apo = single(eye(NSTATES) * 1000);
P_apo = single(200*ones(12));
end
debugOutput = single(zeros(4,1));
%% copy the states
wx= x_apo(1); % x body angular rate
wy= x_apo(2); % y body angular rate
wz= x_apo(3); % z body angular rate
wax= x_apo(4); % x body angular acceleration
way= x_apo(5); % y body angular acceleration
waz= x_apo(6); % z body angular acceleration
zex= x_apo(7); % x component gravity vector
zey= x_apo(8); % y component gravity vector
zez= x_apo(9); % z component gravity vector
mux= x_apo(10); % x component magnetic field vector
muy= x_apo(11); % y component magnetic field vector
muz= x_apo(12); % z component magnetic field vector
%% prediction section
% compute the apriori state estimate from the previous aposteriori estimate
%body angular accelerations
if (use_inertia_matrix==1)
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
else
wak =[wax;way;waz];
end
%body angular rates
wk =[wx; wy; wz] + dt*wak;
%derivative of the prediction rotation matrix
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
%prediction of the earth z vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
else
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
%precision
end
%prediction of the magnetic vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
else
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
%precision
end
x_apr=[wk;wak;zek;muk];
% compute the apriori error covariance estimate from the previous
%aposteriori estimate
EZ=[0,zez,-zey;
-zez,0,zex;
zey,-zex,0]';
MA=[0,muz,-muy;
-muz,0,mux;
muy,-mux,0]';
E=single(eye(3));
Z=single(zeros(3));
A_lin=[ Z, E, Z, Z
Z, Z, Z, Z
EZ, Z, O, Z
MA, Z, Z, O];
A_lin=eye(12)+A_lin*dt;
%process covariance matrix
persistent Q
if (isempty(Q))
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
q_rotAcc,q_rotAcc,q_rotAcc,...
q_acc,q_acc,q_acc,...
q_mag,q_mag,q_mag]);
end
P_apr=A_lin*P_apo*A_lin'+Q;
%% update
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0,0,0,0;
% 0,r_gyro,0,0,0,0,0,0,0;
% 0,0,r_gyro,0,0,0,0,0,0;
% 0,0,0,r_accel,0,0,0,0,0;
% 0,0,0,0,r_accel,0,0,0,0;
% 0,0,0,0,0,r_accel,0,0,0;
% 0,0,0,0,0,0,r_mag,0,0;
% 0,0,0,0,0,0,0,r_mag,0;
% 0,0,0,0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
%observation matrix
%[zw;ze;zmk];
H_k=[ E, Z, Z, Z;
Z, Z, E, Z;
Z, Z, Z, E];
y_k=z(1:9)-H_k*x_apr;
%S_k=H_k*P_apr*H_k'+R;
S_k=H_k*P_apr*H_k';
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
K_k=(P_apr*H_k'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k)*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
R=[r_gyro,0,0;
0,r_gyro,0;
0,0,r_gyro];
R_v=[r_gyro,r_gyro,r_gyro];
%observation matrix
H_k=[ E, Z, Z, Z];
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_accel,0,0;
% 0,0,0,0,r_accel,0;
% 0,0,0,0,0,r_accel];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, E, Z];
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_mag,0,0;
% 0,0,0,0,r_mag,0;
% 0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, Z, E];
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
x_apo=x_apr;
P_apo=P_apr;
end
end
end
end
%% euler anglels extraction
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
y_n_b=cross(z_n_b,m_n_b);
y_n_b=y_n_b./norm(y_n_b);
x_n_b=(cross(y_n_b,z_n_b));
x_n_b=x_n_b./norm(x_n_b);
xa_apo=x_apo;
Pa_apo=P_apo;
% rotation matrix from earth to body system
Rot_matrix=[x_n_b,y_n_b,z_n_b];
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
theta=-asin(Rot_matrix(1,3));
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
eulerAngles=[phi;theta;psi];
src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
+502
View File
@@ -0,0 +1,502 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
<profile key="profile.mex">
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
<param.RanInstrumentedMex>false</param.RanInstrumentedMex>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks>true</param.ResponsivenessChecks>
<param.ExtrinsicCalls>true</param.ExtrinsicCalls>
<param.IntegrityChecks>true</param.IntegrityChecks>
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
<param.EnableVariableSizing>true</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>200000</param.StackUsageMax>
<param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>false</param.MATLABSourceComments>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.EnableDebugging>false</param.EnableDebugging>
<param.GenerateReport>true</param.GenerateReport>
<param.LaunchReport>false</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.EchoExpressions>true</param.EchoExpressions>
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
<unset>
<param.MergeInstrumentationResults />
<param.BuiltInstrumentedMex />
<param.RanInstrumentedMex />
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.BuildFolder />
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks />
<param.ExtrinsicCalls />
<param.IntegrityChecks />
<param.SaturateOnIntegerOverflow />
<param.GlobalDataSyncMethod />
<param.EnableVariableSizing />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.StackUsageMax />
<param.FilePartitionMethod />
<param.GenerateComments />
<param.MATLABSourceComments />
<param.ReservedNameArray />
<param.EnableScreener />
<param.EnableDebugging />
<param.GenerateReport />
<param.LaunchReport />
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes />
<param.mex.GenCodeOnly />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.TargetLang />
<param.EchoExpressions />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.ConstantInputs />
</unset>
</profile>
<profile key="profile.c">
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
<param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
<param.SearchPaths />
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.PurelyIntegerCode>false</param.PurelyIntegerCode>
<param.SupportNonFinite>false</param.SupportNonFinite>
<param.EnableVariableSizing>false</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>4000</param.StackUsageMax>
<param.MultiInstanceCode>false</param.MultiInstanceCode>
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>true</param.MATLABSourceComments>
<param.MATLABFcnDesc>false</param.MATLABFcnDesc>
<param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
<param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
<param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
<param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
<param.MaxIdLength>31</param.MaxIdLength>
<param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
<param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
<param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
<param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
<param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
<param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
<param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
<param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.Verbose>false</param.Verbose>
<param.GenerateReport>true</param.GenerateReport>
<param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
<param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
<param.LaunchReport>true</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
<param.SameHardware>true</param.SameHardware>
<param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
<param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
<var.instance.enabled.Production>true</var.instance.enabled.Production>
<param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
<param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
<param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
<param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
<param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
<param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
<param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
<param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
<param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
<param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
<param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
<param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
<param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
<param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
<param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
<param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
<param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
<var.instance.enabled.Target>false</var.instance.enabled.Target>
<param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
<param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
<param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
<param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
<param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
<param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
<param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
<param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
<param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
<param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
<param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
<param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
<param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
<param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
<param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
<param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
<param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile>true</param.GenerateMakefile>
<param.BuildToolEnable>false</param.BuildToolEnable>
<param.MakeCommand>make_rtw</param.MakeCommand>
<param.TemplateMakefile>default_tmf</param.TemplateMakefile>
<param.BuildToolConfiguration />
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.PassStructByReference>false</param.PassStructByReference>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<unset>
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.SearchPaths />
<param.SaturateOnIntegerOverflow />
<param.PurelyIntegerCode />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.MultiInstanceCode />
<param.GenerateComments />
<param.MATLABFcnDesc />
<param.DataTypeReplacement />
<param.ConvertIfToSwitch />
<param.PreserveExternInFcnDecls />
<param.ParenthesesLevel />
<param.MaxIdLength />
<param.CustomSymbolStrGlobalVar />
<param.CustomSymbolStrType />
<param.CustomSymbolStrField />
<param.CustomSymbolStrFcn />
<param.CustomSymbolStrTmpVar />
<param.CustomSymbolStrMacro />
<param.CustomSymbolStrEMXArray />
<param.CustomSymbolStrEMXArrayFcn />
<param.ReservedNameArray />
<param.EnableScreener />
<param.Verbose />
<param.GenerateReport />
<param.GenerateCodeMetricsReport />
<param.GenerateCodeReplacementReport />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.SameHardware />
<var.instance.enabled.Production />
<param.HardwareSizeChar.Production />
<param.HardwareSizeShort.Production />
<param.HardwareSizeInt.Production />
<param.HardwareSizeLong.Production />
<param.HardwareSizeLongLong.Production />
<param.HardwareSizeFloat.Production />
<param.HardwareSizeDouble.Production />
<param.HardwareSizeWord.Production />
<param.HardwareSizePointer.Production />
<param.HardwareEndianness.Production />
<param.HardwareLongLongMode.Production />
<param.HardwareDivisionRounding.Production />
<var.instance.enabled.Target />
<param.HardwareSizeChar.Target />
<param.HardwareSizeShort.Target />
<param.HardwareSizeInt.Target />
<param.HardwareSizeLongLong.Target />
<param.HardwareSizeFloat.Target />
<param.HardwareSizeDouble.Target />
<param.HardwareEndianness.Target />
<param.HardwareAtomicFloatSize.Target />
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.IncludeTerminateFcn />
<param.TargetLang />
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile />
<param.BuildToolEnable />
<param.MakeCommand />
<param.TemplateMakefile />
<param.BuildToolConfiguration />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.UseECoderFeatures />
</unset>
</profile>
<param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
<param.version>R2012a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
<param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
<param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
<param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
<param.SILDebugging>false</param.SILDebugging>
<param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
<param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
<param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
<param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
<param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
<param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
<param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
<param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
<param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
<param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
<param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
<param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
<param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
<param.UsePreconditions>false</param.UsePreconditions>
<param.FeatureFlags />
<param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
<param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength>16</param.DefaultWordLength>
<param.DefaultFractionLength>4</param.DefaultFractionLength>
<param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
<param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
<param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
<param.LogAllIOValues>false</param.LogAllIOValues>
<param.LogHistogram>false</param.LogHistogram>
<param.ShowCoverage>true</param.ShowCoverage>
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
<param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
<unset>
<param.outputfile />
<param.version />
<param.HasECoderFeatures />
<param.CallGeneratedCodeFromTest />
<param.VerificationMode />
<param.SILDebugging />
<param.AutoInferUseVariableSize />
<param.AutoInferUseUnboundedSize />
<param.AutoInferVariableSizeThreshold />
<param.AutoInferUnboundedSizeThreshold />
<param.mex.outputfile />
<param.grt.outputfile />
<param.FixedPointTypeProposalMode />
<param.DefaultProposedFixedPointType />
<param.MinMaxSafetyMargin />
<param.OptimizeWholeNumbers />
<param.LaunchInstrumentationReport />
<param.OpenInstrumentationReportInBrowser />
<param.CreatePrintableInstrumentationReport />
<param.EnableAutoExtrinsicCalls />
<param.UsePreconditions />
<param.FeatureFlags />
<param.FixedPointMode />
<param.AutoScaleLoopIndexVariables />
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength />
<param.DefaultFractionLength />
<param.FixedPointSafetyMargin />
<param.FixedPointFimath />
<param.FixedPointTypeSource />
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly />
<param.LogAllIOValues />
<param.LogHistogram />
<param.ShowCoverage />
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.GeneratedFixedPointFileSuffix />
<param.DefaultFixedPointSignedness />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
<Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
<Input Name="approx_prediction">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="use_inertia_matrix">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="zFlag">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>3 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="dt">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="z">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>9 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotSpeed">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotAcc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_acc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_gyro">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_accel">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="J">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>3 x 3</Size>
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.testbench>
<file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
</fileset.testbench>
<fileset.package />
<build-deliverables>
<file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
</build-deliverables>
<workflow />
<matlab>
<root>/opt/matlab/r2013b</root>
<toolboxes>
<toolbox name="fixedpoint" />
</toolboxes>
</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.16.1-1-ARCH</osver>
<os32>false</os32>
<os64>true</os64>
<arch>glnxa64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+66 -9
View File
@@ -38,6 +38,7 @@
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -75,8 +76,7 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "codegen/AttitudeEKF.h"
#include "attitude_estimator_ekf_params.h"
#ifdef __cplusplus
}
@@ -133,9 +133,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
14000,
7200,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
@@ -207,10 +207,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
float debugOutput[4] = { 0.0f };
int overloadcounter = 19;
/* Initialize filter */
attitudeKalmanfilter_initialize();
AttitudeEKF_initialize();
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
@@ -275,9 +276,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params {};
struct attitude_estimator_ekf_params ekf_params;
memset(&ekf_params, 0, sizeof(ekf_params));
struct attitude_estimator_ekf_param_handles ekf_param_handles {};
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
@@ -528,8 +530,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* Call the estimator */
AttitudeEKF(false, // approx_prediction
(unsigned char)ekf_params.use_moment_inertia,
update_vect,
dt,
z_k,
ekf_params.q[0], // q_rotSpeed,
ekf_params.q[1], // q_rotAcc
ekf_params.q[2], // q_acc
ekf_params.q[3], // q_mag
ekf_params.r[0], // r_gyro
ekf_params.r[1], // r_accel
ekf_params.r[2], // r_mag
ekf_params.moment_inertia_J,
x_aposteriori,
P_aposteriori,
Rot_matrix,
euler,
debugOutput);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
@@ -576,6 +595,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
att.R_valid = true;
// compute secondary attitude
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R_adapted = R;
//move z to x
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
//move x to z
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
//change direction of pitch (convert to right handed system)
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation
euler_angles_sec = R_adapted.to_euler();
att.roll_sec = euler_angles_sec(0);
att.pitch_sec = euler_angles_sec(1);
att.yaw_sec = euler_angles_sec(2);
memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec));
att.rollspeed_sec = -x_aposteriori[2];
att.pitchspeed_sec = x_aposteriori[1];
att.yawspeed_sec = x_aposteriori[0];
att.rollacc_sec = -x_aposteriori[5];
att.pitchacc_sec = x_aposteriori[4];
att.yawacc_sec = x_aposteriori[3];
att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+90 -16
View File
@@ -44,28 +44,96 @@
/* Extended Kalman Filter covariances */
/* gyro process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/* gyro offsets process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
/**
* Body angular rate process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
/**
* Body angular acceleration process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
/**
* Acceleration process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/**
* Magnet field vector process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
/**
* Gyro measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/* accel measurement noise */
/**
* Accel measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/* mag measurement noise */
/**
* Mag measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
/**
* Moment of inertia matrix diagonal entry (1, 1)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
/**
* Moment of inertia matrix diagonal entry (2, 2)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
/**
* Moment of inertia matrix diagonal entry (3, 3)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
/**
* Moment of inertia enabled in estimator
*
* If set to != 0 the moment of inertia will be used in the estimator
*
* @group attitude_ekf
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(ATT_J_EN, 0);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
@@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->q1 = param_find("EKF_ATT_V3_Q1");
h->q2 = param_find("EKF_ATT_V3_Q2");
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V4_R0");
h->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
h->moment_inertia_J[0] = param_find("ATT_J11");
h->moment_inertia_J[1] = param_find("ATT_J22");
h->moment_inertia_J[2] = param_find("ATT_J33");
h->use_moment_inertia = param_find("ATT_J_EN");
return OK;
}
@@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->q1, &(p->q[1]));
param_get(h->q2, &(p->q[2]));
param_get(h->q3, &(p->q[3]));
param_get(h->q4, &(p->q[4]));
param_get(h->r0, &(p->r[0]));
param_get(h->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI_F / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
for (int i = 0; i < 3; i++) {
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
}
param_get(h->use_moment_inertia, &(p->use_moment_inertia));
return OK;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+8 -4
View File
@@ -42,8 +42,10 @@
#include <systemlib/param/param.h>
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
float r[3];
float q[4];
float moment_inertia_J[9];
int32_t use_moment_inertia;
float roll_off;
float pitch_off;
float yaw_off;
@@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t r0, r1, r2;
param_t q0, q1, q2, q3;
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
param_t use_moment_inertia;
param_t mag_decl;
param_t acc_comp;
};
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
+1456
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
+26
View File
@@ -0,0 +1,26 @@
/*
* AttitudeEKF.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_H__
#define __ATTITUDEEKF_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "AttitudeEKF_types.h"
/* Function Declarations */
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
extern void AttitudeEKF_initialize(void);
extern void AttitudeEKF_terminate(void);
#endif
/* End of code generation (AttitudeEKF.h) */
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+17
View File
@@ -0,0 +1,17 @@
/*
* AttitudeEKF_types.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_TYPES_H__
#define __ATTITUDEEKF_TYPES_H__
/* Include files */
#include "rtwtypes.h"
#endif
/* End of code generation (AttitudeEKF_types.h) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
-1148
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
-34
View File
@@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_H__
#define __ATTITUDEKALMANFILTER_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* attitudeKalmanfilter_initialize.c
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "attitudeKalmanfilter_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void attitudeKalmanfilter_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (attitudeKalmanfilter_initialize.c) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
-34
View File
@@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter_initialize.h
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter_initialize(void);
#endif
/* End of code generation (attitudeKalmanfilter_initialize.h) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* attitudeKalmanfilter_terminate.c
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "attitudeKalmanfilter_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void attitudeKalmanfilter_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (attitudeKalmanfilter_terminate.c) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
-34
View File
@@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter_terminate.h
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter_terminate(void);
#endif
/* End of code generation (attitudeKalmanfilter_terminate.h) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* attitudeKalmanfilter_types.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
#define __ATTITUDEKALMANFILTER_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (attitudeKalmanfilter_types.h) */
src/modules/attitude_estimator_ekf/codegen/cross.c
-37
View File
@@ -1,37 +0,0 @@
/*
* cross.c
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "cross.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
/* End of code generation (cross.c) */
src/modules/attitude_estimator_ekf/codegen/cross.h
-34
View File
@@ -1,34 +0,0 @@
/*
* cross.h
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __CROSS_H__
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
#endif
/* End of code generation (cross.h) */
src/modules/attitude_estimator_ekf/codegen/eye.c
-51
View File
@@ -1,51 +0,0 @@
/*
* eye.c
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "eye.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void b_eye(real_T I[144])
{
int32_T i;
memset(&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
}
}
/*
*
*/
void eye(real_T I[9])
{
int32_T i;
memset(&I[0], 0, 9U * sizeof(real_T));
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1.0;
}
}
/* End of code generation (eye.c) */
src/modules/attitude_estimator_ekf/codegen/eye.h
-35
View File
@@ -1,35 +0,0 @@
/*
* eye.h
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __EYE_H__
#define __EYE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void b_eye(real_T I[144]);
extern void eye(real_T I[9]);
#endif
/* End of code generation (eye.h) */
src/modules/attitude_estimator_ekf/codegen/mrdivide.c
-357
View File
@@ -1,357 +0,0 @@
/*
* mrdivide.c
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "mrdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
{
real32_T b_A[9];
int32_T rtemp;
int32_T k;
real32_T b_B[36];
int32_T r1;
int32_T r2;
int32_T r3;
real32_T maxval;
real32_T a21;
real32_T Y[36];
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
}
}
for (rtemp = 0; rtemp < 12; rtemp++) {
for (k = 0; k < 3; k++) {
b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
}
}
r1 = 0;
r2 = 1;
r3 = 2;
maxval = (real32_T)fabs(b_A[0]);
a21 = (real32_T)fabs(b_A[1]);
if (a21 > maxval) {
maxval = a21;
r1 = 1;
r2 = 0;
}
if ((real32_T)fabs(b_A[2]) > maxval) {
r1 = 2;
r2 = 1;
r3 = 0;
}
b_A[r2] /= b_A[r1];
b_A[r3] /= b_A[r1];
b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
rtemp = r2;
r2 = r3;
r3 = rtemp;
}
b_A[3 + r3] /= b_A[3 + r2];
b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
for (k = 0; k < 12; k++) {
Y[3 * k] = b_B[r1 + 3 * k];
Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+ r3];
Y[2 + 3 * k] /= b_A[6 + r3];
Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
Y[1 + 3 * k] /= b_A[3 + r2];
Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
Y[3 * k] /= b_A[r1];
}
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 12; k++) {
y[k + 12 * rtemp] = Y[rtemp + 3 * k];
}
}
}
/*
*
*/
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
{
real32_T b_A[36];
int8_T ipiv[6];
int32_T i3;
int32_T iy;
int32_T j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T jy;
int32_T ijA;
real32_T Y[72];
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 6; iy++) {
b_A[iy + 6 * i3] = B[i3 + 6 * iy];
}
ipiv[i3] = (int8_T)(1 + i3);
}
for (j = 0; j < 5; j++) {
c = j * 7;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 6 - j; k++) {
ix++;
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
iy = k - 1;
temp = s;
}
}
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy += j;
for (k = 0; k < 6; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 6;
iy += 6;
}
}
i3 = (c - j) + 6;
for (jy = c + 1; jy + 1 <= i3; jy++) {
b_A[jy] /= b_A[c];
}
}
iy = c;
jy = c + 6;
for (k = 1; k <= 5 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i3 = (iy - j) + 12;
for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 6;
iy += 6;
}
}
for (i3 = 0; i3 < 12; i3++) {
for (iy = 0; iy < 6; iy++) {
Y[iy + 6 * i3] = A[i3 + 12 * iy];
}
}
for (jy = 0; jy < 6; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[jy + 6 * j];
Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
Y[(ipiv[jy] + 6 * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 0; k < 6; k++) {
iy = 6 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 7; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
}
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 5; k > -1; k += -1) {
iy = 6 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * i3] = Y[i3 + 6 * iy];
}
}
}
/*
*
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
real32_T b_A[81];
int8_T ipiv[9];
int32_T i2;
int32_T iy;
int32_T j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T jy;
int32_T ijA;
real32_T Y[108];
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * i2] = B[i2 + 9 * iy];
}
ipiv[i2] = (int8_T)(1 + i2);
}
for (j = 0; j < 8; j++) {
c = j * 10;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 9 - j; k++) {
ix++;
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
iy = k - 1;
temp = s;
}
}
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy += j;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 9;
iy += 9;
}
}
i2 = (c - j) + 9;
for (jy = c + 1; jy + 1 <= i2; jy++) {
b_A[jy] /= b_A[c];
}
}
iy = c;
jy = c + 9;
for (k = 1; k <= 8 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i2 = (iy - j) + 18;
for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 9;
iy += 9;
}
}
for (i2 = 0; i2 < 12; i2++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * i2] = A[i2 + 12 * iy];
}
}
for (jy = 0; jy < 9; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[jy + 9 * j];
Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
Y[(ipiv[jy] + 9 * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 0; k < 9; k++) {
iy = 9 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 10; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 8; k > -1; k += -1) {
iy = 9 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * i2] = Y[i2 + 9 * iy];
}
}
}
/* End of code generation (mrdivide.c) */
src/modules/attitude_estimator_ekf/codegen/mrdivide.h
-36
View File
@@ -1,36 +0,0 @@
/*
* mrdivide.h
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __MRDIVIDE_H__
#define __MRDIVIDE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
#endif
/* End of code generation (mrdivide.h) */
src/modules/attitude_estimator_ekf/codegen/norm.c
-54
View File
@@ -1,54 +0,0 @@
/*
* norm.c
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "norm.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
real32_T norm(const real32_T x[3])
{
real32_T y;
real32_T scale;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
scale = 1.17549435E-38F;
for (k = 0; k < 3; k++) {
absxk = (real32_T)fabs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */
src/modules/attitude_estimator_ekf/codegen/norm.h
-34
View File
@@ -1,34 +0,0 @@
/*
* norm.h
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __NORM_H__
#define __NORM_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern real32_T norm(const real32_T x[3]);
#endif
/* End of code generation (norm.h) */
src/modules/attitude_estimator_ekf/codegen/rdivide.c
-38
View File
@@ -1,38 +0,0 @@
/*
* rdivide.c
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "rdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
{
int32_T i;
for (i = 0; i < 3; i++) {
z[i] = x[i] / y;
}
}
/* End of code generation (rdivide.c) */
src/modules/attitude_estimator_ekf/codegen/rdivide.h
-34
View File
@@ -1,34 +0,0 @@
/*
* rdivide.h
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RDIVIDE_H__
#define __RDIVIDE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
#endif
/* End of code generation (rdivide.h) */
src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
-139
View File
@@ -1,139 +0,0 @@
/*
* rtGetInf.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, Inf and MinusInf
*/
#include "rtGetInf.h"
#define NumBitsPerChar 8U
/* Function: rtGetInf ==================================================
* Abstract:
* Initialize rtInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T inf = 0.0;
if (bitsPerReal == 32U) {
inf = rtGetInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
}
}
return inf;
}
/* Function: rtGetInfF ==================================================
* Abstract:
* Initialize rtInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetInfF(void)
{
IEEESingle infF;
infF.wordL.wordLuint = 0x7F800000U;
return infF.wordL.wordLreal;
}
/* Function: rtGetMinusInf ==================================================
* Abstract:
* Initialize rtMinusInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetMinusInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T minf = 0.0;
if (bitsPerReal == 32U) {
minf = rtGetMinusInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
}
}
return minf;
}
/* Function: rtGetMinusInfF ==================================================
* Abstract:
* Initialize rtMinusInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetMinusInfF(void)
{
IEEESingle minfF;
minfF.wordL.wordLuint = 0xFF800000U;
return minfF.wordL.wordLreal;
}
/* End of code generation (rtGetInf.c) */
src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
-23
View File
@@ -1,23 +0,0 @@
/*
* rtGetInf.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#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) */
src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
-96
View File
@@ -1,96 +0,0 @@
/*
* rtGetNaN.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, NaN
*/
#include "rtGetNaN.h"
#define NumBitsPerChar 8U
/* Function: rtGetNaN ==================================================
* Abstract:
* Initialize rtNaN needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetNaN(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T nan = 0.0;
if (bitsPerReal == 32U) {
nan = rtGetNaNF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF80000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
nan = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
nan = tmpVal.fltVal;
break;
}
}
}
return nan;
}
/* Function: rtGetNaNF ==================================================
* Abstract:
* Initialize rtNaNF needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetNaNF(void)
{
IEEESingle nanF = { { 0 } };
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
nanF.wordL.wordLuint = 0xFFC00000U;
break;
}
case BigEndian:
{
nanF.wordL.wordLuint = 0x7FFFFFFFU;
break;
}
}
return nanF.wordL.wordLreal;
}
/* End of code generation (rtGetNaN.c) */
src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
-21
View File
@@ -1,21 +0,0 @@
/*
* rtGetNaN.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#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) */
src/modules/attitude_estimator_ekf/codegen/rt_defines.h
-24
View File
@@ -1,24 +0,0 @@
/*
* rt_defines.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RT_DEFINES_H__
#define __RT_DEFINES_H__
#include <stdlib.h>
#define RT_PI 3.14159265358979323846
#define RT_PIF 3.1415927F
#define RT_LN_10 2.30258509299404568402
#define RT_LN_10F 2.3025851F
#define RT_LOG10E 0.43429448190325182765
#define RT_LOG10EF 0.43429449F
#define RT_E 2.7182818284590452354
#define RT_EF 2.7182817F
#endif
/* End of code generation (rt_defines.h) */
src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
-87
View File
@@ -1,87 +0,0 @@
/*
* rt_nonfinite.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finites,
* (Inf, NaN and -Inf).
*/
#include "rt_nonfinite.h"
#include "rtGetNaN.h"
#include "rtGetInf.h"
real_T rtInf;
real_T rtMinusInf;
real_T rtNaN;
real32_T rtInfF;
real32_T rtMinusInfF;
real32_T rtNaNF;
/* Function: rt_InitInfAndNaN ==================================================
* Abstract:
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
*/
void rt_InitInfAndNaN(size_t realSize)
{
(void) (realSize);
rtNaN = rtGetNaN();
rtNaNF = rtGetNaNF();
rtInf = rtGetInf();
rtInfF = rtGetInfF();
rtMinusInf = rtGetMinusInf();
rtMinusInfF = rtGetMinusInfF();
}
/* Function: rtIsInf ==================================================
* Abstract:
* Test if value is infinite
*/
boolean_T rtIsInf(real_T value)
{
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
}
/* Function: rtIsInfF =================================================
* Abstract:
* Test if single-precision value is infinite
*/
boolean_T rtIsInfF(real32_T value)
{
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
}
/* Function: rtIsNaN ==================================================
* Abstract:
* Test if value is not a number
*/
boolean_T rtIsNaN(real_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan(value)? TRUE:FALSE;
#else
return (value!=value)? 1U:0U;
#endif
}
/* Function: rtIsNaNF =================================================
* Abstract:
* Test if single-precision value is not a number
*/
boolean_T rtIsNaNF(real32_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan((real_T)value)? true:false;
#else
return (value!=value)? 1U:0U;
#endif
}
/* End of code generation (rt_nonfinite.c) */
src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
-53
View File
@@ -1,53 +0,0 @@
/*
* rt_nonfinite.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#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) */
src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
Executable → Regular
+160 -159
View File
@@ -1,159 +1,160 @@
/*
* rtwtypes.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RTWTYPES_H__
#define __RTWTYPES_H__
#ifndef TRUE
# define TRUE (1U)
#endif
#ifndef FALSE
# define FALSE (0U)
#endif
#ifndef __TMWTYPES__
#define __TMWTYPES__
#include <limits.h>
/*=======================================================================*
* Target hardware information
* Device type: Generic->MATLAB Host Computer
* Number of bits: char: 8 short: 16 int: 32
* long: 32 native word size: 32
* Byte ordering: LittleEndian
* Signed integer division rounds to: 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 float real32_T;
typedef double real64_T;
/*===========================================================================*
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
* ulong_T, char_T and byte_T. *
*===========================================================================*/
typedef double real_T;
typedef double time_T;
typedef unsigned char boolean_T;
typedef int int_T;
typedef unsigned int uint_T;
typedef unsigned long ulong_T;
typedef char char_T;
typedef char_T byte_T;
/*===========================================================================*
* Complex number type definitions *
*===========================================================================*/
#define CREAL_T
typedef struct {
real32_T re;
real32_T im;
} creal32_T;
typedef struct {
real64_T re;
real64_T im;
} creal64_T;
typedef struct {
real_T re;
real_T im;
} creal_T;
typedef struct {
int8_T re;
int8_T im;
} cint8_T;
typedef struct {
uint8_T re;
uint8_T im;
} cuint8_T;
typedef struct {
int16_T re;
int16_T im;
} cint16_T;
typedef struct {
uint16_T re;
uint16_T im;
} cuint16_T;
typedef struct {
int32_T re;
int32_T im;
} cint32_T;
typedef struct {
uint32_T re;
uint32_T im;
} cuint32_T;
/*=======================================================================*
* Min and Max: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
*=======================================================================*/
#define MAX_int8_T ((int8_T)(127))
#define MIN_int8_T ((int8_T)(-128))
#define MAX_uint8_T ((uint8_T)(255))
#define MIN_uint8_T ((uint8_T)(0))
#define MAX_int16_T ((int16_T)(32767))
#define MIN_int16_T ((int16_T)(-32768))
#define MAX_uint16_T ((uint16_T)(65535))
#define MIN_uint16_T ((uint16_T)(0))
#define MAX_int32_T ((int32_T)(2147483647))
#define MIN_int32_T ((int32_T)(-2147483647-1))
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
#define MIN_uint32_T ((uint32_T)(0))
/* Logical type definitions */
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
# ifndef false
# define false (0U)
# endif
# ifndef true
# define true (1U)
# endif
#endif
/*
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
* for signed integer values.
*/
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
#error "This code must be compiled using a 2's complement representation for signed integer values"
#endif
/*
* Maximum length of a MATLAB identifier (function/variable)
* including the null-termination character. Referenced by
* rt_logging.c and rt_matrx.c.
*/
#define TMW_NAME_LENGTH_MAX 64
#endif
#endif
/* End of code generation (rtwtypes.h) */
/*
* rtwtypes.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#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: ARM Compatible->ARM Cortex
* Number of bits: char: 8 short: 16 int: 32
* long: 32
* native word size: 32
* Byte ordering: LittleEndian
* Signed integer division rounds to: Undefined
* Shift right on a signed integer as arithmetic shift: 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 float real32_T;
typedef double real64_T;
/*===========================================================================*
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
* ulong_T, char_T and byte_T. *
*===========================================================================*/
typedef double real_T;
typedef double time_T;
typedef unsigned char boolean_T;
typedef int int_T;
typedef unsigned int uint_T;
typedef unsigned long ulong_T;
typedef char char_T;
typedef char_T byte_T;
/*===========================================================================*
* Complex number type definitions *
*===========================================================================*/
#define CREAL_T
typedef struct {
real32_T re;
real32_T im;
} creal32_T;
typedef struct {
real64_T re;
real64_T im;
} creal64_T;
typedef struct {
real_T re;
real_T im;
} creal_T;
typedef struct {
int8_T re;
int8_T im;
} cint8_T;
typedef struct {
uint8_T re;
uint8_T im;
} cuint8_T;
typedef struct {
int16_T re;
int16_T im;
} cint16_T;
typedef struct {
uint16_T re;
uint16_T im;
} cuint16_T;
typedef struct {
int32_T re;
int32_T im;
} cint32_T;
typedef struct {
uint32_T re;
uint32_T im;
} cuint32_T;
/*=======================================================================*
* Min and Max: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
*=======================================================================*/
#define MAX_int8_T ((int8_T)(127))
#define MIN_int8_T ((int8_T)(-128))
#define MAX_uint8_T ((uint8_T)(255))
#define MIN_uint8_T ((uint8_T)(0))
#define MAX_int16_T ((int16_T)(32767))
#define MIN_int16_T ((int16_T)(-32768))
#define MAX_uint16_T ((uint16_T)(65535))
#define MIN_uint16_T ((uint16_T)(0))
#define MAX_int32_T ((int32_T)(2147483647))
#define MIN_int32_T ((int32_T)(-2147483647-1))
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
#define MIN_uint32_T ((uint32_T)(0))
/* Logical type definitions */
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
# ifndef false
# define false (0U)
# endif
# ifndef true
# define true (1U)
# endif
#endif
/*
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
* for signed integer values.
*/
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
#error "This code must be compiled using a 2's complement representation for signed integer values"
#endif
/*
* Maximum length of a MATLAB identifier (function/variable)
* including the null-termination character. Referenced by
* rt_logging.c and rt_matrx.c.
*/
#define TMW_NAME_LENGTH_MAX 64
#endif
#endif
/* End of code generation (rtwtypes.h) */
src/modules/attitude_estimator_ekf/module.mk
+5 -11
View File
@@ -39,16 +39,10 @@ MODULE_COMMAND = attitude_estimator_ekf
SRCS = attitude_estimator_ekf_main.cpp \
attitude_estimator_ekf_params.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
codegen/mrdivide.c \
codegen/rdivide.c \
codegen/attitudeKalmanfilter_initialize.c \
codegen/attitudeKalmanfilter_terminate.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
codegen/AttitudeEKF.c
MODULE_STACKSIZE = 1200
EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600
EXTRACXXFLAGS = -Wframe-larger-than=2200
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+1 -1
View File
@@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 5,
14000,
attitude_estimator_so3_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
src/modules/attitude_estimator_so3/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Wno-float-equal
src/modules/bottle_drop/bottle_drop.cpp
+7
View File
@@ -523,6 +523,9 @@ BottleDrop::task_main()
}
switch (_drop_state) {
case DROP_STATE_INIT:
// do nothing
break;
case DROP_STATE_TARGET_VALID:
{
@@ -689,6 +692,10 @@ BottleDrop::task_main()
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
break;
case DROP_STATE_BAY_CLOSED:
// do nothing
break;
}
counter++;
src/modules/commander/accelerometer_calibration.cpp
+5 -5
View File
@@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[5]) ? "down " : "",
(!data_collected[0]) ? "back " : "",
(!data_collected[1]) ? "front " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
(!data_collected[4]) ? "up " : "");
/* allow user enough time to read the message */
sleep(3);
src/modules/commander/commander.cpp
+82 -29
View File
@@ -81,7 +81,9 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -149,6 +151,9 @@ enum MAV_MODE_FLAG {
/* Mavlink file descriptors */
static int mavlink_fd = 0;
/* Syste autostart ID */
static int autostart_id;
/* flags */
static bool commander_initialized = false;
static volatile bool thread_should_exit = false; /**< daemon exit flag */
@@ -263,7 +268,7 @@ int commander_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 40,
3200,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
while (!thread_running) {
usleep(200);
@@ -310,12 +315,16 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "arm")) {
arm_disarm(true, mavlink_fd, "command line");
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(true, mavlink_fd_local, "command line");
close(mavlink_fd_local);
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
arm_disarm(false, mavlink_fd, "command line");
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(false, mavlink_fd_local, "command line");
close(mavlink_fd_local);
exit(0);
}
@@ -728,6 +737,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
/* welcome user */
warnx("starting");
@@ -917,6 +927,11 @@ int commander_thread_main(int argc, char *argv[])
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
/* Subscribe to geofence result topic */
int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
struct geofence_result_s geofence_result;
memset(&geofence_result, 0, sizeof(geofence_result));
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -1010,6 +1025,13 @@ int commander_thread_main(int argc, char *argv[])
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
/* Subscribe to vtol vehicle status topic */
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
struct vtol_vehicle_status_s vtol_status;
memset(&vtol_status, 0, sizeof(vtol_status));
vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1066,7 +1088,10 @@ int commander_thread_main(int argc, char *argv[])
status.system_type == VEHICLE_TYPE_TRICOPTER ||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
} else {
@@ -1102,6 +1127,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_autostart_id, &autostart_id);
}
orb_check(sp_man_sub, &updated);
@@ -1227,6 +1253,19 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* update vtol vehicle status*/
orb_check(vtol_vehicle_status_sub, &updated);
if (updated) {
/* vtol status changed */
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
/* update global position estimate */
orb_check(global_position_sub, &updated);
@@ -1512,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
/* Check for geofence violation */
if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
//XXX: make this configurable to select different actions (e.g. navigation modes)
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
}
/* start geofence result check */
orb_check(geofence_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
}
/* Check for geofence violation */
if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
//XXX: make this configurable to select different actions (e.g. navigation modes)
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
/* RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
@@ -1544,7 +1590,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC signal regained");
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
status_changed = true;
}
}
@@ -1645,8 +1691,9 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp=sp_man.timestamp;
status_changed = true;
}
}
@@ -1663,7 +1710,7 @@ int commander_thread_main(int argc, char *argv[])
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
mavlink_log_info(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
have_link = true;
@@ -1677,7 +1724,7 @@ int commander_thread_main(int argc, char *argv[])
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
mavlink_log_info(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true;
}
}
@@ -1692,7 +1739,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
@@ -2184,7 +2231,13 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
/* TODO: check this */
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
if (autostart_id < 13000 || autostart_id >= 14000) {
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
} else {
control_mode.flag_external_manual_override_ok = false;
}
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
src/modules/commander/module.mk
+3
View File
@@ -51,3 +51,6 @@ SRCS = commander.cpp \
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wframe-larger-than=1900
src/modules/commander/state_machine_helper.cpp
+1 -1
View File
@@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
}
src/modules/dataman/dataman.c
+5 -9
View File
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
{
work_q_item_t *work;
/* inform about start */
warnx("Initializing..");
/* Initialize global variables */
g_key_offsets[0] = 0;
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
}
else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
}
else
} else {
warnx("Unknown restart");
}
else
}
} else {
warnx("Unknown restart");
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
src/modules/ekf_att_pos_estimator/module.mk
+2 -1
View File
@@ -42,4 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
estimator_23states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+1 -1
View File
@@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 10,
5120,
control_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
src/modules/fw_att_control/fw_att_control_main.cpp
+135 -46
View File
@@ -35,8 +35,9 @@
* @file fw_att_control_main.c
* Implementation of a generic attitude controller based on classic orthogonal PIDs.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
@@ -92,12 +93,12 @@ public:
FixedwingAttitudeControl();
/**
* Destructor, also kills the sensors task.
* Destructor, also kills the main task.
*/
~FixedwingAttitudeControl();
/**
* Start the sensors task.
* Start the main task.
*
* @return OK on success.
*/
@@ -112,9 +113,9 @@ public:
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_should_exit; /**< if true, attitude control task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle for sensor task */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
@@ -130,11 +131,15 @@ private:
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
@@ -189,6 +194,8 @@ private:
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
param_t autostart_id; /* indicates which airframe is used */
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -228,6 +235,8 @@ private:
param_t pitchsp_offset_deg;
param_t man_roll_max;
param_t man_pitch_max;
param_t autostart_id; /* indicates which airframe is used */
} _parameter_handles; /**< handles for interesting parameters */
@@ -289,7 +298,7 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main attitude controller collection task.
*/
void task_main();
@@ -327,7 +336,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_rate_sp_pub(-1),
_attitude_sp_pub(-1),
_actuators_0_pub(-1),
_actuators_1_pub(-1),
_actuators_2_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -341,6 +350,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_att = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
@@ -386,8 +396,19 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
_parameter_handles.autostart_id = param_find("SYS_AUTOSTART");
/* fetch initial parameter values */
parameters_update();
// set correct uORB ID, depending on if vehicle is VTOL or not
if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
@@ -462,6 +483,7 @@ FixedwingAttitudeControl::parameters_update()
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.autostart_id, &_parameters.autostart_id);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
@@ -497,7 +519,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
{
bool vcontrol_mode_updated;
/* Check HIL state if vehicle status has changed */
/* Check if vehicle control mode has changed */
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
if (vcontrol_mode_updated) {
@@ -529,7 +551,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
}
}
@@ -679,6 +700,65 @@ FixedwingAttitudeControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_parameters.autostart_id >= 13000
&& _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude!
/* The following modification to the attitude is vehicle specific and in this case applies
to tail-sitter models !!!
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
//move z to x
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
//move x to z
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
//change direction of pitch (convert to right handed system)
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
//fill in new attitude data
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
_att.R[0][0] = R_adapted(0, 0);
_att.R[0][1] = R_adapted(0, 1);
_att.R[0][2] = R_adapted(0, 2);
_att.R[1][0] = R_adapted(1, 0);
_att.R[1][1] = R_adapted(1, 1);
_att.R[1][2] = R_adapted(1, 2);
_att.R[2][0] = R_adapted(2, 0);
_att.R[2][1] = R_adapted(2, 1);
_att.R[2][2] = R_adapted(2, 2);
// lastly, roll- and yawspeed have to be swaped
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
@@ -696,7 +776,7 @@ FixedwingAttitudeControl::task_main()
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
lock_integrator = false;
} else {
@@ -705,10 +785,10 @@ FixedwingAttitudeControl::task_main()
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[1] = 1.0f;
_actuators_airframe.control[7] = 1.0f;
// warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[1] = 0.0f;
_actuators_airframe.control[7] = 0.0f;
// warnx("_actuators_airframe.control[1] = -1.0f;");
}
@@ -751,14 +831,32 @@ FixedwingAttitudeControl::task_main()
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_velocity_enabled ||
_vcontrol_mode.flag_control_position_enabled ||
if (_vcontrol_mode.flag_control_auto_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
@@ -802,18 +900,18 @@ FixedwingAttitudeControl::task_main()
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed) {
if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
@@ -915,20 +1013,18 @@ FixedwingAttitudeControl::task_main()
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = _yaw_ctrl.get_desired_rate();
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
rates_sp.timestamp = hrt_absolute_time();
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
} else {
@@ -948,28 +1044,21 @@ FixedwingAttitudeControl::task_main()
_actuators.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp = hrt_absolute_time();
/* publish the actuator controls */
if (_actuators_0_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub > 0) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
if (_actuators_1_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
} else {
/* advertise and publish */
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
}
}
loop_counter++;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+124 -27
View File
@@ -163,6 +163,9 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
float _hold_alt; /**< hold altitude for velocity mode */
hrt_abstime _control_position_last_called; /**<last call of control_position */
/* land states */
bool land_noreturn_horizontal;
bool land_noreturn_vertical;
@@ -197,7 +200,11 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_POSITION,
FW_POSCTRL_MODE_OTHER
} _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
struct {
float l1_period;
@@ -316,6 +323,11 @@ private:
*/
void vehicle_status_poll();
/**
* Check for manual setpoint updates.
*/
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
*/
@@ -439,6 +451,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
_hold_alt(0.0f),
_control_position_last_called(0),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
land_stayonground(false),
@@ -458,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos_valid(false),
_l1_control(),
_mTecs(),
_was_pos_control_mode(false)
_control_mode_current(FW_POSCTRL_MODE_OTHER)
{
_nav_capabilities.turn_distance = 0.0f;
@@ -692,6 +707,22 @@ FixedwingPositionControl::vehicle_airspeed_poll()
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
bool manual_updated;
/* Check if manual setpoint has changed */
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
}
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -852,6 +883,12 @@ bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = FLT_MIN; // Using non zero value to a avoid division by zero
if (_control_position_last_called > 0) {
dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
_control_position_last_called = hrt_absolute_time();
bool setpoint = true;
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
@@ -873,21 +910,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
if (_control_mode.flag_control_auto_enabled &&
pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (pos_sp_triplet.current.valid) {
if (!_was_pos_control_mode) {
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_control_mode_current = FW_POSCTRL_MODE_AUTO;
_was_pos_control_mode = true;
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1169,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(_parameters.airspeed_trim),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
/* Tell the attitude controller to stop integrating while we are waiting
@@ -1209,12 +1247,69 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else {
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
_was_pos_control_mode = false;
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
/* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* Get demanded vertical velocity from pitch control */
static bool was_in_deadband = false;
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
was_in_deadband = true;
}
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed,
TECS_MODE_NORMAL);
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
/** MANUAL FLIGHT **/
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* no flight mode applies, do not publish an attitude setpoint */
setpoint = false;
@@ -1225,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
/* Copy thrust output for publication */
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
@@ -1241,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
@@ -1313,8 +1411,6 @@ FixedwingPositionControl::task_main()
continue;
}
perf_begin(_loop_perf);
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
@@ -1333,6 +1429,7 @@ FixedwingPositionControl::task_main()
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
@@ -1350,6 +1447,7 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
@@ -1386,10 +1484,9 @@ FixedwingPositionControl::task_main()
}
}
perf_end(_loop_perf);
}
perf_end(_loop_perf);
}
_task_running = false;
src/modules/fw_pos_control_l1/module.mk
+4
View File
@@ -45,3 +45,7 @@ SRCS = fw_pos_control_l1_main.cpp \
mtecs/mTecs_params.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wno-float-equal
src/modules/mavlink/mavlink_ftp.cpp
+1 -1
View File
@@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req)
/// @brief Copy file (with limited space)
int
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
{
char buff[512];
int src_fd = -1, dst_fd = -1;
src/modules/mavlink/mavlink_ftp.h
+1 -1
View File
@@ -142,7 +142,7 @@ private:
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
int _copy_file(const char *src_path, const char *dst_path, ssize_t length);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
src/modules/mavlink/mavlink_main.cpp
+2 -2
View File
@@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 5.0f);
configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:
@@ -1638,7 +1638,7 @@ Mavlink::start(int argc, char *argv[])
SCHED_PRIORITY_DEFAULT,
2800,
(main_t)&Mavlink::start_helper,
(const char **)argv);
(char * const *)argv);
// Ensure that this shell command
// does not return before the instance
src/modules/mavlink/mavlink_messages.cpp
+40 -29
View File
@@ -810,9 +810,6 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_combined_sub;
uint64_t _sensor_combined_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -828,9 +825,7 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0),
_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_combined_time(0)
_airspeed_time(0)
{}
void send(const hrt_abstime t)
@@ -840,14 +835,12 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@@ -856,7 +849,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = sensor_combined.baro_alt_meter;
msg.alt = pos.alt;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -1358,7 +1351,10 @@ protected:
/* scale outputs depending on system type */
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR) {
system_type == MAV_TYPE_OCTOROTOR ||
system_type == MAV_TYPE_VTOL_DUOROTOR ||
system_type == MAV_TYPE_VTOL_QUADROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
@@ -1372,6 +1368,14 @@ protected:
n = 6;
break;
case MAV_TYPE_VTOL_DUOROTOR:
n = 2;
break;
case MAV_TYPE_VTOL_QUADROTOR:
n = 4;
break;
default:
n = 8;
break;
@@ -1760,6 +1764,9 @@ protected:
case RC_INPUT_SOURCE_PX4IO_ST24:
msg.rssi |= (3 << 4);
break;
case RC_INPUT_SOURCE_UNKNOWN:
// do nothing
break;
}
if (rc.rc_lost) {
@@ -1834,33 +1841,32 @@ protected:
}
};
class MavlinkStreamOpticalFlow : public MavlinkStream
class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamOpticalFlow::get_name_static();
return MavlinkStreamOpticalFlowRad::get_name_static();
}
static const char *get_name_static()
{
return "OPTICAL_FLOW";
return "OPTICAL_FLOW_RAD";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_OPTICAL_FLOW;
return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamOpticalFlow(mavlink);
return new MavlinkStreamOpticalFlowRad(mavlink);
}
unsigned get_size()
{
return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
@@ -1868,11 +1874,11 @@ private:
uint64_t _flow_time;
/* do not allow top copying this class */
MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
protected:
explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
_flow_time(0)
{}
@@ -1882,18 +1888,23 @@ protected:
struct optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) {
mavlink_optical_flow_t msg;
mavlink_optical_flow_rad_t msg;
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
msg.flow_x = flow.flow_raw_x;
msg.flow_y = flow.flow_raw_y;
msg.flow_comp_m_x = flow.flow_comp_x_m;
msg.flow_comp_m_y = flow.flow_comp_y_m;
msg.integrated_x = flow.pixel_flow_x_integral;
msg.integrated_y = flow.pixel_flow_y_integral;
msg.integrated_xgyro = flow.gyro_x_rate_integral;
msg.integrated_ygyro = flow.gyro_y_rate_integral;
msg.integrated_zgyro = flow.gyro_z_rate_integral;
msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
msg.ground_distance = flow.ground_distance_m;
msg.integration_time_us = flow.integration_timespan;
msg.sensor_id = flow.sensor_id;
msg.time_delta_distance_us = flow.time_since_last_sonar_update;
msg.temperature = flow.gyro_temperature;
_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
}
}
};
@@ -2165,7 +2176,7 @@ protected:
msg.id = 0;
msg.orientation = 0;
msg.min_distance = range.minimum_distance * 100;
msg.max_distance = range.minimum_distance * 100;
msg.max_distance = range.maximum_distance * 100;
msg.current_distance = range.distance * 100;
msg.covariance = 20;
@@ -2199,7 +2210,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
src/modules/mavlink/mavlink_mission.cpp
+7 -4
View File
@@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
void
MavlinkMissionManager::send(const hrt_abstime now)
{
/* update interval for slow rate limiter */
_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
bool updated = false;
orb_check(_mission_result_sub, &updated);
@@ -312,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
send_mission_current(_current_seq);
if (mission_result.item_do_jump_changed) {
/* send a mission item again if the remaining DO_JUMPs has changed */
send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
(uint16_t)mission_result.item_changed_index);
}
} else {
if (_slow_rate_limiter.check(now)) {
send_mission_current(_current_seq);
@@ -811,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
break;
default:
src/modules/mavlink/mavlink_receiver.cpp
+24 -16
View File
@@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
handle_message_optical_flow_rad(msg);
break;
case MAVLINK_MSG_ID_SET_MODE:
@@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
{
/* optical flow */
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
mavlink_optical_flow_rad_t flow;
mavlink_msg_optical_flow_rad_decode(msg, &flow);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.timestamp = flow.time_usec;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.integrated_x;
f.flow_raw_y = flow.integrated_y;
f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -112,7 +112,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
src/modules/mc_att_control/mc_att_control_main.cpp
+62 -16
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
@@ -96,12 +97,12 @@ public:
MulticopterAttitudeControl();
/**
* Destructor, also kills the sensors task.
* Destructor, also kills the main task
*/
~MulticopterAttitudeControl();
/**
* Start the sensors task.
* Start the multicopter attitude control task.
*
* @return OK on success.
*/
@@ -109,8 +110,8 @@ public:
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
@@ -119,11 +120,15 @@ private:
int _params_sub; /**< parameter updates subscription */
int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
@@ -133,6 +138,7 @@ private:
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator controls */
struct actuator_armed_s _armed; /**< actuator arming status */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -168,6 +174,8 @@ private:
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t autostart_id; //what frame are we using?
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -182,6 +190,8 @@ private:
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
param_t autostart_id;
} _params;
/**
@@ -229,6 +239,11 @@ private:
*/
void control_attitude_rates(float dt);
/**
* Check for vehicle status updates.
*/
void vehicle_status_poll();
/**
* Shim for calling task_main from task_create.
*/
@@ -264,6 +279,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_vehicle_status_sub(-1),
/* publications */
_att_sp_pub(-1),
@@ -283,6 +299,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators, 0, sizeof(_actuators));
memset(&_armed, 0, sizeof(_armed));
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
_vehicle_status.is_rotary_wing = true;
_params.att_p.zero();
_params.rate_p.zero();
@@ -295,6 +313,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.man_yaw_max = 0.0f;
_params.acro_rate_max.zero();
_params.autostart_id = 0; //default
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
@@ -324,8 +344,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.autostart_id = param_find("SYS_AUTOSTART");
/* fetch initial parameter values */
parameters_update();
// set correct uORB ID, depending on if vehicle is VTOL or not
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -409,6 +440,8 @@ MulticopterAttitudeControl::parameters_update()
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
param_get(_params_handles.autostart_id, &_params.autostart_id);
return OK;
}
@@ -417,7 +450,7 @@ MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
@@ -432,7 +465,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
/* Check if vehicle control mode has changed */
orb_check(_v_control_mode_sub, &updated);
if (updated) {
@@ -489,6 +522,18 @@ MulticopterAttitudeControl::arming_status_poll()
}
}
void
MulticopterAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
/*
* Attitude controller.
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -585,7 +630,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
/* publish the attitude setpoint if needed */
if (publish_att_sp) {
if (publish_att_sp && _vehicle_status.is_rotary_wing) {
_v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
@@ -682,7 +727,7 @@ void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed) {
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
@@ -721,8 +766,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
void
MulticopterAttitudeControl::task_main()
{
warnx("started");
fflush(stdout);
/*
* do subscriptions
@@ -734,6 +777,7 @@ MulticopterAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* initialize parameters cache */
parameters_update();
@@ -785,6 +829,7 @@ MulticopterAttitudeControl::task_main()
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
@@ -797,10 +842,10 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
@@ -821,10 +866,10 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
@@ -849,11 +894,12 @@ MulticopterAttitudeControl::task_main()
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
}
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -7
View File
@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
}
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
@@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt)
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
@@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt)
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
if (_control_mode.flag_control_altitude_enabled) {
if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
/* Control altitude */
_pos_sp(2) = _pos_sp_triplet.current.z;
} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
@@ -857,10 +858,8 @@ MulticopterPositionControl::control_auto(float dt)
void
MulticopterPositionControl::task_main()
{
warnx("started");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mpc] started");
/*
* do subscriptions
src/modules/navigator/datalinkloss.cpp
+4 -4
View File
@@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
@@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
@@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
warnx("time is up, state should have been changed manually by now");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
src/modules/navigator/geofence.cpp
+13 -4
View File
@@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
/* if the line starts with #, skip */
if (line[textStart] == commentChar)
if (line[textStart] == commentChar) {
continue;
}
/* if there is only a linefeed, skip it */
if (line[0] == '\n') {
continue;
}
if (gotVertical) {
/* Parse the line as a geofence point */
@@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
/* Handle degree minute second format */
float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
warnx("Scanf to parse DMS geofence vertex failed.");
return ERROR;
}
// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
@@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)
} else {
/* Handle decimal degree format */
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
warnx("Scanf to parse geofence vertex failed.");
return ERROR;
}
}
if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
src/modules/navigator/gpsfailure.cpp
+1 -1
View File
@@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
warnx("gps fail: request flight termination");
}
default:
src/modules/navigator/mission.cpp
+17 -12
View File
@@ -38,6 +38,7 @@
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
*/
#include <sys/types.h>
@@ -149,18 +150,12 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
set_mission_item_reached();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
} else {
/* else just report that item reached */
if (_mission_type == MISSION_TYPE_OFFBOARD) {
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
set_mission_item_reached();
}
}
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@@ -395,7 +390,6 @@ Mission::set_mission_items()
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached();
set_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
@@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
"ERROR DO JUMP waypoint could not be written");
return false;
}
report_do_jump_mission_changed(*mission_index_ptr,
mission_item_tmp.do_jump_repeat_count);
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -706,23 +702,32 @@ Mission::save_offboard_mission_state()
dm_unlock(DM_KEY_MISSION_STATE);
}
void
Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
{
/* inform about the change */
_navigator->get_mission_result()->item_do_jump_changed = true;
_navigator->get_mission_result()->item_changed_index = index;
_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
_navigator->set_mission_result_updated();
}
void
Mission::set_mission_item_reached()
{
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
}
void
Mission::set_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
save_offboard_mission_state();
}
@@ -731,5 +736,5 @@ void
Mission::set_mission_finished()
{
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
}
src/modules/navigator/mission.h
+6
View File
@@ -38,6 +38,7 @@
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
*/
#ifndef NAVIGATOR_MISSION_H
@@ -130,6 +131,11 @@ private:
*/
void save_offboard_mission_state();
/**
* Inform about a changed mission item after a DO_JUMP
*/
void report_do_jump_mission_changed(int index, int do_jumps_remaining);
/**
* Set a mission item as reached
*/
src/modules/navigator/module.mk
+2
View File
@@ -62,3 +62,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wno-sign-compare
src/modules/navigator/navigator.h
+15 -3
View File
@@ -54,6 +54,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include "navigator_mode.h"
@@ -107,9 +108,9 @@ public:
void load_fence_from_file(const char *filename);
/**
* Publish the mission result so commander and mavlink know what is going on
* Publish the geofence result
*/
void publish_mission_result();
void publish_geofence_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
@@ -122,6 +123,7 @@ public:
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
void set_mission_result_updated() { _mission_result_updated = true; }
/**
* Getters
@@ -134,6 +136,7 @@ public:
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +167,7 @@ private:
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
orb_advert_t _geofence_result_pub;
orb_advert_t _att_sp_pub; /**< publish att sp
used only in very special failsafe modes
when pos control is deactivated */
@@ -179,7 +183,8 @@ private:
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
mission_result_s _mission_result;
vehicle_attitude_setpoint_s _att_sp;
geofence_result_s _geofence_result;
vehicle_attitude_setpoint_s _att_sp;
bool _mission_item_valid; /**< flags if the current mission item is valid */
@@ -206,6 +211,7 @@ private:
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated; /**< flags if mission result has seen an update */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
@@ -271,6 +277,12 @@ private:
*/
void publish_position_setpoint_triplet();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/
src/modules/navigator/navigator_main.cpp
+33 -4
View File
@@ -110,6 +110,7 @@ Navigator::Navigator() :
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_geofence_result_pub(-1),
_att_sp_pub(-1),
_vstatus{},
_control_mode{},
@@ -138,6 +139,7 @@ Navigator::Navigator() :
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_pos_sp_triplet_published_invalid_once(false),
_mission_result_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -398,8 +400,8 @@ Navigator::task_main()
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
publish_mission_result();
_geofence_result.geofence_violated = true;
publish_geofence_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -408,8 +410,8 @@ Navigator::task_main()
}
} else {
/* inform other apps via the mission result */
_mission_result.geofence_violated = false;
publish_mission_result();
_geofence_result.geofence_violated = false;
publish_geofence_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -490,6 +492,11 @@ Navigator::task_main()
_pos_sp_triplet_updated = false;
}
if (_mission_result_updated) {
publish_mission_result();
_mission_result_updated = false;
}
perf_end(_loop_perf);
}
warnx("exiting.");
@@ -639,6 +646,28 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset some of the flags */
_mission_result.seq_reached = false;
_mission_result.seq_current = 0;
_mission_result.item_do_jump_changed = false;
_mission_result.item_changed_index = 0;
_mission_result.item_do_jump_remaining = 0;
}
void
Navigator::publish_geofence_result()
{
/* lazily publish the geofence result only once available */
if (_geofence_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
} else {
/* advertise and publish */
_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
}
}
void
src/modules/navigator/navigator_mode.cpp
+1 -1
View File
@@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
on_activation();
} else {
src/modules/navigator/rcloss.cpp
+3 -3
View File
@@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -162,7 +162,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
}
break;
@@ -171,7 +171,7 @@ RCLoss::advance_rcl()
warnx("time is up, no RC regain, terminating");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
src/modules/position_estimator_inav/module.mk
+3
View File
@@ -41,3 +41,6 @@ SRCS = position_estimator_inav_main.c \
inertial_filter.c
MODULE_STACKSIZE = 1200
EXTRACFLAGS = -Wframe-larger-than=3500
src/modules/position_estimator_inav/position_estimator_inav_main.c
+16 -20
View File
@@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[])
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL);
(argv) ? (char * const *) &argv[2] : (char * const *) NULL);
exit(0);
}
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = true;
} else {
warnx("app not started");
warnx("not started");
}
exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("app is running");
warnx("is running");
} else {
warnx("app not started");
warnx("not started");
}
exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started");
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
@@ -298,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement
//hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
@@ -389,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", (double)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
warnx("baro offs: %d", (int)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@@ -491,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
// flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
@@ -520,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
}
} else {
@@ -550,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
//todo check direction of x und y axis
flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
@@ -750,8 +749,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* initialize projection */
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
@@ -1081,10 +1081,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false;
landed_time = 0;
}
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
src/modules/px4iofirmware/mixer.cpp
+10 -10
View File
@@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0;
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
/* abort if we're in the mixer */
/* disable mixing, will be enabled once load is complete */
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
/* abort if we're in the mixer - the caller is expected to retry */
if (in_mixer) {
return 1;
}
@@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
if (length < sizeof(px4io_mixdata)) {
return 0;
}
unsigned text_length = length - sizeof(px4io_mixdata);
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* FIRST mark the mixer as invalid */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* THEN actually delete it */
mixer_group.reset();
mixer_text_length = 0;
@@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* disable mixing during the update */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
src/modules/px4iofirmware/protocol.h
+2
View File
@@ -33,6 +33,8 @@
#pragma once
#include <inttypes.h>
/**
* @file protocol.h
*
src/modules/px4iofirmware/registers.c
+8 -10
View File
@@ -407,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return mixer_handle_text(values, num_values * sizeof(*values));
}
break;
/* do not change the mixer if FMU is armed and IO's safety is off
* this state defines an active system. This check is done in the
* text handling function.
*/
return mixer_handle_text(values, num_values * sizeof(*values));
default:
/* avoid offset wrap */
@@ -583,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
// don't allow reboot while armed
break;
}
@@ -630,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/**
* do not allow a RC config change while outputs armed
* do not allow a RC config change while safety is off
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
break;
}
src/modules/sdlog2/module.mk
+3
View File
@@ -45,3 +45,6 @@ SRCS = sdlog2.c \
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACFLAGS = -Wframe-larger-than=1200
src/modules/sdlog2/sdlog2.c
+43 -6
View File
@@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[])
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
(char * const *)argv);
exit(0);
}
@@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg)
/* set name */
prctl(PR_SET_NAME, "sdlog2_writer", 0);
perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write");
int log_fd = open_log_file();
if (log_fd < 0) {
@@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg)
n = available;
}
perf_begin(perf_write);
n = write(log_fd, read_ptr, n);
perf_end(perf_write);
should_wait = (n == available) && !is_part;
@@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg)
fsync(log_fd);
close(log_fd);
/* free performance counter */
perf_free(perf_write);
return NULL;
}
@@ -934,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_s act_controls1;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
@@ -1015,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_1_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
@@ -1048,6 +1057,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -1079,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[])
if (_extended_logging) {
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
} else {
subs.sat_info_sub = 0;
}
/* close non-needed fd's */
@@ -1368,6 +1380,18 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
// secondary attitude
log_msg.msg_type = LOG_ATT2_MSG;
log_msg.body.log_ATT.roll = buf.att.roll_sec;
log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
/* --- ATTITUDE SETPOINT --- */
@@ -1406,6 +1430,16 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL FW VTOL --- */
if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
log_msg.msg_type = LOG_ATC1_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- LOCAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
log_msg.msg_type = LOG_LPOS_MSG;
@@ -1511,11 +1545,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
src/modules/sdlog2/sdlog2_messages.h
+18 -9
View File
@@ -50,6 +50,7 @@
#pragma pack(push, 1)
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
#define LOG_ATT2_MSG 41
struct log_ATT_s {
float roll;
float pitch;
@@ -149,6 +150,7 @@ struct log_GPS_s {
/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
#define LOG_ATTC_MSG 9
#define LOG_ATC1_MSG 40
struct log_ATTC_s {
float roll;
float pitch;
@@ -200,13 +202,19 @@ struct log_ARSP_s {
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
float flow_comp_x;
float flow_comp_y;
float distance;
uint8_t quality;
uint64_t timestamp;
uint8_t sensor_id;
float pixel_flow_x_integral;
float pixel_flow_y_integral;
float gyro_x_rate_integral;
float gyro_y_rate_integral;
float gyro_z_rate_integral;
float ground_distance_m;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t frame_count_since_last_readout;
int16_t gyro_temperature;
uint8_t quality;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
@@ -416,7 +424,6 @@ struct log_ENCD_s {
float vel1;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -444,7 +451,8 @@ struct log_PARM_s {
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
@@ -453,7 +461,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
src/modules/segway/segway_main.cpp
+1 -1
View File
@@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 10,
5120,
segway_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
src/modules/sensors/module.mk
+2
View File
@@ -44,3 +44,5 @@ SRCS = sensors.cpp \
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wno-type-limits
src/modules/systemlib/mixer/mixer.h
+1
View File
@@ -466,6 +466,7 @@ public:
OCTA_X,
OCTA_PLUS,
OCTA_COX,
TWIN_ENGINE, /**< VTOL: one engine on each wing */
MAX_GEOMETRY
};
src/modules/systemlib/mixer/mixer_multirotor.cpp
+15 -4
View File
@@ -76,6 +76,7 @@ float constrain(float val, float min, float max)
/*
* These tables automatically generated by multi_tables - do not edit.
*/
const MultirotorMixer::Rotor _config_quad_x[] = {
{ -0.707107, 0.707107, 1.00 },
{ 0.707107, -0.707107, 1.00 },
@@ -88,11 +89,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
{ 0.000000, 1.000000, -1.00 },
{ -0.000000, -1.000000, -1.00 },
};
//Add table for quad in V configuration, which is not generated by multi_tables!
const MultirotorMixer::Rotor _config_quad_v[] = {
{ -0.927184, 0.374607, 1.00 },
{ 0.694658, -0.719340, 1.00 },
{ 0.927184, 0.374607, -1.00 },
{ -0.694658, -0.719340, -1.00 },
{ -0.3223, 0.9466, 0.4242 },
{ 0.3223, -0.9466, 1.0000 },
{ 0.3223, 0.9466, -0.4242 },
{ -0.3223, -0.9466, -1.0000 },
};
const MultirotorMixer::Rotor _config_quad_wide[] = {
{ -0.927184, 0.374607, 1.00 },
@@ -154,6 +156,11 @@ const MultirotorMixer::Rotor _config_octa_cox[] = {
{ -0.707107, -0.707107, 1.00 },
{ 0.707107, -0.707107, -1.00 },
};
const MultirotorMixer::Rotor _config_duorotor[] = {
{ -1.000000, 0.000000, 0.00 },
{ 1.000000, 0.000000, 0.00 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
@@ -165,6 +172,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_octa_x[0],
&_config_octa_plus[0],
&_config_octa_cox[0],
&_config_duorotor[0],
};
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
4, /* quad_x */
@@ -177,6 +185,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
8, /* octa_x */
8, /* octa_plus */
8, /* octa_cox */
2, /* twin_engine */
};
}
@@ -274,6 +283,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "8c")) {
geometry = MultirotorMixer::OCTA_COX;
} else if (!strcmp(geomname, "2-")) {
geometry = MultirotorMixer::TWIN_ENGINE;
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
src/modules/systemlib/mixer/multi_tables
+4 -7
View File
@@ -6,6 +6,7 @@
proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
proc rcos {a} { expr cos([rad $a])}
set quad_x {
45 CCW
-135 CCW
@@ -20,12 +21,6 @@ set quad_plus {
180 CW
}
set quad_v {
68 CCW
-136 CCW
-68 CW
136 CW
}
set quad_wide {
68 CCW
@@ -94,7 +89,9 @@ set octa_cox {
-135 CW
}
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
src/modules/systemlib/module.mk
+2
View File
@@ -57,3 +57,5 @@ SRCS = err.c \
mcu_version.c
MAXOPTIMIZATION = -Os
EXTRACFLAGS = -Wno-sign-compare
src/modules/systemlib/perf_counter.c
+42 -11
View File
@@ -41,7 +41,7 @@
#include <stdio.h>
#include <sys/queue.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "perf_counter.h"
/**
@@ -84,7 +84,8 @@ struct perf_ctr_interval {
uint64_t time_last;
uint64_t time_least;
uint64_t time_most;
float mean;
float M2;
};
/**
@@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name)
case PC_INTERVAL:
ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
break;
default:
@@ -156,15 +158,23 @@ perf_count(perf_counter_t handle)
case 1:
pci->time_least = now - pci->time_last;
pci->time_most = now - pci->time_last;
pci->mean = pci->time_least / 1e6f;
pci->M2 = 0;
break;
default: {
hrt_abstime interval = now - pci->time_last;
if (interval < pci->time_least)
pci->time_least = interval;
if (interval > pci->time_most)
pci->time_most = interval;
break;
}
hrt_abstime interval = now - pci->time_last;
if (interval < pci->time_least)
pci->time_least = interval;
if (interval > pci->time_most)
pci->time_most = interval;
// maintain mean and variance of interval in seconds
// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
float dt = interval / 1e6f;
float delta_intvl = dt - pci->mean;
pci->mean += delta_intvl / pci->event_count;
pci->M2 += delta_intvl * (dt - pci->mean);
break;
}
}
pci->time_last = now;
pci->event_count++;
@@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
float rms = sqrtf(pci->M2 / (pci->event_count-1));
dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
handle->name,
pci->event_count,
(pci->time_last - pci->time_first) / pci->event_count,
pci->time_least,
pci->time_most);
pci->time_most,
(double)(1000 * pci->mean),
(double)(1000 * rms));
break;
}
@@ -365,6 +378,21 @@ perf_print_all(int fd)
}
}
extern const uint16_t latency_bucket_count;
extern uint32_t latency_counters[];
extern const uint16_t latency_buckets[];
void
perf_print_latency(int fd)
{
dprintf(fd, "bucket : events\n");
for (int i = 0; i < latency_bucket_count; i++) {
printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]);
}
// print the overflow bucket value
dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
}
void
perf_reset_all(void)
{
@@ -374,4 +402,7 @@ perf_reset_all(void)
perf_reset(handle);
handle = (perf_counter_t)sq_next(&handle->link);
}
for (int i = 0; i <= latency_bucket_count; i++) {
latency_counters[i] = 0;
}
}
src/modules/systemlib/perf_counter.h
+8 -1
View File
@@ -94,7 +94,7 @@ __EXPORT extern void perf_begin(perf_counter_t handle);
* End a performance event.
*
* This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
* If a call is made without a corresopnding perf_begin call, or if perf_cancel
* If a call is made without a corresponding perf_begin call, or if perf_cancel
* has been called subsequently, no change is made to the counter.
*
* @param handle The handle returned from perf_alloc.
@@ -142,6 +142,13 @@ __EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle);
*/
__EXPORT extern void perf_print_all(int fd);
/**
* Print hrt latency counters.
*
* @param fd File descriptor to print to - e.g. 0 for stdout
*/
__EXPORT extern void perf_print_latency(int fd);
/**
* Reset all of the performance counters.
*/
src/modules/systemlib/systemlib.c
+1 -1
View File
@@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
kill(tcb->pid, SIGUSR1);
}
int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[])
{
int pid;
src/modules/systemlib/systemlib.h
+2 -1
View File
@@ -41,6 +41,7 @@
#define SYSTEMLIB_H_
#include <float.h>
#include <stdint.h>
#include <sched.h>
__BEGIN_DECLS
@@ -63,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name,
int scheduler,
int stack_size,
main_t entry,
const char *argv[]);
char * const argv[]);
enum MULT_PORTS {
MULT_0_US2_RXTX = 0,
src/modules/uORB/Publication.cpp
+2
View File
@@ -46,6 +46,7 @@
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/actuator_outputs.h"
#include "topics/actuator_direct.h"
#include "topics/encoders.h"
#include "topics/tecs_status.h"
@@ -76,6 +77,7 @@ template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
src/modules/uORB/objects_common.cpp
+17 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2014 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
@@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
#include "topics/vtol_vehicle_status.h"
ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
#include "topics/safety.h"
ORB_DEFINE(safety, struct safety_s);
@@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -143,11 +148,16 @@ ORB_DEFINE(onboard_mission, struct mission_s);
#include "topics/mission_result.h"
ORB_DEFINE(mission_result, struct mission_result_s);
#include "topics/geofence_result.h"
ORB_DEFINE(geofence_result, struct geofence_result_s);
#include "topics/fence.h"
ORB_DEFINE(fence, unsigned);
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
@@ -182,6 +192,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
//Virtual control groups, used for VTOL operation
ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
@@ -192,6 +205,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
#include "topics/actuator_direct.h"
ORB_DEFINE(actuator_direct, struct actuator_direct_s);
#include "topics/multirotor_motor_limits.h"
ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
src/modules/uORB/topics/actuator_controls.h
+3
View File
@@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
ORB_DECLARE(actuator_controls_virtual_mc);
ORB_DECLARE(actuator_controls_virtual_fw);
#endif
src/modules/uORB/topics/actuator_direct.h
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file actuator_direct.h
*
* Actuator direct values.
*
* Values published to this topic are the direct actuator values which
* should be passed to actuators, bypassing mixing
*/
#ifndef TOPIC_ACTUATOR_DIRECT_H
#define TOPIC_ACTUATOR_DIRECT_H
#include <stdint.h>
#include "../uORB.h"
#define NUM_ACTUATORS_DIRECT 16
/**
* @addtogroup topics
* @{
*/
struct actuator_direct_s {
uint64_t timestamp; /**< timestamp in us since system boot */
float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */
unsigned nvalues; /**< number of valid values */
};
/**
* @}
*/
/* actuator direct ORB */
ORB_DECLARE(actuator_direct);
#endif // TOPIC_ACTUATOR_DIRECT_H
src/modules/uORB/topics/geofence_result.h
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (C) 2014 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.
*
****************************************************************************/
/**
* @file geofence_result.h
* Status of the plance concerning the geofence
*
* @author Ban Siesta <bansiesta@gmail.com>
*/
#ifndef TOPIC_GEOFENCE_RESULT_H
#define TOPIC_GEOFENCE_RESULT_H
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct geofence_result_s
{
bool geofence_violated; /**< true if the geofence is violated */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(geofence_result);
#endif
src/modules/uORB/topics/home_position.h
+1 -1
View File
@@ -61,7 +61,7 @@ struct home_position_s
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float alt; /**< Altitude in meters (AMSL) */
float x; /**< X coordinate in meters */
float y; /**< Y coordinate in meters */
src/modules/uORB/topics/mission.h
+1 -1
View File
@@ -83,7 +83,7 @@ struct mission_item_s {
bool altitude_is_relative; /**< true if altitude is relative from start point */
double lat; /**< latitude in degrees */
double lon; /**< longitude in degrees */
float altitude; /**< altitude in meters */
float altitude; /**< altitude in meters (AMSL) */
float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
src/modules/uORB/topics/mission_result.h
+9 -5
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012-2014 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
@@ -37,6 +34,11 @@
/**
* @file mission_result.h
* Mission results that navigator needs to pass on to commander and mavlink.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Ban Siesta <bansiesta@gmail.com>
*/
#ifndef TOPIC_MISSION_RESULT_H
@@ -58,8 +60,10 @@ struct mission_result_s
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
bool geofence_violated; /**< true if the geofence is violated */
bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */
bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */
unsigned item_changed_index; /**< indicate which item has changed */
unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
};
/**
src/modules/uORB/topics/optical_flow.h
+15 -9
View File
@@ -55,16 +55,22 @@
*/
struct optical_flow_s {
uint64_t timestamp; /**< in microseconds since system start */
uint64_t flow_timestamp; /**< timestamp from flow sensor */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint64_t timestamp; /**< in microseconds since system start */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */
float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */
float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint32_t integration_timespan; /**<accumulation timespan in microseconds */
uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
};
src/modules/uORB/topics/vehicle_attitude.h
+17
View File
@@ -80,6 +80,23 @@ struct vehicle_attitude_s {
bool R_valid; /**< Rotation matrix valid */
bool q_valid; /**< Quaternion valid */
// secondary attitude, use for VTOL
float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */
float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */
float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */
float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */
float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */
float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */
float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */
float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */
float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */
float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */
float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q_sec[4]; /**< Quaternion (NED) */
float g_comp_sec[3]; /**< Compensated gravity vector */
bool R_valid_sec; /**< Rotation matrix valid */
bool q_valid_sec; /**< Quaternion valid */
};
/**
src/modules/uORB/topics/vehicle_attitude_setpoint.h
+2
View File
@@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s {
/* register this as object request broker structure */
ORB_DECLARE(vehicle_attitude_setpoint);
ORB_DECLARE(mc_virtual_attitude_setpoint);
ORB_DECLARE(fw_virtual_attitude_setpoint);
#endif /* TOPIC_ARDRONE_CONTROL_H_ */
src/modules/uORB/topics/vehicle_command.h
+1 -1
View File
@@ -65,7 +65,7 @@ enum VEHICLE_CMD {
VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */
VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */
VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
src/modules/uORB/topics/vehicle_gps_position.h
+1 -1
View File
@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */
uint64_t timestamp_time; /**< Timestamp for time information */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
uint8_t satellites_used; /**< Number of satellites used */
};
src/modules/uORB/topics/vehicle_rates_setpoint.h
+2 -1
View File
@@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s {
/* register this as object request broker structure */
ORB_DECLARE(vehicle_rates_setpoint);
ORB_DECLARE(mc_virtual_rates_setpoint);
ORB_DECLARE(fw_virtual_rates_setpoint);
#endif
src/modules/uORB/topics/vehicle_status.h
+5 -1
View File
@@ -147,7 +147,10 @@ enum VEHICLE_TYPE {
VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
VEHICLE_TYPE_KITE = 17, /* Kite | */
VEHICLE_TYPE_ENUM_END = 18, /* | */
VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
VEHICLE_TYPE_ENUM_END = 21 /* | */
};
enum VEHICLE_BATTERY_WARNING {
@@ -201,6 +204,7 @@ struct vehicle_status_s {
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */
src/modules/uORB/topics/vtol_vehicle_status.h
+66
View File
@@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (c) 2013 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.
*
****************************************************************************/
/**
* @file vtol_status.h
*
* Vtol status topic
*
*/
#ifndef TOPIC_VTOL_STATUS_H
#define TOPIC_VTOL_STATUS_H
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/* Indicates in which mode the vtol aircraft is in */
struct vtol_vehicle_status_s {
uint64_t timestamp; /**< Microseconds since system boot */
bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vtol_vehicle_status);
#endif

Some files were not shown because too many files have changed in this diff Show More