From a30a5d2665d3d0f68262d7de68aa5d4086a42321 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 14:27:49 +0200 Subject: [PATCH 001/122] update attitude_estimator_ekf, includes matlab This adds the latest c implementation (matlab coder) of attitude_estimator_ekf, the .m matlab script and the .prj file with the settings to export the matlab code to c --- .../attitude_estimator_ekf/AttitudeEKF.m | 286 +++ .../attitudeKalmanfilter.prj | 503 ++++++ .../attitude_estimator_ekf_main.cpp | 33 +- .../attitude_estimator_ekf_params.c | 41 +- .../attitude_estimator_ekf_params.h | 10 +- .../codegen/AttitudeEKF.c | 1573 +++++++++++++++++ .../codegen/AttitudeEKF.h | 26 + .../codegen/AttitudeEKF_types.h | 17 + .../codegen/attitudeKalmanfilter.c | 1148 ------------ .../codegen/attitudeKalmanfilter.h | 34 - .../codegen/attitudeKalmanfilter_initialize.c | 31 - .../codegen/attitudeKalmanfilter_initialize.h | 34 - .../codegen/attitudeKalmanfilter_terminate.c | 31 - .../codegen/attitudeKalmanfilter_terminate.h | 34 - .../codegen/attitudeKalmanfilter_types.h | 16 - .../attitude_estimator_ekf/codegen/cross.c | 37 - .../attitude_estimator_ekf/codegen/cross.h | 34 - .../attitude_estimator_ekf/codegen/eye.c | 51 - .../attitude_estimator_ekf/codegen/eye.h | 35 - .../attitude_estimator_ekf/codegen/mrdivide.c | 357 ---- .../attitude_estimator_ekf/codegen/mrdivide.h | 36 - .../attitude_estimator_ekf/codegen/norm.c | 54 - .../attitude_estimator_ekf/codegen/norm.h | 34 - .../attitude_estimator_ekf/codegen/rdivide.c | 38 - .../attitude_estimator_ekf/codegen/rdivide.h | 34 - .../attitude_estimator_ekf/codegen/rtGetInf.c | 139 -- .../attitude_estimator_ekf/codegen/rtGetInf.h | 23 - .../attitude_estimator_ekf/codegen/rtGetNaN.c | 96 - .../attitude_estimator_ekf/codegen/rtGetNaN.h | 21 - .../codegen/rt_defines.h | 24 - .../codegen/rt_nonfinite.c | 87 - .../codegen/rt_nonfinite.h | 53 - .../attitude_estimator_ekf/codegen/rtwtypes.h | 319 ++-- src/modules/attitude_estimator_ekf/module.mk | 12 +- 34 files changed, 2632 insertions(+), 2669 deletions(-) create mode 100644 src/modules/attitude_estimator_ekf/AttitudeEKF.m create mode 100644 src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_defines.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h mode change 100755 => 100644 src/modules/attitude_estimator_ekf/codegen/rtwtypes.h diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 0000000000..1218cb65da --- /dev/null +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -0,0 +1,286 @@ +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]; + %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; + 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]; + %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); + 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]; + + %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); + 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]; + %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); + 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]; + diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100644 index 0000000000..a8315bf571 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,503 @@ + + + + + false + false + false + option.WorkingFolder.Project + + option.BuildFolder.Project + + + true + true + true + true + option.GlobalDataSyncMethod.SyncAlways + true + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + option.FilePartitionMethod.MapMFileToCFile + true + false + + true + false + true + false + + + + + + + + + true + false + 40000 + 100 + option.TargetLang.C + true + 10 + 200 + 4000 + true + 64 + true + true + option.ConstantInputs.CheckValues + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + option.WorkingFolder.Project + + option.BuildFolder.Specified + codegen + + true + false + false + false + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + false + option.FilePartitionMethod.SingleFile + true + true + false + option.DataTypeReplacement.CBuiltIn + false + true + option.ParenthesesLevel.Nominal + 31 + $M$N + $M$N + $M$N + $M$N + $M$N + $M$N + emxArray_$M$N + emx$M$N + + true + false + true + true + false + true + + + + + + + + + C89/C90 (ANSI) + true + ARM Compatible + ARM Cortex + true + 8 + 16 + 32 + 32 + 64 + 32 + 64 + 32 + 32 + option.HardwareEndianness.Little + true + false + option.HardwareAtomicIntegerSize.Long + option.HardwareAtomicFloatSize.Double + option.HardwareDivisionRounding.Undefined + Generic + MATLAB Host Computer + false + 8 + 16 + 32 + 64 + 64 + 32 + 64 + 64 + 64 + option.HardwareEndianness.Little + true + true + option.HardwareAtomicIntegerSize.Char + option.HardwareAtomicFloatSize.None + option.HardwareDivisionRounding.Zero + Automatically locate an installed toolchain + Faster Builds + + 40000 + 100 + true + option.TargetLang.C + option.CCompilerOptimization.On + + true + false + make_rtw + default_tmf + + 10 + 200 + 4000 + true + 64 + true + true + false + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + R2012a + true + t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html + /home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html + true + option.VerificationMode.None + false + ${PROJECT_ROOT}/AttitudeEKF_Test.m + ${PROJECT_ROOT}/AttitudeEKF_Test.m + false + false + 1024 + 2048 + AttitudeEKF_mex + AttitudeEKF + option.target.artifact.lib + option.FixedPointTypeProposalMode.ProposeFractionLengths + numerictype([],16,12) + 0 + true + false + false + false + true + false + + option.FixedPointMode.None + false + + + 16 + 4 + 0 + fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128) + option.FixedPointTypeSource.SimAndDerived + + false + false + false + true + + + + + + + + + true + _fixpt + option.DefaultFixedPointSignedness.Automatic + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 3 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 9 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 3 x 3 + false + + + + + + ${PROJECT_ROOT}/AttitudeEKF_Test.m + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + + + + /opt/matlab/r2013b + + + + + + true + false + false + false + false + false + true + false + 3.15.5-1-ARCH + false + true + glnxa64 + true + + + + diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6a..e1bbf5bc7a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -74,8 +75,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 } @@ -206,6 +206,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ + float debugOutput[4] = { 0.0f }; + // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); @@ -213,7 +215,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 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(); @@ -277,9 +279,9 @@ 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 = { 0 }; - 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); @@ -508,8 +510,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 + false, // use_inertia_matrix + 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])) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index bc0e3b93a2..5b57bfb4db 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -50,7 +50,6 @@ 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 */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); @@ -58,14 +57,38 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); /* mag measurement noise */ 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); + + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -73,17 +96,19 @@ 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"); + return OK; } @@ -93,17 +118,19 @@ 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])); + } + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 5985541cac..fbb6a18ff4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -42,8 +42,9 @@ #include struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; + float r[3]; + float q[4]; + float moment_inertia_J[9]; float roll_off; float pitch_off; float yaw_off; @@ -52,8 +53,9 @@ 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 mag_decl; param_t acc_comp; }; diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c new file mode 100644 index 0000000000..d568e57376 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -0,0 +1,1573 @@ +/* + * AttitudeEKF.c + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +/* Include files */ +#include "AttitudeEKF.h" + +/* Variable Definitions */ +static float Ji[9]; +static boolean_T Ji_not_empty; +static float x_apo[12]; +static float P_apo[144]; +static float Q[144]; +static boolean_T Q_not_empty; + +/* Function Declarations */ +static void AttitudeEKF_init(void); +static void b_mrdivide(const float A[72], const float B[36], float y[72]); +static void inv(const float x[9], float y[9]); +static void mrdivide(const float A[108], const float B[81], float y[108]); +static float norm(const float x[3]); + +/* Function Definitions */ +static void AttitudeEKF_init(void) +{ + int i; + static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + -9.81F, 1.0F, 0.0F, 0.0F }; + + for (i = 0; i < 12; i++) { + x_apo[i] = fv5[i]; + } + + for (i = 0; i < 144; i++) { + P_apo[i] = 200.0F; + } +} + +/* + * + */ +static void b_mrdivide(const float A[72], const float B[36], float y[72]) +{ + float b_A[36]; + signed char ipiv[6]; + int i1; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[72]; + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i1] = B[i1 + 6 * iy]; + } + + ipiv[i1] = (signed char)(1 + i1); + } + + 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] = (signed char)((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; + } + } + + i1 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i1; 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; + i1 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i1 = 0; i1 < 12; i1++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i1] = A[i1 + 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 (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i1] = Y[i1 + 6 * iy]; + } + } +} + +/* + * + */ +static void inv(const float x[9], float y[9]) +{ + float b_x[9]; + int p1; + int p2; + int p3; + float absx11; + float absx21; + float absx31; + int itmp; + for (p1 = 0; p1 < 9; p1++) { + b_x[p1] = x[p1]; + } + + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = (real32_T)fabs(x[0]); + absx21 = (real32_T)fabs(x[1]); + absx31 = (real32_T)fabs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + absx11 = b_x[1] / b_x[0]; + b_x[1] /= b_x[0]; + absx21 = b_x[2] / b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= absx11 * b_x[3]; + b_x[5] -= absx21 * b_x[3]; + b_x[7] -= absx11 * b_x[6]; + b_x[8] -= absx21 * b_x[6]; + if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + b_x[1] = absx21; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + absx11 = b_x[5] / b_x[4]; + b_x[5] /= b_x[4]; + b_x[8] -= absx11 * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0F / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; +} + +/* + * + */ +static void mrdivide(const float A[108], const float B[81], float y[108]) +{ + float b_A[81]; + signed char ipiv[9]; + int i0; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[108]; + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i0] = B[i0 + 9 * iy]; + } + + ipiv[i0] = (signed char)(1 + i0); + } + + 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] = (signed char)((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; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; 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; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i0 = 0; i0 < 12; i0++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i0] = A[i0 + 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 (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i0] = Y[i0 + 9 * iy]; + } + } +} + +/* + * + */ +static float norm(const float x[3]) +{ + float y; + float scale; + int k; + float absxk; + float 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); +} + +/* + * 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) + */ +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]) +{ + int i; + float fv0[3]; + int r2; + float zek[3]; + float muk[3]; + float b_muk[3]; + float c_muk[3]; + float fv1[3]; + float wak[3]; + float O[9]; + float b_O[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float fv2[3]; + float maxval; + int r1; + float fv3[9]; + float fv4[3]; + float x_apr[12]; + signed char I[144]; + float A_lin[144]; + static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float b_A_lin[144]; + float v[12]; + float P_apr[144]; + float b_P_apr[108]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float K_k[108]; + float a[81]; + static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[81]; + float c_a[81]; + float d_a[36]; + static const signed char e_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float c_r_gyro[9]; + float b_K_k[36]; + int r3; + float a21; + float f_a[36]; + float c_P_apr[72]; + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 }; + + float c_K_k[72]; + static const signed char g_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + + float b_z[6]; + static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1 }; + + static const signed char h_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; + + float i_a[6]; + float c_z[6]; + + /* 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 */ + /* 'AttitudeEKF:48' if isempty(Ji) */ + if (!Ji_not_empty) { + /* 'AttitudeEKF:49' Ji=single(inv(J)); */ + inv(J, Ji); + Ji_not_empty = TRUE; + } + + /* % init */ + /* 'AttitudeEKF:54' if(isempty(x_apo)) */ + /* 'AttitudeEKF:64' if(isempty(P_apo)) */ + /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ + for (i = 0; i < 4; i++) { + debugOutput[i] = 0.0F; + } + + /* % copy the states */ + /* 'AttitudeEKF:72' wx= x_apo(1); */ + /* x body angular rate */ + /* 'AttitudeEKF:73' wy= x_apo(2); */ + /* y body angular rate */ + /* 'AttitudeEKF:74' wz= x_apo(3); */ + /* z body angular rate */ + /* 'AttitudeEKF:76' wax= x_apo(4); */ + /* x body angular acceleration */ + /* 'AttitudeEKF:77' way= x_apo(5); */ + /* y body angular acceleration */ + /* 'AttitudeEKF:78' waz= x_apo(6); */ + /* z body angular acceleration */ + /* 'AttitudeEKF:80' zex= x_apo(7); */ + /* x component gravity vector */ + /* 'AttitudeEKF:81' zey= x_apo(8); */ + /* y component gravity vector */ + /* 'AttitudeEKF:82' zez= x_apo(9); */ + /* z component gravity vector */ + /* 'AttitudeEKF:84' mux= x_apo(10); */ + /* x component magnetic field vector */ + /* 'AttitudeEKF:85' muy= x_apo(11); */ + /* y component magnetic field vector */ + /* 'AttitudeEKF:86' muz= x_apo(12); */ + /* z component magnetic field vector */ + /* % prediction section */ + /* compute the apriori state estimate from the previous aposteriori estimate */ + /* body angular accelerations */ + /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ + if (use_inertia_matrix == 1) { + /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ + fv0[0] = x_apo[3]; + fv0[1] = x_apo[4]; + fv0[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += J[r2 + 3 * i] * fv0[i]; + } + } + + muk[0] = x_apo[3]; + muk[1] = x_apo[4]; + muk[2] = x_apo[5]; + b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; + b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; + b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; + for (r2 = 0; r2 < 3; r2++) { + c_muk[r2] = -b_muk[r2]; + } + + fv1[0] = x_apo[3]; + fv1[1] = x_apo[4]; + fv1[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + fv0[r2] = 0.0F; + for (i = 0; i < 3; i++) { + fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; + } + + wak[r2] = fv1[r2] + fv0[r2] * dt; + } + } else { + /* 'AttitudeEKF:96' else */ + /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ + wak[0] = x_apo[3]; + wak[1] = x_apo[4]; + wak[2] = x_apo[5]; + } + + /* body angular rates */ + /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_apo[2]; + O[2] = x_apo[1]; + O[3] = x_apo[2]; + O[4] = 0.0F; + O[5] = -x_apo[0]; + O[6] = -x_apo[1]; + O[7] = x_apo[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'AttitudeEKF:107' if (approx_prediction==1) */ + 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 */ + /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += b_O[r2 + 3 * i] * fv2[i]; + } + } + } else { + /* 'AttitudeEKF:112' else */ + /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += fv3[r2 + 3 * i] * fv2[i]; + } + } + + /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ + /* precision */ + } + + /* prediction of the magnetic vector */ + /* 'AttitudeEKF:121' if (approx_prediction==1) */ + 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 */ + /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += b_O[r2 + 3 * i] * fv4[i]; + } + } + } else { + /* 'AttitudeEKF:125' else */ + /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += fv3[r2 + 3 * i] * fv4[i]; + } + } + + /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ + /* precision */ + } + + /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ + x_apr[0] = x_apo[0] + dt * wak[0]; + x_apr[1] = x_apo[1] + dt * wak[1]; + x_apr[2] = x_apo[2] + dt * wak[2]; + for (i = 0; i < 3; i++) { + x_apr[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 6] = zek[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 9] = muk[i]; + } + + /* compute the apriori error covariance estimate from the previous */ + /* aposteriori estimate */ + /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ + /* 'AttitudeEKF:137' -zez,0,zex; */ + /* 'AttitudeEKF:138' zey,-zex,0]'; */ + /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ + /* 'AttitudeEKF:140' -muz,0,mux; */ + /* 'AttitudeEKF:141' muy,-mux,0]'; */ + /* 'AttitudeEKF:143' E=single(eye(3)); */ + /* 'AttitudeEKF:144' Z=single(zeros(3)); */ + /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ + /* 'AttitudeEKF:147' Z, Z, Z, Z */ + /* 'AttitudeEKF:148' EZ, Z, O, Z */ + /* 'AttitudeEKF:149' MA, Z, Z, O]; */ + /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + for (r2 = 0; r2 < 3; r2++) { + A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; + } + + for (r2 = 0; r2 < 3; r2++) { + A_lin[(r2 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_apo[8]; + A_lin[8] = -x_apo[7]; + A_lin[18] = -x_apo[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_apo[6]; + A_lin[30] = x_apo[7]; + A_lin[31] = -x_apo[6]; + A_lin[32] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_apo[11]; + A_lin[11] = -x_apo[10]; + A_lin[21] = -x_apo[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_apo[9]; + A_lin[33] = x_apo[10]; + A_lin[34] = -x_apo[9]; + A_lin[35] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; + } + } + + /* process covariance matrix */ + /* 'AttitudeEKF:156' if (isempty(Q)) */ + if (!Q_not_empty) { + /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ + /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ + /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ + /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ + v[0] = q_rotSpeed; + v[1] = q_rotSpeed; + v[2] = q_rotSpeed; + v[3] = q_rotAcc; + v[4] = q_rotAcc; + v[5] = q_rotAcc; + v[6] = q_acc; + v[7] = q_acc; + v[8] = q_acc; + v[9] = q_mag; + v[10] = q_mag; + v[11] = q_mag; + memset(&Q[0], 0, 144U * sizeof(float)); + for (i = 0; i < 12; i++) { + Q[i + 12 * i] = v[i]; + } + + Q_not_empty = TRUE; + } + + /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + A_lin[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; + } + + P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; + } + } + + /* % update */ + /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { + /* 'AttitudeEKF:169' R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 'AttitudeEKF:170' 0,r_gyro,0,0,0,0,0,0,0; */ + /* 'AttitudeEKF:171' 0,0,r_gyro,0,0,0,0,0,0; */ + /* 'AttitudeEKF:172' 0,0,0,r_accel,0,0,0,0,0; */ + /* 'AttitudeEKF:173' 0,0,0,0,r_accel,0,0,0,0; */ + /* 'AttitudeEKF:174' 0,0,0,0,0,r_accel,0,0,0; */ + /* 'AttitudeEKF:175' 0,0,0,0,0,0,r_mag,0,0; */ + /* 'AttitudeEKF:176' 0,0,0,0,0,0,0,r_mag,0; */ + /* 'AttitudeEKF:177' 0,0,0,0,0,0,0,0,r_mag]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'AttitudeEKF:180' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:181' Z, Z, E, Z; */ + /* 'AttitudeEKF:182' Z, Z, Z, E]; */ + /* 'AttitudeEKF:184' y_k=z(1:9)-H_k*x_apr; */ + /* 'AttitudeEKF:187' S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:188' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 9; i++) { + b_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 12; i++) { + K_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + K_k[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + a[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 9 * i] += K_k[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + b_r_gyro[0] = r_gyro; + b_r_gyro[9] = 0.0F; + b_r_gyro[18] = 0.0F; + b_r_gyro[27] = 0.0F; + b_r_gyro[36] = 0.0F; + b_r_gyro[45] = 0.0F; + b_r_gyro[54] = 0.0F; + b_r_gyro[63] = 0.0F; + b_r_gyro[72] = 0.0F; + b_r_gyro[1] = 0.0F; + b_r_gyro[10] = r_gyro; + b_r_gyro[19] = 0.0F; + b_r_gyro[28] = 0.0F; + b_r_gyro[37] = 0.0F; + b_r_gyro[46] = 0.0F; + b_r_gyro[55] = 0.0F; + b_r_gyro[64] = 0.0F; + b_r_gyro[73] = 0.0F; + b_r_gyro[2] = 0.0F; + b_r_gyro[11] = 0.0F; + b_r_gyro[20] = r_gyro; + b_r_gyro[29] = 0.0F; + b_r_gyro[38] = 0.0F; + b_r_gyro[47] = 0.0F; + b_r_gyro[56] = 0.0F; + b_r_gyro[65] = 0.0F; + b_r_gyro[74] = 0.0F; + b_r_gyro[3] = 0.0F; + b_r_gyro[12] = 0.0F; + b_r_gyro[21] = 0.0F; + b_r_gyro[30] = r_accel; + b_r_gyro[39] = 0.0F; + b_r_gyro[48] = 0.0F; + b_r_gyro[57] = 0.0F; + b_r_gyro[66] = 0.0F; + b_r_gyro[75] = 0.0F; + b_r_gyro[4] = 0.0F; + b_r_gyro[13] = 0.0F; + b_r_gyro[22] = 0.0F; + b_r_gyro[31] = 0.0F; + b_r_gyro[40] = r_accel; + b_r_gyro[49] = 0.0F; + b_r_gyro[58] = 0.0F; + b_r_gyro[67] = 0.0F; + b_r_gyro[76] = 0.0F; + b_r_gyro[5] = 0.0F; + b_r_gyro[14] = 0.0F; + b_r_gyro[23] = 0.0F; + b_r_gyro[32] = 0.0F; + b_r_gyro[41] = 0.0F; + b_r_gyro[50] = r_accel; + b_r_gyro[59] = 0.0F; + b_r_gyro[68] = 0.0F; + b_r_gyro[77] = 0.0F; + b_r_gyro[6] = 0.0F; + b_r_gyro[15] = 0.0F; + b_r_gyro[24] = 0.0F; + b_r_gyro[33] = 0.0F; + b_r_gyro[42] = 0.0F; + b_r_gyro[51] = 0.0F; + b_r_gyro[60] = r_mag; + b_r_gyro[69] = 0.0F; + b_r_gyro[78] = 0.0F; + b_r_gyro[7] = 0.0F; + b_r_gyro[16] = 0.0F; + b_r_gyro[25] = 0.0F; + b_r_gyro[34] = 0.0F; + b_r_gyro[43] = 0.0F; + b_r_gyro[52] = 0.0F; + b_r_gyro[61] = 0.0F; + b_r_gyro[70] = r_mag; + b_r_gyro[79] = 0.0F; + b_r_gyro[8] = 0.0F; + b_r_gyro[17] = 0.0F; + b_r_gyro[26] = 0.0F; + b_r_gyro[35] = 0.0F; + b_r_gyro[44] = 0.0F; + b_r_gyro[53] = 0.0F; + b_r_gyro[62] = 0.0F; + b_r_gyro[71] = 0.0F; + b_r_gyro[80] = r_mag; + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 9; i++) { + c_a[i + 9 * r2] = a[i + 9 * r2] + b_r_gyro[i + 9 * r2]; + } + } + + mrdivide(b_P_apr, c_a, K_k); + + /* 'AttitudeEKF:191' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 9; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; + } + + b_O[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 9; i++) { + maxval += K_k[r2 + 12 * i] * b_O[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:192' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 9; r1++) { + maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:193' else */ + /* 'AttitudeEKF:194' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:196' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:197' 0,r_gyro,0; */ + /* 'AttitudeEKF:198' 0,0,r_gyro]; */ + /* observation matrix */ + /* 'AttitudeEKF:201' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:203' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* 'AttitudeEKF:205' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:206' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 3 * i] += (float)e_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_O[r2 + 3 * i] += d_a[i + 3 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + c_r_gyro[0] = r_gyro; + c_r_gyro[1] = 0.0F; + c_r_gyro[2] = 0.0F; + c_r_gyro[3] = 0.0F; + c_r_gyro[4] = r_gyro; + c_r_gyro[5] = 0.0F; + c_r_gyro[6] = 0.0F; + c_r_gyro[7] = 0.0F; + c_r_gyro[8] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + O[i + 3 * r2] = b_O[i + 3 * r2] + c_r_gyro[i + 3 * r2]; + } + + for (i = 0; i < 12; i++) { + b_K_k[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_K_k[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(O[0]); + a21 = (real32_T)fabs(O[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(O[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + O[r2] /= O[r1]; + O[r3] /= O[r1]; + O[3 + r2] -= O[r2] * O[3 + r1]; + O[3 + r3] -= O[r3] * O[3 + r1]; + O[6 + r2] -= O[r2] * O[6 + r1]; + O[6 + r3] -= O[r3] * O[6 + r1]; + if ((real32_T)fabs(O[3 + r3]) > (real32_T)fabs(O[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + O[3 + r3] /= O[3 + r2]; + O[6 + r3] -= O[3 + r3] * O[6 + r2]; + for (i = 0; i < 12; i++) { + f_a[3 * i] = b_K_k[r1 + 3 * i]; + f_a[1 + 3 * i] = b_K_k[r2 + 3 * i] - f_a[3 * i] * O[r2]; + f_a[2 + 3 * i] = (b_K_k[r3 + 3 * i] - f_a[3 * i] * O[r3]) - f_a[1 + 3 * + i] * O[3 + r3]; + f_a[2 + 3 * i] /= O[6 + r3]; + f_a[3 * i] -= f_a[2 + 3 * i] * O[6 + r1]; + f_a[1 + 3 * i] -= f_a[2 + 3 * i] * O[6 + r2]; + f_a[1 + 3 * i] /= O[3 + r2]; + f_a[3 * i] -= f_a[1 + 3 * i] * O[3 + r1]; + f_a[3 * i] /= O[r1]; + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_K_k[i + 12 * r2] = f_a[r2 + 3 * i]; + } + } + + /* 'AttitudeEKF:209' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)e_a[r2 + 3 * i] * x_apr[i]; + } + + b_muk[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 3; i++) { + maxval += b_K_k[r2 + 12 * i] * b_muk[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:210' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 3 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:211' else */ + /* 'AttitudeEKF:212' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:214' R=[r_gyro,0,0,0,0,0; */ + /* 'AttitudeEKF:215' 0,r_gyro,0,0,0,0; */ + /* 'AttitudeEKF:216' 0,0,r_gyro,0,0,0; */ + /* 'AttitudeEKF:217' 0,0,0,r_accel,0,0; */ + /* 'AttitudeEKF:218' 0,0,0,0,r_accel,0; */ + /* 'AttitudeEKF:219' 0,0,0,0,0,r_accel]; */ + /* observation matrix */ + /* 'AttitudeEKF:223' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:224' Z, Z, E, Z]; */ + /* 'AttitudeEKF:226' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* 'AttitudeEKF:228' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:229' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + c_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * + i]; + } + } + } + + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + c_K_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_K_k[r2 + 6 * i] += (float)g_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + b_K_k[0] = r_gyro; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r_gyro; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r_gyro; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r_accel; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r_accel; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r_accel; + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 6; i++) { + f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + } + } + + b_mrdivide(c_P_apr, f_a, c_K_k); + + /* 'AttitudeEKF:232' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 6; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)g_a[r2 + 6 * i] * x_apr[i]; + } + + b_z[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += c_K_k[r2 + 12 * i] * b_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:233' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += c_K_k[r2 + 12 * r1] * (float)g_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:234' else */ + /* 'AttitudeEKF:235' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { + /* 'AttitudeEKF:236' R=[r_gyro,0,0,0,0,0; */ + /* 'AttitudeEKF:237' 0,r_gyro,0,0,0,0; */ + /* 'AttitudeEKF:238' 0,0,r_gyro,0,0,0; */ + /* 'AttitudeEKF:239' 0,0,0,r_mag,0,0; */ + /* 'AttitudeEKF:240' 0,0,0,0,r_mag,0; */ + /* 'AttitudeEKF:241' 0,0,0,0,0,r_mag]; */ + /* observation matrix */ + /* 'AttitudeEKF:244' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:245' Z, Z, Z, E]; */ + /* 'AttitudeEKF:247' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* 'AttitudeEKF:249' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:250' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + c_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 + * i]; + } + } + } + + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + c_K_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_K_k[r2 + 6 * i] += (float)h_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + b_K_k[0] = r_gyro; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r_gyro; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r_gyro; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r_mag; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r_mag; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r_mag; + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 6; i++) { + f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + } + } + + b_mrdivide(c_P_apr, f_a, c_K_k); + + /* 'AttitudeEKF:253' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + b_z[r2] = z[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + b_z[r2 + 3] = z[6 + r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + i_a[r2] = 0.0F; + for (i = 0; i < 12; i++) { + i_a[r2] += (float)h_a[r2 + 6 * i] * x_apr[i]; + } + + c_z[r2] = b_z[r2] - i_a[r2]; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += c_K_k[r2 + 12 * i] * c_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:254' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += c_K_k[r2 + 12 * r1] * (float)h_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:255' else */ + /* 'AttitudeEKF:256' x_apo=x_apr; */ + for (i = 0; i < 12; i++) { + x_apo[i] = x_apr[i]; + } + + /* 'AttitudeEKF:257' P_apo=P_apr; */ + memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'AttitudeEKF:266' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + maxval = norm(*(float (*)[3])&x_apo[6]); + a21 = norm(*(float (*)[3])&x_apo[9]); + for (i = 0; i < 3; i++) { + /* 'AttitudeEKF:267' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + muk[i] = -x_apo[i + 6] / maxval; + zek[i] = x_apo[i + 9] / a21; + } + + /* 'AttitudeEKF:269' y_n_b=cross(z_n_b,m_n_b); */ + wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; + wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; + wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; + + /* 'AttitudeEKF:270' y_n_b=y_n_b./norm(y_n_b); */ + maxval = norm(wak); + for (r2 = 0; r2 < 3; r2++) { + wak[r2] /= maxval; + } + + /* 'AttitudeEKF:272' x_n_b=(cross(y_n_b,z_n_b)); */ + zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; + zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; + zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; + + /* 'AttitudeEKF:273' x_n_b=x_n_b./norm(x_n_b); */ + maxval = norm(zek); + for (r2 = 0; r2 < 3; r2++) { + zek[r2] /= maxval; + } + + /* 'AttitudeEKF:276' xa_apo=x_apo; */ + for (i = 0; i < 12; i++) { + xa_apo[i] = x_apo[i]; + } + + /* 'AttitudeEKF:277' Pa_apo=P_apo; */ + memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); + + /* rotation matrix from earth to body system */ + /* 'AttitudeEKF:279' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (r2 = 0; r2 < 3; r2++) { + Rot_matrix[r2] = zek[r2]; + Rot_matrix[3 + r2] = wak[r2]; + Rot_matrix[6 + r2] = muk[r2]; + } + + /* 'AttitudeEKF:282' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:283' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:284' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:285' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); +} + +void AttitudeEKF_initialize(void) +{ + Q_not_empty = FALSE; + Ji_not_empty = FALSE; + AttitudeEKF_init(); +} + +void AttitudeEKF_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (AttitudeEKF.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h new file mode 100644 index 0000000000..fc02752109 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -0,0 +1,26 @@ +/* + * AttitudeEKF.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __ATTITUDEEKF_H__ +#define __ATTITUDEEKF_H__ +/* Include files */ +#include +#include +#include +#include + +#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h new file mode 100644 index 0000000000..63eb7e5018 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -0,0 +1,17 @@ +/* + * AttitudeEKF_types.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __ATTITUDEEKF_TYPES_H__ +#define __ATTITUDEEKF_TYPES_H__ + +/* Include files */ +#include "rtwtypes.h" + +#endif +/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a8..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/* - * attitudeKalmanfilter.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" -#include "norm.h" -#include "cross.h" -#include "eye.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) -{ - real32_T y; - int32_T b_u0; - int32_T b_u1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - if (u0 > 0.0F) { - b_u0 = 1; - } else { - b_u0 = -1; - } - - if (u1 > 0.0F) { - b_u1 = 1; - } else { - b_u1 = -1; - } - - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); - } else if (u1 == 0.0F) { - if (u0 > 0.0F) { - y = RT_PIF / 2.0F; - } else if (u0 < 0.0F) { - y = -(RT_PIF / 2.0F); - } else { - y = 0.0F; - } - } else { - y = (real32_T)atan2(u0, u1); - } - - return y; -} - -/* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) - */ -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]) -{ - real32_T wak[3]; - real32_T O[9]; - real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[9]; - real32_T x_n_b[3]; - real32_T b_x_aposteriori_k[3]; - real32_T z_n_b[3]; - real32_T c_a[3]; - real32_T d_a[3]; - int32_T i0; - real32_T x_apriori[12]; - real_T dv1[144]; - real32_T A_lin[144]; - static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T b_A_lin[144]; - real32_T b_q[144]; - real32_T c_A_lin[144]; - real32_T d_A_lin[144]; - real32_T e_A_lin[144]; - int32_T i1; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T K_k[108]; - real32_T fv0[81]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T b_r[81]; - real32_T fv1[81]; - real32_T f0; - real32_T c_P_apriori[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T fv2[36]; - static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T c_r[9]; - real32_T b_K_k[36]; - real32_T d_P_apriori[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0 }; - - real32_T c_K_k[72]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0 }; - - real32_T b_z[6]; - static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1 }; - - static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - - real32_T fv3[6]; - real32_T c_z[6]; - - /* Extended Attitude Kalmanfilter */ - /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ - /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ - /* */ - /* Example.... */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* coder.varsize('udpIndVect', [9,1], [1,0]) */ - /* udpIndVect=find(updVect); */ - /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* observation matrix */ - /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ - /* % prediction section */ - /* body angular accelerations */ - /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ - wak[0] = x_aposteriori_k[3]; - wak[1] = x_aposteriori_k[4]; - wak[2] = x_aposteriori_k[5]; - - /* body angular rates */ - /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_aposteriori_k[2]; - O[2] = x_aposteriori_k[1]; - O[3] = x_aposteriori_k[2]; - O[4] = 0.0F; - O[5] = -x_aposteriori_k[0]; - O[6] = -x_aposteriori_k[1]; - O[7] = x_aposteriori_k[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* prediction of the magnetic vector */ - /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:66' -zez,0,zex; */ - /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:69' -muz,0,mux; */ - /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:74' E=eye(3); */ - /* 'attitudeKalmanfilter:76' Z=zeros(3); */ - /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[0]; - x_n_b[1] = x_aposteriori_k[1]; - x_n_b[2] = x_aposteriori_k[2]; - b_x_aposteriori_k[0] = x_aposteriori_k[6]; - b_x_aposteriori_k[1] = x_aposteriori_k[7]; - b_x_aposteriori_k[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; - for (i = 0; i < 3; i++) { - c_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; - } - - d_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; - } - - x_apriori[i] = x_n_b[i] + dt * wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 6] = c_a[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 9] = d_a[i]; - } - - /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ - /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; - } - - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_aposteriori_k[8]; - A_lin[8] = -x_aposteriori_k[7]; - A_lin[18] = -x_aposteriori_k[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_aposteriori_k[6]; - A_lin[30] = x_aposteriori_k[7]; - A_lin[31] = -x_aposteriori_k[6]; - A_lin[32] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_aposteriori_k[11]; - A_lin[11] = -x_aposteriori_k[10]; - A_lin[21] = -x_aposteriori_k[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_aposteriori_k[9]; - A_lin[33] = x_aposteriori_k[7]; - A_lin[34] = -x_aposteriori_k[9]; - A_lin[35] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * - dt; - } - } - - /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ - /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ - /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ - /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ - /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ - /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - b_q[0] = q[0]; - b_q[12] = 0.0F; - b_q[24] = 0.0F; - b_q[36] = 0.0F; - b_q[48] = 0.0F; - b_q[60] = 0.0F; - b_q[72] = 0.0F; - b_q[84] = 0.0F; - b_q[96] = 0.0F; - b_q[108] = 0.0F; - b_q[120] = 0.0F; - b_q[132] = 0.0F; - b_q[1] = 0.0F; - b_q[13] = q[0]; - b_q[25] = 0.0F; - b_q[37] = 0.0F; - b_q[49] = 0.0F; - b_q[61] = 0.0F; - b_q[73] = 0.0F; - b_q[85] = 0.0F; - b_q[97] = 0.0F; - b_q[109] = 0.0F; - b_q[121] = 0.0F; - b_q[133] = 0.0F; - b_q[2] = 0.0F; - b_q[14] = 0.0F; - b_q[26] = q[0]; - b_q[38] = 0.0F; - b_q[50] = 0.0F; - b_q[62] = 0.0F; - b_q[74] = 0.0F; - b_q[86] = 0.0F; - b_q[98] = 0.0F; - b_q[110] = 0.0F; - b_q[122] = 0.0F; - b_q[134] = 0.0F; - b_q[3] = 0.0F; - b_q[15] = 0.0F; - b_q[27] = 0.0F; - b_q[39] = q[1]; - b_q[51] = 0.0F; - b_q[63] = 0.0F; - b_q[75] = 0.0F; - b_q[87] = 0.0F; - b_q[99] = 0.0F; - b_q[111] = 0.0F; - b_q[123] = 0.0F; - b_q[135] = 0.0F; - b_q[4] = 0.0F; - b_q[16] = 0.0F; - b_q[28] = 0.0F; - b_q[40] = 0.0F; - b_q[52] = q[1]; - b_q[64] = 0.0F; - b_q[76] = 0.0F; - b_q[88] = 0.0F; - b_q[100] = 0.0F; - b_q[112] = 0.0F; - b_q[124] = 0.0F; - b_q[136] = 0.0F; - b_q[5] = 0.0F; - b_q[17] = 0.0F; - b_q[29] = 0.0F; - b_q[41] = 0.0F; - b_q[53] = 0.0F; - b_q[65] = q[1]; - b_q[77] = 0.0F; - b_q[89] = 0.0F; - b_q[101] = 0.0F; - b_q[113] = 0.0F; - b_q[125] = 0.0F; - b_q[137] = 0.0F; - b_q[6] = 0.0F; - b_q[18] = 0.0F; - b_q[30] = 0.0F; - b_q[42] = 0.0F; - b_q[54] = 0.0F; - b_q[66] = 0.0F; - b_q[78] = q[2]; - b_q[90] = 0.0F; - b_q[102] = 0.0F; - b_q[114] = 0.0F; - b_q[126] = 0.0F; - b_q[138] = 0.0F; - b_q[7] = 0.0F; - b_q[19] = 0.0F; - b_q[31] = 0.0F; - b_q[43] = 0.0F; - b_q[55] = 0.0F; - b_q[67] = 0.0F; - b_q[79] = 0.0F; - b_q[91] = q[2]; - b_q[103] = 0.0F; - b_q[115] = 0.0F; - b_q[127] = 0.0F; - b_q[139] = 0.0F; - b_q[8] = 0.0F; - b_q[20] = 0.0F; - b_q[32] = 0.0F; - b_q[44] = 0.0F; - b_q[56] = 0.0F; - b_q[68] = 0.0F; - b_q[80] = 0.0F; - b_q[92] = 0.0F; - b_q[104] = q[2]; - b_q[116] = 0.0F; - b_q[128] = 0.0F; - b_q[140] = 0.0F; - b_q[9] = 0.0F; - b_q[21] = 0.0F; - b_q[33] = 0.0F; - b_q[45] = 0.0F; - b_q[57] = 0.0F; - b_q[69] = 0.0F; - b_q[81] = 0.0F; - b_q[93] = 0.0F; - b_q[105] = 0.0F; - b_q[117] = q[3]; - b_q[129] = 0.0F; - b_q[141] = 0.0F; - b_q[10] = 0.0F; - b_q[22] = 0.0F; - b_q[34] = 0.0F; - b_q[46] = 0.0F; - b_q[58] = 0.0F; - b_q[70] = 0.0F; - b_q[82] = 0.0F; - b_q[94] = 0.0F; - b_q[106] = 0.0F; - b_q[118] = 0.0F; - b_q[130] = q[3]; - b_q[142] = 0.0F; - b_q[11] = 0.0F; - b_q[23] = 0.0F; - b_q[35] = 0.0F; - b_q[47] = 0.0F; - b_q[59] = 0.0F; - b_q[71] = 0.0F; - b_q[83] = 0.0F; - b_q[95] = 0.0F; - b_q[107] = 0.0F; - b_q[119] = 0.0F; - b_q[131] = 0.0F; - b_q[143] = q[3]; - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * - i0]; - } - - c_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 12; i0++) { - d_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - e_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; - } - } - - /* % update */ - /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:112' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ - /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ - /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 - + 12 * i0]; - } - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } - - b_r[0] = r[0]; - b_r[9] = 0.0F; - b_r[18] = 0.0F; - b_r[27] = 0.0F; - b_r[36] = 0.0F; - b_r[45] = 0.0F; - b_r[54] = 0.0F; - b_r[63] = 0.0F; - b_r[72] = 0.0F; - b_r[1] = 0.0F; - b_r[10] = r[0]; - b_r[19] = 0.0F; - b_r[28] = 0.0F; - b_r[37] = 0.0F; - b_r[46] = 0.0F; - b_r[55] = 0.0F; - b_r[64] = 0.0F; - b_r[73] = 0.0F; - b_r[2] = 0.0F; - b_r[11] = 0.0F; - b_r[20] = r[0]; - b_r[29] = 0.0F; - b_r[38] = 0.0F; - b_r[47] = 0.0F; - b_r[56] = 0.0F; - b_r[65] = 0.0F; - b_r[74] = 0.0F; - b_r[3] = 0.0F; - b_r[12] = 0.0F; - b_r[21] = 0.0F; - b_r[30] = r[1]; - b_r[39] = 0.0F; - b_r[48] = 0.0F; - b_r[57] = 0.0F; - b_r[66] = 0.0F; - b_r[75] = 0.0F; - b_r[4] = 0.0F; - b_r[13] = 0.0F; - b_r[22] = 0.0F; - b_r[31] = 0.0F; - b_r[40] = r[1]; - b_r[49] = 0.0F; - b_r[58] = 0.0F; - b_r[67] = 0.0F; - b_r[76] = 0.0F; - b_r[5] = 0.0F; - b_r[14] = 0.0F; - b_r[23] = 0.0F; - b_r[32] = 0.0F; - b_r[41] = 0.0F; - b_r[50] = r[1]; - b_r[59] = 0.0F; - b_r[68] = 0.0F; - b_r[77] = 0.0F; - b_r[6] = 0.0F; - b_r[15] = 0.0F; - b_r[24] = 0.0F; - b_r[33] = 0.0F; - b_r[42] = 0.0F; - b_r[51] = 0.0F; - b_r[60] = r[2]; - b_r[69] = 0.0F; - b_r[78] = 0.0F; - b_r[7] = 0.0F; - b_r[16] = 0.0F; - b_r[25] = 0.0F; - b_r[34] = 0.0F; - b_r[43] = 0.0F; - b_r[52] = 0.0F; - b_r[61] = 0.0F; - b_r[70] = r[2]; - b_r[79] = 0.0F; - b_r[8] = 0.0F; - b_r[17] = 0.0F; - b_r[26] = 0.0F; - b_r[35] = 0.0F; - b_r[44] = 0.0F; - b_r[53] = 0.0F; - b_r[62] = 0.0F; - b_r[71] = 0.0F; - b_r[80] = r[2]; - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv1, K_k); - - /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - O[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * O[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:138' else */ - /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ - /* 'attitudeKalmanfilter:142' 0,r(1),0; */ - /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - c_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv3[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * - i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - O[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; - } - } - } - - c_r[0] = r[0]; - c_r[3] = 0.0F; - c_r[6] = 0.0F; - c_r[1] = 0.0F; - c_r[4] = r[0]; - c_r[7] = 0.0F; - c_r[2] = 0.0F; - c_r[5] = 0.0F; - c_r[8] = r[0]; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; - } - } - - b_mrdivide(c_P_apriori, a, b_K_k); - - /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; - } - - x_n_b[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:156' else */ - /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) - { - /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:159' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ - /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ - /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv5[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[1]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[1]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[1]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 6; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; - } - - b_z[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * b_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:181' else */ - /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) - { - /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv7[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * - i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[2]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[2]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[2]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - b_z[i] = z[i]; - } - - for (i = 0; i < 3; i++) { - b_z[i + 3] = z[i + 6]; - } - - for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; - } - - c_z[i] = b_z[i] - fv3[i]; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * c_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:202' else */ - /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ - for (i = 0; i < 12; i++) { - x_aposteriori[i] = x_apriori[i]; - } - - /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ - memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = -x_aposteriori[i + 6]; - } - - rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - - /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), wak); - - /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - cross(z_n_b, x_n_b, wak); - - /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - rdivide(x_n_b, norm(wak), wak); - - /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(wak, z_n_b, x_n_b); - - /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ - for (i = 0; i < 3; i++) { - b_x_aposteriori_k[i] = x_n_b[i]; - } - - rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - - /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (i = 0; i < 3; i++) { - Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = wak[i]; - Rot_matrix[6 + i] = z_n_b[i]; - } - - /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); -} - -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a91..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be663..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f97274196..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a50244..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002b..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684a..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe76717..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4f..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51ff..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b571..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe28..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -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 -#include -#include -#include -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d1044..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd02..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -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 -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -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 -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 3564983630..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -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 - -#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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -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) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d94..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -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 -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100755 new mode 100644 index 9a5c96267e..9245a24c8f --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -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 - -/*=======================================================================* - * 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: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: 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) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 99d0c5bf28..749b0a91c3 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -39,16 +39,6 @@ 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 From 63fa17ef0dd4b0a184f6e3e298113bb143b2cb44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:23:26 +0200 Subject: [PATCH 002/122] att ekf: add param to enable/disable J --- .../attitude_estimator_ekf_main.cpp | 2 +- .../attitude_estimator_ekf_params.c | 19 +++++++++++++++---- .../attitude_estimator_ekf_params.h | 2 ++ 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e1bbf5bc7a..667b74d1d7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,7 +512,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Call the estimator */ AttitudeEKF(false, // approx_prediction - false, // use_inertia_matrix + (unsigned char)ekf_params.use_moment_inertia, update_vect, dt, z_k, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5b57bfb4db..5c33bc2ac7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -79,7 +79,6 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); - /* * Moment of inertia matrix diagonal entry (3, 3) * @@ -88,6 +87,16 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); */ 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) { @@ -105,9 +114,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) 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->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; } @@ -131,6 +141,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru 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; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index fbb6a18ff4..5d3b6b2440 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -45,6 +45,7 @@ struct attitude_estimator_ekf_params { 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; @@ -56,6 +57,7 @@ struct attitude_estimator_ekf_param_handles { 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; }; From 963394f0e43a57dd765541b9c226b5cbf70c9073 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:42:01 +0200 Subject: [PATCH 003/122] att ekf: add descriptions for params --- .../attitude_estimator_ekf_params.c | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5c33bc2ac7..5637ec102f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,18 +44,54 @@ /* Extended Kalman Filter covariances */ -/* gyro process 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); -/* gyro offsets process noise */ + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -/* gyro measurement noise */ +/** + * 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); /* magnetic declination, in degrees */ @@ -63,7 +99,7 @@ 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 @@ -71,7 +107,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); */ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (2, 2) * * @group attitude_ekf @@ -79,7 +115,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (3, 3) * * @group attitude_ekf @@ -87,7 +123,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); */ 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 From 58aabe648acf296267bd1e29e8a02b33ea9e7cf7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 11:25:09 +0200 Subject: [PATCH 004/122] update attitude estimator ekf to latest version mainly saves stack size --- .../attitude_estimator_ekf/AttitudeEKF.m | 62 +- .../attitudeKalmanfilter.prj | 7 +- .../attitude_estimator_ekf_main.cpp | 2 +- .../codegen/AttitudeEKF.c | 629 +++++++----------- .../codegen/AttitudeEKF.h | 2 +- .../codegen/AttitudeEKF_types.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 7 files changed, 300 insertions(+), 406 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m index 1218cb65da..fea1a773e3 100644 --- a/src/modules/attitude_estimator_ekf/AttitudeEKF.m +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -166,15 +166,16 @@ 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=[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; @@ -184,7 +185,9 @@ if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 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'+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)); @@ -196,13 +199,16 @@ else 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)'+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)); @@ -211,13 +217,14 @@ else 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=[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; @@ -225,7 +232,9 @@ else 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)'+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)); @@ -233,12 +242,13 @@ else 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=[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; @@ -246,7 +256,9 @@ else 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)'+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)); diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj index a8315bf571..9ea5203463 100644 --- a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -1,6 +1,6 @@ - + false false @@ -112,7 +112,7 @@ false option.DynamicMemoryAllocation.Threshold 65536 - 200000 + 4000 false option.FilePartitionMethod.SingleFile true @@ -215,7 +215,6 @@ - @@ -492,7 +491,7 @@ false true false - 3.15.5-1-ARCH + 3.16.1-1-ARCH false true glnxa64 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 667b74d1d7..766171a0fd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -132,7 +132,7 @@ 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); exit(0); diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c index d568e57376..68db382cfe 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ @@ -438,67 +438,66 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char float fv4[3]; float x_apr[12]; signed char I[144]; - float A_lin[144]; + static float A_lin[144]; static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - float b_A_lin[144]; + static float b_A_lin[144]; float v[12]; - float P_apr[144]; - float b_P_apr[108]; - static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - float K_k[108]; - float a[81]; + static float P_apr[144]; + float a[108]; static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - float b_r_gyro[81]; - float c_a[81]; - float d_a[36]; - static const signed char e_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + float S_k[81]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[9]; + float K_k[108]; + float b_S_k[36]; + static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - float c_r_gyro[9]; - float b_K_k[36]; + float c_r_gyro[3]; + float B[36]; int r3; float a21; - float f_a[36]; - float c_P_apr[72]; + float Y[36]; + float d_a[72]; + static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; - float c_K_k[72]; - static const signed char g_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0 }; + float d_r_gyro[6]; + float c_S_k[6]; + float b_K_k[72]; + static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; - float b_z[6]; static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - static const signed char h_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1 }; - - float i_a[6]; - float c_z[6]; + float b_z[6]; /* LQG Postion Estimator and Controller */ /* Observer: */ @@ -898,138 +897,71 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char /* % update */ /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { - /* 'AttitudeEKF:169' R=[r_gyro,0,0,0,0,0,0,0,0; */ - /* 'AttitudeEKF:170' 0,r_gyro,0,0,0,0,0,0,0; */ - /* 'AttitudeEKF:171' 0,0,r_gyro,0,0,0,0,0,0; */ - /* 'AttitudeEKF:172' 0,0,0,r_accel,0,0,0,0,0; */ - /* 'AttitudeEKF:173' 0,0,0,0,r_accel,0,0,0,0; */ - /* 'AttitudeEKF:174' 0,0,0,0,0,r_accel,0,0,0; */ - /* 'AttitudeEKF:175' 0,0,0,0,0,0,r_mag,0,0; */ - /* 'AttitudeEKF:176' 0,0,0,0,0,0,0,r_mag,0; */ - /* 'AttitudeEKF:177' 0,0,0,0,0,0,0,0,r_mag]; */ + /* 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]; */ + /* 'AttitudeEKF:178' 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]; */ - /* 'AttitudeEKF:180' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:181' Z, Z, E, Z; */ - /* 'AttitudeEKF:182' Z, Z, Z, E]; */ - /* 'AttitudeEKF:184' y_k=z(1:9)-H_k*x_apr; */ - /* 'AttitudeEKF:187' S_k=H_k*P_apr*H_k'+R; */ - /* 'AttitudeEKF:188' K_k=(P_apr*H_k'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 9; i++) { - b_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - b_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; - } - } - } - + /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:182' Z, Z, E, Z; */ + /* 'AttitudeEKF:183' Z, Z, Z, E]; */ + /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ + /* S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ for (r2 = 0; r2 < 9; r2++) { for (i = 0; i < 12; i++) { - K_k[r2 + 9 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - K_k[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 9; i++) { a[r2 + 9 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - a[r2 + 9 * i] += K_k[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + S_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; } } } + /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ b_r_gyro[0] = r_gyro; - b_r_gyro[9] = 0.0F; - b_r_gyro[18] = 0.0F; - b_r_gyro[27] = 0.0F; - b_r_gyro[36] = 0.0F; - b_r_gyro[45] = 0.0F; - b_r_gyro[54] = 0.0F; - b_r_gyro[63] = 0.0F; - b_r_gyro[72] = 0.0F; - b_r_gyro[1] = 0.0F; - b_r_gyro[10] = r_gyro; - b_r_gyro[19] = 0.0F; - b_r_gyro[28] = 0.0F; - b_r_gyro[37] = 0.0F; - b_r_gyro[46] = 0.0F; - b_r_gyro[55] = 0.0F; - b_r_gyro[64] = 0.0F; - b_r_gyro[73] = 0.0F; - b_r_gyro[2] = 0.0F; - b_r_gyro[11] = 0.0F; - b_r_gyro[20] = r_gyro; - b_r_gyro[29] = 0.0F; - b_r_gyro[38] = 0.0F; - b_r_gyro[47] = 0.0F; - b_r_gyro[56] = 0.0F; - b_r_gyro[65] = 0.0F; - b_r_gyro[74] = 0.0F; - b_r_gyro[3] = 0.0F; - b_r_gyro[12] = 0.0F; - b_r_gyro[21] = 0.0F; - b_r_gyro[30] = r_accel; - b_r_gyro[39] = 0.0F; - b_r_gyro[48] = 0.0F; - b_r_gyro[57] = 0.0F; - b_r_gyro[66] = 0.0F; - b_r_gyro[75] = 0.0F; - b_r_gyro[4] = 0.0F; - b_r_gyro[13] = 0.0F; - b_r_gyro[22] = 0.0F; - b_r_gyro[31] = 0.0F; - b_r_gyro[40] = r_accel; - b_r_gyro[49] = 0.0F; - b_r_gyro[58] = 0.0F; - b_r_gyro[67] = 0.0F; - b_r_gyro[76] = 0.0F; - b_r_gyro[5] = 0.0F; - b_r_gyro[14] = 0.0F; - b_r_gyro[23] = 0.0F; - b_r_gyro[32] = 0.0F; - b_r_gyro[41] = 0.0F; - b_r_gyro[50] = r_accel; - b_r_gyro[59] = 0.0F; - b_r_gyro[68] = 0.0F; - b_r_gyro[77] = 0.0F; - b_r_gyro[6] = 0.0F; - b_r_gyro[15] = 0.0F; - b_r_gyro[24] = 0.0F; - b_r_gyro[33] = 0.0F; - b_r_gyro[42] = 0.0F; - b_r_gyro[51] = 0.0F; - b_r_gyro[60] = r_mag; - b_r_gyro[69] = 0.0F; - b_r_gyro[78] = 0.0F; - b_r_gyro[7] = 0.0F; - b_r_gyro[16] = 0.0F; - b_r_gyro[25] = 0.0F; - b_r_gyro[34] = 0.0F; - b_r_gyro[43] = 0.0F; - b_r_gyro[52] = 0.0F; - b_r_gyro[61] = 0.0F; - b_r_gyro[70] = r_mag; - b_r_gyro[79] = 0.0F; - b_r_gyro[8] = 0.0F; - b_r_gyro[17] = 0.0F; - b_r_gyro[26] = 0.0F; - b_r_gyro[35] = 0.0F; - b_r_gyro[44] = 0.0F; - b_r_gyro[53] = 0.0F; - b_r_gyro[62] = 0.0F; - b_r_gyro[71] = 0.0F; - b_r_gyro[80] = r_mag; + b_r_gyro[1] = r_gyro; + b_r_gyro[2] = r_gyro; + b_r_gyro[3] = r_accel; + b_r_gyro[4] = r_accel; + b_r_gyro[5] = r_accel; + b_r_gyro[6] = r_mag; + b_r_gyro[7] = r_mag; + b_r_gyro[8] = r_mag; for (r2 = 0; r2 < 9; r2++) { + b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; + } + + for (r2 = 0; r2 < 9; r2++) { + S_k[10 * r2] = b_O[r2]; + } + + /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 9; i++) { - c_a[i + 9 * r2] = a[i + 9 * r2] + b_r_gyro[i + 9 * r2]; + a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } } } - mrdivide(b_P_apr, c_a, K_k); + mrdivide(a, S_k, K_k); - /* 'AttitudeEKF:191' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 9; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { @@ -1048,7 +980,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:192' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1074,53 +1006,56 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:193' else */ - /* 'AttitudeEKF:194' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + /* 'AttitudeEKF:196' else */ + /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { - /* 'AttitudeEKF:196' R=[r_gyro,0,0; */ - /* 'AttitudeEKF:197' 0,r_gyro,0; */ - /* 'AttitudeEKF:198' 0,0,r_gyro]; */ + /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:200' 0,r_gyro,0; */ + /* 'AttitudeEKF:201' 0,0,r_gyro]; */ + /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ /* observation matrix */ - /* 'AttitudeEKF:201' H_k=[ E, Z, Z, Z]; */ - /* 'AttitudeEKF:203' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ - /* 'AttitudeEKF:205' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'AttitudeEKF:206' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:207' 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); */ + /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 12; i++) { - d_a[r2 + 3 * i] = 0.0F; + b_S_k[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 3 * i] += (float)e_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; } } - } - for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 3; i++) { - b_O[r2 + 3 * i] = 0.0F; + O[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - b_O[r2 + 3 * i] += d_a[i + 3 * r1] * (float)b_b[r1 + 12 * r2]; + O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; } } } + /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ c_r_gyro[0] = r_gyro; - c_r_gyro[1] = 0.0F; - c_r_gyro[2] = 0.0F; - c_r_gyro[3] = 0.0F; - c_r_gyro[4] = r_gyro; - c_r_gyro[5] = 0.0F; - c_r_gyro[6] = 0.0F; - c_r_gyro[7] = 0.0F; - c_r_gyro[8] = r_gyro; + c_r_gyro[1] = r_gyro; + c_r_gyro[2] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + O[r2 << 2] = b_muk[r2]; + } + + /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 3; i++) { - O[i + 3 * r2] = b_O[i + 3 * r2] + c_r_gyro[i + 3 * r2]; + b_O[i + 3 * r2] = O[r2 + 3 * i]; } for (i = 0; i < 12; i++) { - b_K_k[r2 + 3 * i] = 0.0F; + B[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - b_K_k[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; } } } @@ -1128,58 +1063,58 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char r1 = 0; r2 = 1; r3 = 2; - maxval = (real32_T)fabs(O[0]); - a21 = (real32_T)fabs(O[1]); + maxval = (real32_T)fabs(b_O[0]); + a21 = (real32_T)fabs(b_O[1]); if (a21 > maxval) { maxval = a21; r1 = 1; r2 = 0; } - if ((real32_T)fabs(O[2]) > maxval) { + if ((real32_T)fabs(b_O[2]) > maxval) { r1 = 2; r2 = 1; r3 = 0; } - O[r2] /= O[r1]; - O[r3] /= O[r1]; - O[3 + r2] -= O[r2] * O[3 + r1]; - O[3 + r3] -= O[r3] * O[3 + r1]; - O[6 + r2] -= O[r2] * O[6 + r1]; - O[6 + r3] -= O[r3] * O[6 + r1]; - if ((real32_T)fabs(O[3 + r3]) > (real32_T)fabs(O[3 + r2])) { + b_O[r2] /= b_O[r1]; + b_O[r3] /= b_O[r1]; + b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; + b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; + b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; + b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; + if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { i = r2; r2 = r3; r3 = i; } - O[3 + r3] /= O[3 + r2]; - O[6 + r3] -= O[3 + r3] * O[6 + r2]; + b_O[3 + r3] /= b_O[3 + r2]; + b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; for (i = 0; i < 12; i++) { - f_a[3 * i] = b_K_k[r1 + 3 * i]; - f_a[1 + 3 * i] = b_K_k[r2 + 3 * i] - f_a[3 * i] * O[r2]; - f_a[2 + 3 * i] = (b_K_k[r3 + 3 * i] - f_a[3 * i] * O[r3]) - f_a[1 + 3 * - i] * O[3 + r3]; - f_a[2 + 3 * i] /= O[6 + r3]; - f_a[3 * i] -= f_a[2 + 3 * i] * O[6 + r1]; - f_a[1 + 3 * i] -= f_a[2 + 3 * i] * O[6 + r2]; - f_a[1 + 3 * i] /= O[3 + r2]; - f_a[3 * i] -= f_a[1 + 3 * i] * O[3 + r1]; - f_a[3 * i] /= O[r1]; + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * + b_O[3 + r3]; + Y[2 + 3 * i] /= b_O[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; + Y[1 + 3 * i] /= b_O[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; + Y[3 * i] /= b_O[r1]; } for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 12; i++) { - b_K_k[i + 12 * r2] = f_a[r2 + 3 * i]; + b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; } } - /* 'AttitudeEKF:209' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 3; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { - maxval += (float)e_a[r2 + 3 * i] * x_apr[i]; + maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; } b_muk[r2] = z[r2] - maxval; @@ -1188,13 +1123,13 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 3; i++) { - maxval += b_K_k[r2 + 12 * i] * b_muk[i]; + maxval += b_S_k[r2 + 12 * i] * b_muk[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:210' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1204,7 +1139,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 3; r1++) { - maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 3 * i]; + maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1220,111 +1155,85 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:211' else */ - /* 'AttitudeEKF:212' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + /* 'AttitudeEKF:217' else */ + /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { - /* 'AttitudeEKF:214' R=[r_gyro,0,0,0,0,0; */ - /* 'AttitudeEKF:215' 0,r_gyro,0,0,0,0; */ - /* 'AttitudeEKF:216' 0,0,r_gyro,0,0,0; */ - /* 'AttitudeEKF:217' 0,0,0,r_accel,0,0; */ - /* 'AttitudeEKF:218' 0,0,0,0,r_accel,0; */ - /* 'AttitudeEKF:219' 0,0,0,0,0,r_accel]; */ + /* 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]; */ + /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ /* observation matrix */ - /* 'AttitudeEKF:223' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:224' Z, Z, E, Z]; */ - /* 'AttitudeEKF:226' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ - /* 'AttitudeEKF:228' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:229' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - c_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * - i]; - } - } - } - + /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:231' Z, Z, E, Z]; */ + /* 'AttitudeEKF:233' 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); */ + /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ for (r2 = 0; r2 < 6; r2++) { for (i = 0; i < 12; i++) { - c_K_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_K_k[r2 + 6 * i] += (float)g_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { d_a[r2 + 6 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; } } } - b_K_k[0] = r_gyro; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r_gyro; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r_gyro; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r_accel; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r_accel; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r_accel; + /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_accel; + d_r_gyro[4] = r_accel; + d_r_gyro[5] = r_accel; for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 6; i++) { - f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; + } } } - b_mrdivide(c_P_apr, f_a, c_K_k); + b_mrdivide(d_a, b_S_k, b_K_k); - /* 'AttitudeEKF:232' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 6; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { - maxval += (float)g_a[r2 + 6 * i] * x_apr[i]; + maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; } - b_z[r2] = z[r2] - maxval; + d_r_gyro[r2] = z[r2] - maxval; } for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 6; i++) { - maxval += c_K_k[r2 + 12 * i] * b_z[i]; + maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:233' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1334,7 +1243,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 6; r1++) { - maxval += c_K_k[r2 + 12 * r1] * (float)g_a[r1 + 6 * i]; + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1350,119 +1259,93 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:234' else */ - /* 'AttitudeEKF:235' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + /* 'AttitudeEKF:243' else */ + /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { - /* 'AttitudeEKF:236' R=[r_gyro,0,0,0,0,0; */ - /* 'AttitudeEKF:237' 0,r_gyro,0,0,0,0; */ - /* 'AttitudeEKF:238' 0,0,r_gyro,0,0,0; */ - /* 'AttitudeEKF:239' 0,0,0,r_mag,0,0; */ - /* 'AttitudeEKF:240' 0,0,0,0,r_mag,0; */ - /* 'AttitudeEKF:241' 0,0,0,0,0,r_mag]; */ + /* 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]; */ + /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ /* observation matrix */ - /* 'AttitudeEKF:244' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:245' Z, Z, Z, E]; */ - /* 'AttitudeEKF:247' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ - /* 'AttitudeEKF:249' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:250' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - c_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 - * i]; - } - } - } - + /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:255' Z, Z, Z, E]; */ + /* 'AttitudeEKF:257' 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); */ + /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ for (r2 = 0; r2 < 6; r2++) { for (i = 0; i < 12; i++) { - c_K_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_K_k[r2 + 6 * i] += (float)h_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { d_a[r2 + 6 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; } } } - b_K_k[0] = r_gyro; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r_gyro; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r_gyro; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r_mag; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r_mag; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r_mag; + /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_mag; + d_r_gyro[4] = r_mag; + d_r_gyro[5] = r_mag; for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 6; i++) { - f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; + } } } - b_mrdivide(c_P_apr, f_a, c_K_k); + b_mrdivide(d_a, b_S_k, b_K_k); - /* 'AttitudeEKF:253' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 3; r2++) { - b_z[r2] = z[r2]; + d_r_gyro[r2] = z[r2]; } for (r2 = 0; r2 < 3; r2++) { - b_z[r2 + 3] = z[6 + r2]; + d_r_gyro[r2 + 3] = z[6 + r2]; } for (r2 = 0; r2 < 6; r2++) { - i_a[r2] = 0.0F; + c_S_k[r2] = 0.0F; for (i = 0; i < 12; i++) { - i_a[r2] += (float)h_a[r2 + 6 * i] * x_apr[i]; + c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; } - c_z[r2] = b_z[r2] - i_a[r2]; + b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; } for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 6; i++) { - maxval += c_K_k[r2 + 12 * i] * c_z[i]; + maxval += b_K_k[r2 + 12 * i] * b_z[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:254' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1472,7 +1355,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 6; r1++) { - maxval += c_K_k[r2 + 12 * r1] * (float)h_a[r1 + 6 * i]; + maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1488,13 +1371,13 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:255' else */ - /* 'AttitudeEKF:256' x_apo=x_apr; */ + /* 'AttitudeEKF:267' else */ + /* 'AttitudeEKF:268' x_apo=x_apr; */ for (i = 0; i < 12; i++) { x_apo[i] = x_apr[i]; } - /* 'AttitudeEKF:257' P_apo=P_apr; */ + /* 'AttitudeEKF:269' P_apo=P_apr; */ memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); } } @@ -1502,57 +1385,57 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } /* % euler anglels extraction */ - /* 'AttitudeEKF:266' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ maxval = norm(*(float (*)[3])&x_apo[6]); a21 = norm(*(float (*)[3])&x_apo[9]); for (i = 0; i < 3; i++) { - /* 'AttitudeEKF:267' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ muk[i] = -x_apo[i + 6] / maxval; zek[i] = x_apo[i + 9] / a21; } - /* 'AttitudeEKF:269' y_n_b=cross(z_n_b,m_n_b); */ + /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; - /* 'AttitudeEKF:270' y_n_b=y_n_b./norm(y_n_b); */ + /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ maxval = norm(wak); for (r2 = 0; r2 < 3; r2++) { wak[r2] /= maxval; } - /* 'AttitudeEKF:272' x_n_b=(cross(y_n_b,z_n_b)); */ + /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; - /* 'AttitudeEKF:273' x_n_b=x_n_b./norm(x_n_b); */ + /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ maxval = norm(zek); for (r2 = 0; r2 < 3; r2++) { zek[r2] /= maxval; } - /* 'AttitudeEKF:276' xa_apo=x_apo; */ + /* 'AttitudeEKF:288' xa_apo=x_apo; */ for (i = 0; i < 12; i++) { xa_apo[i] = x_apo[i]; } - /* 'AttitudeEKF:277' Pa_apo=P_apo; */ + /* 'AttitudeEKF:289' Pa_apo=P_apo; */ memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); /* rotation matrix from earth to body system */ - /* 'AttitudeEKF:279' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (r2 = 0; r2 < 3; r2++) { Rot_matrix[r2] = zek[r2]; Rot_matrix[3 + r2] = wak[r2]; Rot_matrix[6 + r2] = muk[r2]; } - /* 'AttitudeEKF:282' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'AttitudeEKF:283' theta=-asin(Rot_matrix(1,3)); */ - /* 'AttitudeEKF:284' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'AttitudeEKF:285' eulerAngles=[phi;theta;psi]; */ + /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h index fc02752109..7094da1da5 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h index 63eb7e5018..3f7522ffa0 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h index 9245a24c8f..b5a02a7a6d 100644 --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ From cedfdfca6018c7c35a56e2370b84f0d4f589cf68 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 22 Aug 2014 16:00:34 +0200 Subject: [PATCH 005/122] some hacks for the quadshot --- ROMFS/px4fmu_common/init.d/13000_quadshot | 14 ++++++++++++++ .../px4fmu_common/init.d/Roman_mavlink_stream_conf | 12 ++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 9 +++++++++ ROMFS/px4fmu_common/init.d/rcS | 1 + 4 files changed, 36 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot create mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot new file mode 100644 index 0000000000..8ee306a387 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13000_quadshot @@ -0,0 +1,14 @@ +#!nsh +# +# Generic quadshot configuration file +# +# Roman Bapst +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER FMU_quadshot + +set PWM_OUTPUTS 1234 +set PWM_MIN 1070 +set PWM_MAX 2000 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf new file mode 100644 index 0000000000..d26e4a3722 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf @@ -0,0 +1,12 @@ +#!nsh +# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard +# +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50 +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50 +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50 +usleep 100000 +mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50 +echo "Added additional streams on TELEM2" \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9de7d9ecdb..4f0ec02787 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -16,6 +16,7 @@ # 10000 .. 10999 Wide arm / H frame # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox +# 13000 .. 13999 Vtol # # Simulation setups @@ -232,3 +233,11 @@ if param compare SYS_AUTOSTART 12001 then sh /etc/init.d/12001_octo_cox fi + +# +# Quadshot +# + if param compare SYS_AUTOSTART 13000 + then + sh /etc/init.d/13000_quadshot + fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27cac..cd1aa14b0c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -586,6 +586,7 @@ then then echo "[init] Starting addons script: $EXTRAS_FILE" sh $EXTRAS_FILE + sh /etc/init.d/Roman_mavlink_stream_conf else echo "[init] No addons script: $EXTRAS_FILE" fi From cce45865d53eab4d298e8a22671f6eb6fb32801c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 10:57:59 +0200 Subject: [PATCH 006/122] NuttX update --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 41fffa0df1..088146b90e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c +Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 From 87259cf89de207b3cb21ab9f44578fd2c21ce119 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 10:58:46 +0200 Subject: [PATCH 007/122] added updates --- .catkin_workspace | 1 + build/CATKIN_IGNORE | 0 build/CMakeCache.txt | 450 ++++++++++++++++++ .../CMakeFiles/2.8.12.2/CMakeCCompiler.cmake | 56 +++ .../2.8.12.2/CMakeCXXCompiler.cmake | 57 +++ .../2.8.12.2/CMakeDetermineCompilerABI_C.bin | Bin 0 -> 8547 bytes .../CMakeDetermineCompilerABI_CXX.bin | Bin 0 -> 8560 bytes build/CMakeFiles/2.8.12.2/CMakeSystem.cmake | 15 + .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c | 389 +++++++++++++++ build/CMakeFiles/2.8.12.2/CompilerIdC/a.out | Bin 0 -> 8643 bytes .../CompilerIdCXX/CMakeCXXCompilerId.cpp | 377 +++++++++++++++ build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 0 -> 8652 bytes .../CMakeDirectoryInformation.cmake | 16 + build/CMakeFiles/CMakeError.log | 53 +++ build/CMakeFiles/CMakeOutput.log | 293 ++++++++++++ build/CMakeFiles/CMakeRuleHashes.txt | 5 + build/CMakeFiles/Makefile.cmake | 150 ++++++ build/CMakeFiles/Makefile2 | 266 +++++++++++ build/CMakeFiles/TargetDirectories.txt | 6 + .../clean_test_results.dir/DependInfo.cmake | 20 + .../clean_test_results.dir/build.make | 66 +++ .../clean_test_results.dir/cmake_clean.cmake | 8 + .../clean_test_results.dir/progress.make | 1 + build/CMakeFiles/cmake.check_cache | 1 + build/CMakeFiles/doxygen.dir/DependInfo.cmake | 20 + build/CMakeFiles/doxygen.dir/build.make | 65 +++ .../CMakeFiles/doxygen.dir/cmake_clean.cmake | 8 + build/CMakeFiles/doxygen.dir/progress.make | 1 + build/CMakeFiles/progress.marks | 1 + .../CMakeFiles/run_tests.dir/DependInfo.cmake | 20 + build/CMakeFiles/run_tests.dir/build.make | 65 +++ .../run_tests.dir/cmake_clean.cmake | 8 + build/CMakeFiles/run_tests.dir/progress.make | 1 + build/CMakeFiles/tests.dir/DependInfo.cmake | 20 + build/CMakeFiles/tests.dir/build.make | 65 +++ build/CMakeFiles/tests.dir/cmake_clean.cmake | 8 + build/CMakeFiles/tests.dir/progress.make | 1 + build/CTestTestfile.cmake | 7 + build/Makefile | 262 ++++++++++ .../catkin_generated/version/package.cmake | 9 + build/catkin_generated/env_cached.sh | 16 + .../catkin_generated/generate_cached_setup.py | 29 ++ .../catkin_generated/installspace/.rosinstall | 2 + .../installspace/_setup_util.py | 287 +++++++++++ build/catkin_generated/installspace/env.sh | 16 + .../catkin_generated/installspace/setup.bash | 8 + build/catkin_generated/installspace/setup.sh | 87 ++++ build/catkin_generated/installspace/setup.zsh | 8 + build/catkin_generated/order_packages.cmake | 10 + build/catkin_generated/order_packages.py | 5 + build/catkin_generated/setup_cached.sh | 20 + .../Project/interrogate_setup_dot_py.py.stamp | 250 ++++++++++ .../Project/order_packages.cmake.em.stamp | 56 +++ .../stamps/Project/package.xml.stamp | 37 ++ build/catkin_make.cache | 1 + build/cmake_install.cmake | 140 ++++++ .../CMakeDirectoryInformation.cmake | 16 + .../CMakeFiles/gtest.dir/DependInfo.cmake | 27 ++ build/gtest/CMakeFiles/gtest.dir/build.make | 102 ++++ .../CMakeFiles/gtest.dir/cmake_clean.cmake | 10 + build/gtest/CMakeFiles/gtest.dir/depend.make | 2 + build/gtest/CMakeFiles/gtest.dir/flags.make | 8 + build/gtest/CMakeFiles/gtest.dir/link.txt | 1 + .../gtest/CMakeFiles/gtest.dir/progress.make | 2 + .../gtest_main.dir/DependInfo.cmake | 28 ++ .../CMakeFiles/gtest_main.dir/build.make | 103 ++++ .../gtest_main.dir/cmake_clean.cmake | 10 + .../CMakeFiles/gtest_main.dir/depend.make | 2 + .../CMakeFiles/gtest_main.dir/flags.make | 8 + .../gtest/CMakeFiles/gtest_main.dir/link.txt | 1 + .../CMakeFiles/gtest_main.dir/progress.make | 2 + build/gtest/CMakeFiles/progress.marks | 1 + build/gtest/CTestTestfile.cmake | 6 + build/gtest/Makefile | 262 ++++++++++ build/gtest/cmake_install.cmake | 34 ++ devel/.catkin | 1 + devel/.rosinstall | 2 + devel/_setup_util.py | 287 +++++++++++ devel/env.sh | 16 + devel/setup.bash | 8 + devel/setup.sh | 87 ++++ devel/setup.zsh | 8 + mavlink/include/mavlink/v1.0 | 2 +- uavcan | 2 +- 84 files changed, 4799 insertions(+), 2 deletions(-) create mode 100644 .catkin_workspace create mode 100644 build/CATKIN_IGNORE create mode 100644 build/CMakeCache.txt create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin create mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out create mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake create mode 100644 build/CMakeFiles/CMakeError.log create mode 100644 build/CMakeFiles/CMakeOutput.log create mode 100644 build/CMakeFiles/CMakeRuleHashes.txt create mode 100644 build/CMakeFiles/Makefile.cmake create mode 100644 build/CMakeFiles/Makefile2 create mode 100644 build/CMakeFiles/TargetDirectories.txt create mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/clean_test_results.dir/build.make create mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make create mode 100644 build/CMakeFiles/cmake.check_cache create mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/doxygen.dir/build.make create mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/doxygen.dir/progress.make create mode 100644 build/CMakeFiles/progress.marks create mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/run_tests.dir/build.make create mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/run_tests.dir/progress.make create mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/tests.dir/build.make create mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/tests.dir/progress.make create mode 100644 build/CTestTestfile.cmake create mode 100644 build/Makefile create mode 100644 build/catkin/catkin_generated/version/package.cmake create mode 100755 build/catkin_generated/env_cached.sh create mode 100644 build/catkin_generated/generate_cached_setup.py create mode 100644 build/catkin_generated/installspace/.rosinstall create mode 100755 build/catkin_generated/installspace/_setup_util.py create mode 100755 build/catkin_generated/installspace/env.sh create mode 100644 build/catkin_generated/installspace/setup.bash create mode 100644 build/catkin_generated/installspace/setup.sh create mode 100644 build/catkin_generated/installspace/setup.zsh create mode 100644 build/catkin_generated/order_packages.cmake create mode 100644 build/catkin_generated/order_packages.py create mode 100755 build/catkin_generated/setup_cached.sh create mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp create mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp create mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp create mode 100644 build/catkin_make.cache create mode 100644 build/cmake_install.cmake create mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt create mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make create mode 100644 build/gtest/CMakeFiles/progress.marks create mode 100644 build/gtest/CTestTestfile.cmake create mode 100644 build/gtest/Makefile create mode 100644 build/gtest/cmake_install.cmake create mode 100644 devel/.catkin create mode 100644 devel/.rosinstall create mode 100755 devel/_setup_util.py create mode 100755 devel/env.sh create mode 100644 devel/setup.bash create mode 100644 devel/setup.sh create mode 100644 devel/setup.zsh diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 0000000000..52fd97e7ea --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt new file mode 100644 index 0000000000..8fc5c4b4a8 --- /dev/null +++ b/build/CMakeCache.txt @@ -0,0 +1,450 @@ +# This is the CMakeCache file. +# For build in directory: /home/roman/src/Firmware/build +# It was generated by CMake: /usr/bin/cmake +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//Build shared libraries (DLLs). +BUILD_SHARED_LIBS:BOOL=ON + +//List of ';' separated packages to exclude +CATKIN_BLACKLIST_PACKAGES:STRING= + +//catkin devel space +CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel + +//Catkin enable testing +CATKIN_ENABLE_TESTING:BOOL=ON + +//Catkin skip testing +CATKIN_SKIP_TESTING:BOOL=OFF + +//List of ';' separated packages to build +CATKIN_WHITELIST_PACKAGES:STRING= + +//Path to a program. +CMAKE_AR:FILEPATH=/usr/bin/ar + +//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or +// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel. +CMAKE_BUILD_TYPE:STRING= + +//Enable/Disable color output during build. +CMAKE_COLOR_MAKEFILE:BOOL=ON + +//CXX compiler. +CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + +//Flags used by the compiler during all build types. +CMAKE_CXX_FLAGS:STRING= + +//Flags used by the compiler during debug builds. +CMAKE_CXX_FLAGS_DEBUG:STRING=-g + +//Flags used by the compiler during release minsize builds. +CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the compiler during release builds (/MD /Ob1 /Oi +// /Ot /Oy /Gs will produce slightly less optimized but smaller +// files). +CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the compiler during Release with Debug Info builds. +CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//C compiler. +CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + +//Flags used by the compiler during all build types. +CMAKE_C_FLAGS:STRING= + +//Flags used by the compiler during debug builds. +CMAKE_C_FLAGS_DEBUG:STRING=-g + +//Flags used by the compiler during release minsize builds. +CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the compiler during release builds (/MD /Ob1 /Oi +// /Ot /Oy /Gs will produce slightly less optimized but smaller +// files). +CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the compiler during Release with Debug Info builds. +CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//Flags used by the linker. +CMAKE_EXE_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Enable/Disable output of compile commands during generation. +CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF + +//Install path prefix, prepended onto install directories. +CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install + +//Path to a program. +CMAKE_LINKER:FILEPATH=/usr/bin/ld + +//Path to a program. +CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make + +//Flags used by the linker during the creation of modules. +CMAKE_MODULE_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_NM:FILEPATH=/usr/bin/nm + +//Path to a program. +CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy + +//Path to a program. +CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump + +//Value Computed by CMake +CMAKE_PROJECT_NAME:STATIC=Project + +//Path to a program. +CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib + +//Flags used by the linker during the creation of dll's. +CMAKE_SHARED_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//If set, runtime paths are not added when installing shared libraries, +// but are added when building. +CMAKE_SKIP_INSTALL_RPATH:BOOL=NO + +//If set, runtime paths are not added when using shared libraries. +CMAKE_SKIP_RPATH:BOOL=NO + +//Flags used by the linker during the creation of static libraries. +CMAKE_STATIC_LINKER_FLAGS:STRING= + +//Flags used by the linker during debug builds. +CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_STRIP:FILEPATH=/usr/bin/strip + +//If true, cmake will use relative paths in makefiles and projects. +CMAKE_USE_RELATIVE_PATHS:BOOL=OFF + +//If this value is on, makefiles will be generated without the +// .SILENT directive, and all commands will be echoed to the console +// during the make. This is useful for debugging only. With Visual +// Studio IDE projects all commands are done without /nologo. +CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE + +//Path to a program. +DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND + +//Path to a program. +EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy + +//Empy script +EMPY_SCRIPT:STRING=/usr/bin/empy + +//Path to a file. +GTEST_INCLUDE_DIR:PATH=/usr/include + +//Path to a library. +GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND + +//Path to a library. +GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND + +//Path to a library. +GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND + +//Path to a library. +GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND + +//lsb_release executable was found +LSB_FOUND:BOOL=TRUE + +//Path to a program. +LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release + +//Path to a program. +NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7 + +//Path to a program. +PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python + +//Specify specific Python version to use ('major.minor' or 'major') +PYTHON_VERSION:STRING= + +//Value Computed by CMake +Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build + +//Value Computed by CMake +Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src + +//Path to a library. +RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so + +//Enable debian style python package layout +SETUPTOOLS_DEB_LAYOUT:BOOL=ON + +//LSB Distrib tag +UBUNTU:BOOL=TRUE + +//LSB Distrib - codename tag +UBUNTU_TRUSTY:BOOL=TRUE + +//Path to a file. +_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h + +//Path to a file. +_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc + +//The directory containing a CMake configuration file for catkin. +catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake + +//Value Computed by CMake +gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest + +//Dependencies for the target +gtest_LIB_DEPENDS:STATIC=general;-lpthread; + +//Value Computed by CMake +gtest_SOURCE_DIR:STATIC=/usr/src/gtest + +//Build gtest's sample programs. +gtest_build_samples:BOOL=OFF + +//Build all of gtest's own tests. +gtest_build_tests:BOOL=OFF + +//Disable uses of pthreads in gtest. +gtest_disable_pthreads:BOOL=OFF + +//Use shared (DLL) run-time lib even when Google Test is built +// as static lib. +gtest_force_shared_crt:BOOL=OFF + +//Dependencies for the target +gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest; + + +######################## +# INTERNAL cache entries +######################## + +//catkin environment +CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh +CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results +//ADVANCED property for variable: CMAKE_AR +CMAKE_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_BUILD_TOOL +CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1 +//What is the target build tool cmake is generating for. +CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=12 +//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE +CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=/usr/bin/cmake +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest +//ADVANCED property for variable: CMAKE_CXX_COMPILER +CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS +CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG +CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL +CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE +CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO +CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER +CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS +CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG +CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL +CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE +CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO +CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//Executable file format +CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS +CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG +CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE +CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS +CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Unix Makefiles +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Have symbol pthread_create +CMAKE_HAVE_LIBC_CREATE:INTERNAL= +//Have library pthreads +CMAKE_HAVE_PTHREADS_CREATE:INTERNAL= +//Have library pthread +CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1 +//Have include pthread.h +CMAKE_HAVE_PTHREAD_H:INTERNAL=1 +//Start directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src +//Install .so files without execute permission. +CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 +//ADVANCED property for variable: CMAKE_LINKER +CMAKE_LINKER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MAKE_PROGRAM +CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS +CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG +CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE +CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_NM +CMAKE_NM-ADVANCED:INTERNAL=1 +//number of local generators +CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2 +//ADVANCED property for variable: CMAKE_OBJCOPY +CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJDUMP +CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RANLIB +CMAKE_RANLIB-ADVANCED:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS +CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG +CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE +CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH +CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_RPATH +CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS +CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG +CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE +CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STRIP +CMAKE_STRIP-ADVANCED:INTERNAL=1 +//uname command +CMAKE_UNAME:INTERNAL=/bin/uname +//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS +CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE +CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 +//Details about finding PythonInterp +FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()] +//Details about finding Threads +FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()] +GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE +GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include +GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest +GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest +GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main +//ADVANCED property for variable: GTEST_INCLUDE_DIR +GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_LIBRARY +GTEST_LIBRARY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_LIBRARY_DEBUG +GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_MAIN_LIBRARY +GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG +GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: PYTHON_EXECUTABLE +PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1 +//This needs to be in PYTHONPATH when 'setup.py install' is called. +// And it needs to match. But setuptools won't tell us where +// it will install things. +PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages + diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake new file mode 100644 index 0000000000..83254ce491 --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake @@ -0,0 +1,56 @@ +set(CMAKE_C_COMPILER "/usr/bin/cc") +set(CMAKE_C_COMPILER_ARG1 "") +set(CMAKE_C_COMPILER_ID "GNU") +set(CMAKE_C_COMPILER_VERSION "4.8.2") +set(CMAKE_C_PLATFORM_ID "Linux") + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_COMPILER_IS_GNUCC 1) +set(CMAKE_C_COMPILER_LOADED 1) +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_C_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_C_COMPILER_ENV_VAR "CC") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_C_COMPILER_ID_RUN 1) +set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) +set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_C_LINKER_PREFERENCE 10) + +# Save compiler ABI information. +set(CMAKE_C_SIZEOF_DATA_PTR "8") +set(CMAKE_C_COMPILER_ABI "ELF") +set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_C_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_C_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") +endif() + +if(CMAKE_C_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + + + + +set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") +set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") + + + diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake new file mode 100644 index 0000000000..c4373d570c --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake @@ -0,0 +1,57 @@ +set(CMAKE_CXX_COMPILER "/usr/bin/c++") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "GNU") +set(CMAKE_CXX_COMPILER_VERSION "4.8.2") +set(CMAKE_CXX_PLATFORM_ID "Linux") + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_COMPILER_IS_GNUCXX 1) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "ELF") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + + + + +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") + + + diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin new file mode 100755 index 0000000000000000000000000000000000000000..2f2ebe478334290d81ee498f150d9bab78baa7e7 GIT binary patch literal 8547 zcmeHMeQaCR6+iaNheH#`?FyunQE#JLV9Fycp)(*x9M|b{>$F{)z_wy;o%kg&_@lF* zOUufr$f#yP8T%0HDug=3ANhm*Ip{Vt(gjhZF{x}~LhB|Wx@>J}M-d&05Y3&=edoNq zJUbH;5`XnqHP`pNbM86!5EjE%9;57GjC|=D`-)<{8d7?<2X>>K_cr1l1>e_XD&&2UQO zMQok1GWgq~hOB_yFD`qCk4u*O#*CFgJ3lEx#JG+0EjBI}sDE5i6>iF9hu8Jol*@GG zvW3c?u04@;UF&-Ea#3HaY{K@Dcf;l(rHRL}gT`e!fLa6i{rvhPFK(P%(X%PF@1?gk zKJ|y5ANiia^@06Cxn^9Cq5T2)!c|KG4GnESXnb%yRRkom*1ZH@xIpE zezxTKO{b>*`o~kxtQ|gi{;ltP^V<6!KD_VO2j362E`R5lqaBS$-~ID@G@G^Pa%O%Z zmH@!Ag(QW-BK*Hxgnn!h`nOeIIv^6V*CiuO^iB``X+>WFy&n3AYQLj~{~-Wa3hmHK z>xtLRhyTuT_*SM`EijrDQ-c;;;>rCfa&>uB}G z=M&X8?lr3K7!&`vuyrtTd;OL#9oiJo{;d(SC zW`jm`<^g|d`Y0+u!!D*?#!vF?_Er0lb`)#Oe{5DWsxwCQ_r|facN)GE#v8L0e`@+S zw(y^>3>|BQ({UTX>iSl2lw zEFXF~9NY;;bw-{@o%ybmvV9etDN8A_jp{?^jB4fWtpeA#-a8X_T%zz|J{@? z{xMTb7vc|ST!5$Aw_vpZBsTYNxy|K9JGvt&o0uNX?uaM*Qvkk|+U?!6kDt3F+6uo$p82P`!ihQ+&_k@Bf*TMPxZd_a?scO*Po(RKDxX()TNBfJLR?K{fX=Z@ztCvg!*;Vix`sOSIcfLj1o2C%^LSpM`Xqh- ztibnJFLqNu4|vY)j195aZQ;(L;Yz`(gnRUezP4-4trb;WbAwF)(93)CR(crpvQ@In zasHJpSY~M#=!K$X>e0SLmz5r)+E}5Y4_C6eOjkAosy3c3kAt4sTPW|%+oe^qzw9x%Iux<|7-zeDi*g#Ah~556BV)_CMC@;i$f#B-G!UU@t(eJEv-^|N0eA_Cuf+0WUZ z7@Gl&(%B*5eEwV&}Iy_>K+$DIo#&;4Zm-u~ll0lUib9LE9_NICAIJjQmf z{1Y^;9CtbHM~J}joZ+4Ssz-h&#g&~N{S1De;_{~+d4898lH{NDOc={C4twNzpX#BG za=d1E?LR{DJbwI+(o6Dq{dL2uf6^n*?|J;r$M1Ozul^T2@{{xqGD-R`vLJ!yk1>T; z9oLr}sdX=Y5OcW%>P; z*C*@edHAPCKfi;_Vy9f|ox$>4zUz_aa|0{rWI+PUaf$PTOVqKS&K;d(Alu1cImSn1 zBg^yqTZ9C?{2q4zN&4B(87%LO-z?81KL4|BH|}Bm%t-K5Rh@mujnfpkx@qF@T@Y>L z{%~AF|E#CKW0l?7CQsvD{r?LZ_?-RJ4GsLxKIw)AnzR48p+SSQueqT?qqASRp+S?g z54oX1v$Marp+UgeH{8&m#n}(s(BKLu&by)2d5=YC(CWlxH#BH-;;kE6z4utv&ip&G z8ybY1xaWojOPzS;h6c-=IOT>0pL61m8yYNk;))v@T-L2Z6x0C&2 zRMBh4^B}du_d&+b6@Tr#;A>~Se&N31gLc5@O9UrgP=7uCHx>Udem42+{D?Q-vBSri#)Wu5 z(QETKkNE67jyFH@6CZ9xwfUpNN^fo6=w_b0~di`aRR?BVkepIgpSf4zC7 zk5v1$c`Hf$e6C|O`LSQ~>z1+1Uq9b;wZHy&esvLgT(wi1pMsH+RkkXlqxuL4u`#wy zBoo^Qg@F0=k+AY&1QSSQ;QWn?Bt)ht#&X5sbWUWfVyP_Bl|3+0%f_*v7RPy<~An2;9YAVmPfHgLdzTb?+iuqmHoLP$YCG-(HwUr*u zf)M@Nq62Xe-@G2PSE^h58*j%>OPIFGl#60KUC8825bM9SIXaMtfpD{_L>mY(I1uAj zHYB(7MU!GnfB*LQpcstyCF7_^<>fw67S5GxTQnhOr}DhXzTRGUzU)(H?YMLE$vZov M_a>@?ti0lyfbasU7T literal 0 HcmV?d00001 diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin new file mode 100755 index 0000000000000000000000000000000000000000..16c737f2604406275900604b68676d59840e9fbb GIT binary patch literal 8560 zcmeHMZ){w}5udx~e@E=QO9-t=()d~u)qwo4Lr4t~jnC(E-lgY+IyR(*%jWDm#~1F8 zd-o3PkU~^9sHYGbRYgss3Zbe}^MOiyqO?>csS!bls(}_nOOc`kS4jveh?H6t@m8~M z=K1;ErJ_Qr`XRq4-rL!k+1amof)H4|IbM zdSErIga!bpS2PPfB*EnzsphhY_%r~h1AuO?Md4bk|x?6b_U zOXNk|HEX8ucONP0YqG}0<$B`dlI6ZWYo^f7Pm3Ti4pM(t`Ys>P__(Ah+>*_VY*>Fw zHr0{Ml3C;pDD^j($%s5OA?>sLPZ;2&=I&ZfP- z=XbU~@$T5sWa2cg4;&B5H3Gol;Ql7~;&NY8U0uuf>YxYPaH_GN-sl?__xK`jK7aNH z`AWsOZ;2kBdI2^tV>=X8~X-1RzN3 ziPz0TYFsUFxY1f?Y3lfHqO%`h9HxHVcH+D)gBBPdJsU~jfTAyfAo&x<7l1lNaSuI_1=!$!AOu={yogN+lNG z@_-nmz9QLVsgy2RCe$=wb3E1?5$p7I`fX}(JaW$!{#meEt@Vy5ugLgtcf3OFa{+7J z;J(1U!)T@TfqRI1LWCT2C%-X$vB@}C|Bf#N#)0Ez{kfX~z?goqX~xnhZ+qPr0=f22 zz7Pc3dO)tl`ZzY8KP{z{YhS@SZvFG8q?BFn$11PwG%9C|>32W4YbbW|IL^3n@&qn8 zzJ|zKrQPzfqZe(3_Zf4m&`DE+t;TCV4v0wjXKF zVU77u%xOkt)~Ni>IB{W<;W=fzHdpp0W`1o6ziwVi7?qbS`7CW^jDQ^qq4JApmo=ZNl>57efABU{q%1sl@q*q?Ca+IA4-dqK?Y9 zFB+Bd>66h7fxZ6V_q|bn z>*HF~A39VQ^|wD(Z}>YOZ7}@Z(~SfEo~*w+?C%Ww+k5??UVm$^KiKPUQg%KK0LSK~ zbndsSxQeT|ivM?n=>7&=68PTFWi!$FUe`i(zPDY^?etz+Np*f7t)O}oAkM zskS>`)jY^o!Z0#zGNmc~I(P322dIjb~_R?7qFVkuL|FRF+l=*4t4i30R%B5Q)K zF7LXTo-{#M&jh_#P;UV}Jub$I$y{2Dr-}k7QpXA zMwd(8qS&G}-5d6B_vJ z^S}uWuCe{R6IzXUEJA~3+b=tzL5uBgozQBKRomfL>C2@S5Z{e=@6e9ra*PH1@Br~T=KRwJ&C(BOJ|KRTgRor=~zsDlr2 zJ1zpAxt<#K<2%ZW@p;!hTqpW*7d=dL_xtT0rGLRr-XCL%Uj01}kskX!Ncgehubvl- zj@BC2Da9WE41gl|9cKCC0ALst!ATjktCpURl%Az9?z8wAn7XX>-4M0(w5WkEt6D#+ zm7Z4Ubj|zCie9~b){`DSM;V(*5Bq1vc15pVR~e6$H*x1draYX1T#P3ay*j`1ln0NZ z>ip5qmHz5{$}beX8n2QbKA#wWzJfjHJhk&m|D4=JBjbuP7EqN)%htHEt(~>JT|6}f)HCGgCZW=J|qMzq?Lr36Qh_| zDgh^Y93&x91+hC@7)fSD$}AL1B3Yh<(L!z_n>N!aeZ%cvSy!7$<*=-$rby?_;uKhq z(^NT^n*u>kRaH|7sYR@jf#m(^$c`P6LT)0HO&7zxF@1Dm0>n4tT^lm_v4U^}^O4%E zh{=%*2+=&vsX_B^d^_9yVD>9jDv0r9K7|>zzHe*|55yuMoNOvR2SN-D zL|Dt___p3~Tx{#_-yR(jL*d?d6#G%xyib+8bEfPljc4Wi+!?{19%q*AGw1qvEDI?= OEA9AS<^8PW;lBYd8IS4! literal 0 HcmV?d00001 diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake new file mode 100644 index 0000000000..0616b7c1ee --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake @@ -0,0 +1,15 @@ +set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic") +set(CMAKE_HOST_SYSTEM_NAME "Linux") +set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic") +set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") + + + +set(CMAKE_SYSTEM "Linux-3.13.0-24-generic") +set(CMAKE_SYSTEM_NAME "Linux") +set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic") +set(CMAKE_SYSTEM_PROCESSOR "x86_64") + +set(CMAKE_CROSSCOMPILING "FALSE") + +set(CMAKE_SYSTEM_LOADED 1) diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c new file mode 100644 index 0000000000..cba81d4a6b --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c @@ -0,0 +1,389 @@ +#ifdef __cplusplus +# error "A C++ compiler has been selected for C." +#endif + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__18CXX) +# define ID_VOID_MAIN +#endif + +#if defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" + /* __INTEL_COMPILER = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) + +#elif defined(__SUNPRO_C) +# define COMPILER_ID "SunPro" +# if __SUNPRO_C >= 0x5100 + /* __SUNPRO_C = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# else + /* __SUNPRO_C = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# endif + +#elif defined(__HP_cc) +# define COMPILER_ID "HP" + /* __HP_cc = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) + +#elif defined(__DECC) +# define COMPILER_ID "Compaq" + /* __DECC_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) + +#elif defined(__IBMC__) +# if defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" +# else +# if __IBMC__ >= 800 +# define COMPILER_ID "XL" +# else +# define COMPILER_ID "VisualAge" +# endif + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__TINYC__) +# define COMPILER_ID "TinyCC" + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +/* Analog VisualDSP++ >= 4.5.6 */ +#elif defined(__VISUALDSPVERSION__) +# define COMPILER_ID "ADSP" + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) + +/* Analog VisualDSP++ < 4.5.6 */ +#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" + +/* IAR Systems compiler for embedded systems. + http://www.iar.com */ +#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" + +/* sdcc, the small devices C compiler for embedded systems, + http://sdcc.sourceforge.net */ +#elif defined(SDCC) +# define COMPILER_ID "SDCC" + /* SDCC = VRP */ +# define COMPILER_VERSION_MAJOR DEC(SDCC/100) +# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) +# define COMPILER_VERSION_PATCH DEC(SDCC % 10) + +#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) +# define COMPILER_ID "MIPSpro" +# if defined(_SGI_COMPILER_VERSION) + /* _SGI_COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) +# else + /* _COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) +# endif + +/* This compiler is either not known or is too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__sgi) +# define COMPILER_ID "MIPSpro" + +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" + +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) +# define PLATFORM_ID "IRIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#else /* unknown platform */ +# define PLATFORM_ID "" + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM) +# define ARCHITECTURE_ID "ARM" + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID "" +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number components. */ +#ifdef COMPILER_VERSION_MAJOR +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +/*--------------------------------------------------------------------------*/ + +#ifdef ID_VOID_MAIN +void main() {} +#else +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif + (void)argv; + return require; +} +#endif diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out new file mode 100755 index 0000000000000000000000000000000000000000..33a3d2b580160e3c32c235a17c6d2c6dc266b182 GIT binary patch literal 8643 zcmeHMZ){sv6+iY%nsiBRr?il^uvlY7S+%^iSytRlqjp@U&y|ywG+8&b>5UURiJAWt z`*}+SgF@IOE9+JXgtEbheL$i}d_<-2fwg2>KLAo%4QU9F%0#NRYo(%Xptym>4Vzg$uO2&Kdc-64^VysR6mycEHiA0X%UAO z%oKi`Sg4X#y-bo_mfDug`|5(3BFZO1fKv5Sd8@U{ZK@wj3c_RA%*gJp$Fix8Y$jix z?3fJg?%3U>mkN5PvI+CYwEO!8RY}~At<)}?0Mr=3`0u=RxufOa;FsR~)zp0H+TQP; z?)%Zp$mRe41x*03u?FD_c8|J{&5Vqq*}Fjq{1zoMSu{m1naP6?Phk8Z-5= z<{%iaoHy&QJq`fI^t+97%(Nh%=xh#x+}_n31b+@2| zUAN+uOUI4MhsN~ZZyy?nUpzlw9|q&%g<&Nw-l_}3wO>jp?~M7|_5qwb5*UU-qvKFu z7>wyfpHW$O(K|GE4hulT?hL(yQSyqv?Ih~XVTjXTSk#Qlf>HUcapA^Z!!vJOS}c2q z=6-FFFPnFUjLLf!|3l)ReEMfiL4b10pix=4_AM#p(0lbiYzl(s<2kIKG5z$j>w^I1 zgSH8q2Hy$#hQX*T$oZk`-;h%J+u(XhN{M+?UcPBm%2zJN?x|8|Hg`4!VJ3Fdm^D1} z{%x@7Ozhs!+}P>Jkw`^Go{3lPMh4@RKSl-; zr?$^F24T=R)q(PG;*o`2e>N`M*RY4LZC;Ync;%M)pug>VXdfghHxiY*`zkUbH~qz! zp7R)cJ}Te9jz2mSITjg;9FK^(u`!*~e>{i7;$|Zc4bO^ERE>CF&%q~03%L`SY`S=K zf8XHH$}}tD#S7_TDO1QF-D#mqr;iYSbPidJ6WOFWRw(9LjqP>p$_kN(4b zDZh@2h%fM}H@ab+H{0CMXvL+_rz}aC#`tbu;0K@udXwE zq3MQRUpVUvMSMFWzV>cku-n(#?F)4K8kLpRcx2n9oOBDcnluDL&_M{6Cju0h7$qxye5BQF(mU zIY;piP<|njcn(0|c>}STBm>W%3bfzmwO8#4V0phm$4U4cC7-17SwBo+wwudaPw6?l zB(3^{DZQjHx94pNhbaEj0-s~uxJq{1UCkakkM@%sC%HcweKOcSI8x4=oM6a1CQo9W^S(DMZ|tw*}!9cFTzLgV?eK2pwPQyrNU zsL({RGy!^QDqot)S<)<8F|<)#5fDYti|K3<3G{U$Yl5E1XH3w|^rQ*;SSFtVy;w*k z%_QjQ2{Beo=F(y!Ra_2P43R7rlT#LhzrQwGRAwY|nNiTk3#R(9O0jYrDV0DUE#z|P zyyE%vd~ScY{fw9BK85-0kA{iQ_2ly1 z^~X~t_6hUZzfBUK{UP$v=ezl@Q@e6Kc|ABoeD?DUcmG#h{CV;>^RDt4d~e0_2QEI} zTXI2{T^+_e##=5vuV3#|gRviHxb1(N_}qS1X#KlF{O>c7!2QWMe|j*L)x7PVWx?9sv9fKCkDkbYJRD?+$-J_EUEQ z-rOL9pOF7ox}Rk}uV;K;%lEQ8KDm4z2UlI?^L^bSR?0En8O&$-k&Dm!2`-?Wi3H}c z#Qwn{YTHlyk#;h0H9q5CWFzzOx(#uN_*@RdH61`w`8>}V%y;{5=Cj0S0WRAKlgsCX z1XoandG4$LZ6JrKnY0zM=FHUICXs;ViXt3U151i280o%_zp+S@Fzn##a z+4jp$Xt?Wlof8lm_-sGwga&@wKRTg7!1jAiXwYK&Gbc3IX!|KAH293|f1J=@lkHcW z&|tIeFPzZev$h{_Lc>!4%`Yc3Xa%0PPH3>jo{vswRjZ;k59;6_I2~62*IZ8x`*C`B zVFT=RwZBgKVV6EadiS~Rh{}K2PM#lQN?+ZcrzjsjCo8tw%Z{=&%3(Kjc|DDRG z1!fzpb0p(WN?*GlYvr?`h#RZM!(Ek6b-!B;Uv2+J54L}GzqfnvooNH$YYBqgOvuA8 zhbu9l^wr};cyOs`tr{m|D*x(vlvVm_%uqh=aX7n*JwNpTw0K)#r)xa_0`-o0%KMEu zrEi5WrRR0!614+97a`PLH~{=j*|P)AR(!2e}Ey_RS%KzV#53g&C2Cvor_&$YD zcl5YN_113xpx3IeJD*2aN#CRLul9!t%m@5{4?T!Z^$YNv(zk-)vL8l^X2~p%jp?Hx z#DQqPNW>2h2mz^dF+H9sndzc1b7C}G$frwCLnTD2AjY$Wkz`h+%tEmwlI2Oj7n^L_ zOsDkSd%oORJCVS{taUUM>AYE-0_!lFD(7-jAn0hTLITggE0RWglh3E4QF<7PrdKIkU7nR};tmfEx}wpKOJ25?4O$3Xv)m#6&Wm z%BDfY4~mkR%H&14lukh=KUNU@pyf1FT^xus*4L_<>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) + +#elif defined(__SUNPRO_CC) +# define COMPILER_ID "SunPro" +# if __SUNPRO_CC >= 0x5100 + /* __SUNPRO_CC = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# endif + +#elif defined(__HP_aCC) +# define COMPILER_ID "HP" + /* __HP_aCC = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) + +#elif defined(__DECCXX) +# define COMPILER_ID "Compaq" + /* __DECCXX_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) + +#elif defined(__IBMCPP__) +# if defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" +# else +# if __IBMCPP__ >= 800 +# define COMPILER_ID "XL" +# else +# define COMPILER_ID "VisualAge" +# endif + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +/* Analog VisualDSP++ >= 4.5.6 */ +#elif defined(__VISUALDSPVERSION__) +# define COMPILER_ID "ADSP" + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) + +/* Analog VisualDSP++ < 4.5.6 */ +#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" + +/* IAR Systems compiler for embedded systems. + http://www.iar.com */ +#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" + +#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) +# define COMPILER_ID "MIPSpro" +# if defined(_SGI_COMPILER_VERSION) + /* _SGI_COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) +# else + /* _COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) +# endif + +/* This compiler is either not known or is too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__sgi) +# define COMPILER_ID "MIPSpro" + +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" + +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) +# define PLATFORM_ID "IRIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#else /* unknown platform */ +# define PLATFORM_ID "" + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM) +# define ARCHITECTURE_ID "ARM" + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID "" +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number components. */ +#ifdef COMPILER_VERSION_MAJOR +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +/*--------------------------------------------------------------------------*/ + +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif + (void)argv; + return require; +} diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out new file mode 100755 index 0000000000000000000000000000000000000000..db35dbde0e2fe3ccc9c963b6592aa3f3f3eb7206 GIT binary patch literal 8652 zcmeHMZ){sv6+iY%nsiBRw`(D7S!0zIWfgf#vMji(*4as&zEnN-WDilIy=Ab6mi$hQRl*V= zs4|d;z6N|&dB8WMey|La9|lS0VG?;w#A_lR%XxB)CASaThX4DizJ6*S%e|HxwnSaT z!38sczfCl%q}48yWV@AZOXhuc!Aub4Lm@y~hsfUD`sD_-k0mAH!E|c4qy52jqB)() z7AKo0LmkZ>?Rp`nw<({ne$?I9H=rzWKQ>UmYy?nq0L%T~ZTiWcUqAPwue>7PY2JPP zXE#4J@>S&W|NjMb0I;C~;XZprT}Y>fN6_sK5CVUTf*H@7A`?$#L5Q(TE=xHA_QiTS zyF{DbraugzT7*90Kd}3=xFxj~%RWtJ7X8*d!?Cfa763E^tbClV$Y%r{`A&Uf`dY1V zsybR91mlHSv-3 z^oK})?DUSA+8_)Vr<+k8jypt+vNG>yxN+w@oQp$P{sCa?!JbMT{lL61x`kLBTJT(D~ zBe+L|?nt@Q;Th&5R_X)XPx^1&mJ)pcU!dF9u-{+zOzlZ{Y|HMQ58b~Vv$0N$(Z9VT z?w6!YVtJ=8@HKDL*YL6y^#xB?MSV>#RU5vR7uOiR z&~#0&FP!#;BEFW0uc^})?DREs`U0K4TIJ^t(f=hWo$GxSt60SyfdzB=E zzfoD%Q^Aa8oln+L`a-pp-b`seUu~r{pO3asx`ED9e9q(Ye=Nxys+^&7lYQnRdwkY8 zN9FfWeIb%~4nW{}1F?xD1J9od^u5h{ulgo{zJ0{xmuo1mw%DHC)vIcb7En#!g?&*u_x zGY)!kT#V-9nWPv`_>{%q-%pO@l^gL)Y6SGLoT>g;R;(I_3kA?ea+ypr zYr4Dq{{pTnoI^Z+`Tm6ORd`)7c)l?{383aO^Z9;)?=7s`lV$LI1EbZ&H)%bZR3UzE zJK^TzH})+lb8Mg2`x(k$|9SoMI%D(!sI|^~jtA!`!yrYB7ua^@^SXST?je}Z@r45f z#}kIT|4sm?wak28zaa`R9H-cS)?@h;fQk*A=J*gMKKH-df5x+PpTc~OM?=KtcCvkU z`|;F@W5RrnZ`2LUY`xtKf zXI=amio-KBAUByv;PGS3;t`nE0D|{xz7KTkyVJh`0KbFJ`*{Q1m%8=c>9@##8g9Uw z8$|F!^8ZZtv&`rHjPGmtUY6%4+vj=kp36Sp*DYeB9P^#Qe3tLK`20P=2AY^iU>-{x zADp7L|MWf5L=N7K&-fSl$b7tRLmVVN+hMqt14y#Z>zu)Scl>5POMDh!+fJBlpA`wN zq)PMJSpoWh9HN24`&->H9vs(jeO9tR)M(X~c2(}S|KFg2$KFq!(7&#PJts6+Z^ttyG}vIrDJL|z*N#6z2e)+iIt$d|+{Z8ro0WZ=K zZB!B3ucZHu(q9i}YAk)m-&B6(@vT(PzZG#q**w8JW>1Crut{}DdApjFe))XZQl`TQw*a0_WDn?KL1`pehXmsEZ^zDM=A=jV^_;?FNU0PDRC z(BhiszruXS`sMGCKdSr&2$MeVH-Dvmz-J_cx+4dGe=2`A!%C*Ti(x|chNtt>MxIz!{n#o@)Jh#X2C3u zj_M;I#Qv^BBGz-bUkFGf^U1MP!A$0bnGqxDTsB#N3RyxVa$+o<8;++%!p!9hB3_&X ze6dL<&16FF*!7vV%1Qzcv)0j6B(r9I3arCyqL|4{fuN(UN(npzuTbskjX#y_I(oE= zo`!l7`pCoth|k4Z+fv!l96$Ad5Z#9&y-^YE+baY$k$0QDM52%r + +int main(int argc, char** argv) +{ + (void)argv; +#ifndef pthread_create + return ((int*)(&pthread_create))[argc]; +#else + (void)argc; + return 0; +#endif +} + +Determining if the function pthread_create exists in the pthreads failed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c +Linking C executable cmTryCompileExec26988121 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -o cmTryCompileExec26988121 -rdynamic -lpthreads +/usr/bin/ld: cannot find -lpthreads +collect2: error: ld returned 1 exit status +make[1]: *** [cmTryCompileExec26988121] Error 1 +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +make: *** [cmTryCompileExec26988121/fast] Error 2 + + diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log new file mode 100644 index 0000000000..a3757fc345 --- /dev/null +++ b/build/CMakeFiles/CMakeOutput.log @@ -0,0 +1,293 @@ +The system is: Linux - 3.13.0-24-generic - x86_64 +Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. +Compiler: /usr/bin/cc +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out" + +The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out" + +Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. +Compiler: /usr/bin/c++ +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out" + +The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out" + +Determining if the C compiler works passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c +Linking C executable cmTryCompileExec4189733644 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -o cmTryCompileExec4189733644 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Detecting C compiler ABI info compiled with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c +Linking C executable cmTryCompileExec2944435992 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1 +/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic +Using built-in specs. +COLLECT_GCC=/usr/bin/cc +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Parsed C implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"] + ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build] + ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] + ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] + ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] + ignore line: [/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c] + ignore line: [Linking C executable cmTryCompileExec2944435992] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1] + ignore line: [/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/cc] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore + arg [--sysroot=/] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTryCompileExec2944435992] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] + arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore + arg [-lgcc] ==> lib [gcc] + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--no-as-needed] ==> ignore + arg [-lc] ==> lib [c] + arg [-lgcc] ==> lib [gcc] + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--no-as-needed] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore + remove lib [gcc] + remove lib [gcc_s] + remove lib [gcc] + remove lib [gcc_s] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] + implicit libs: [c] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + +Determining if the CXX compiler works passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o +/usr/bin/c++ -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx +Linking CXX executable cmTryCompileExec191173331 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1 +/usr/bin/c++ CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -o cmTryCompileExec191173331 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Detecting CXX compiler ABI info compiled with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o +/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp +Linking CXX executable cmTryCompileExec843003076 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1 +/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic +Using built-in specs. +COLLECT_GCC=/usr/bin/c++ +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Parsed CXX implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"] + ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build] + ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] + ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] + ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] + ignore line: [/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp] + ignore line: [Linking CXX executable cmTryCompileExec843003076] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1] + ignore line: [/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/c++] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore + arg [--sysroot=/] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTryCompileExec843003076] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] + arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore + arg [-lstdc++] ==> lib [stdc++] + arg [-lm] ==> lib [m] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [-lc] ==> lib [c] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore + remove lib [gcc_s] + remove lib [gcc] + remove lib [gcc_s] + remove lib [gcc] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] + implicit libs: [stdc++;m;c] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + +Determining if files pthread.h exist passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c +Linking C executable cmTryCompileExec3897003384 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -o cmTryCompileExec3897003384 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Determining if the function pthread_create exists in the pthread passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c +Linking C executable cmTryCompileExec4077710905 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -o cmTryCompileExec4077710905 -rdynamic -lpthread +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt new file mode 100644 index 0000000000..b1bfe30c0e --- /dev/null +++ b/build/CMakeFiles/CMakeRuleHashes.txt @@ -0,0 +1,5 @@ +# Hashes of file build rules. +353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results +d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen +d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests +d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake new file mode 100644 index 0000000000..d232bacff9 --- /dev/null +++ b/build/CMakeFiles/Makefile.cmake @@ -0,0 +1,150 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# The generator used is: +SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") + +# The top level Makefile was generated from the following files: +SET(CMAKE_MAKEFILE_DEPENDS + "CMakeCache.txt" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeSystem.cmake" + "catkin/catkin_generated/version/package.cmake" + "catkin_generated/order_packages.cmake" + "/home/roman/src/Firmware/src/CMakeLists.txt" + "/opt/ros/indigo/share/catkin/cmake/../package.xml" + "/opt/ros/indigo/share/catkin/cmake/all.cmake" + "/opt/ros/indigo/share/catkin/cmake/assert.cmake" + "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake" + "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake" + "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em" + "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake" + "/opt/ros/indigo/share/catkin/cmake/empy.cmake" + "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in" + "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake" + "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py" + "/opt/ros/indigo/share/catkin/cmake/legacy.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake" + "/opt/ros/indigo/share/catkin/cmake/python.cmake" + "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake" + "/opt/ros/indigo/share/catkin/cmake/stamp.cmake" + "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake" + "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in" + "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in" + "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c" + "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp" + "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake" + "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake" + "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake" + "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake" + "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake" + "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake" + "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake" + "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake" + "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c" + "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake" + "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake" + "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake" + "/usr/share/cmake-2.8/Modules/FindGTest.cmake" + "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake" + "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake" + "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake" + "/usr/share/cmake-2.8/Modules/FindThreads.cmake" + "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake" + "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake" + "/usr/src/gtest/CMakeLists.txt" + "/usr/src/gtest/cmake/internal_utils.cmake" + ) + +# The corresponding makefile is: +SET(CMAKE_MAKEFILE_OUTPUTS + "Makefile" + "CMakeFiles/cmake.check_cache" + ) + +# Byproducts of CMake generate step: +SET(CMAKE_MAKEFILE_PRODUCTS + "CMakeFiles/2.8.12.2/CMakeSystem.cmake" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "catkin_generated/stamps/Project/package.xml.stamp" + "catkin_generated/installspace/_setup_util.py" + "catkin_generated/installspace/env.sh" + "catkin_generated/installspace/setup.bash" + "catkin_generated/installspace/setup.sh" + "catkin_generated/installspace/setup.zsh" + "catkin_generated/installspace/.rosinstall" + "catkin_generated/generate_cached_setup.py" + "catkin_generated/env_cached.sh" + "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp" + "catkin_generated/order_packages.py" + "catkin_generated/stamps/Project/order_packages.cmake.em.stamp" + "CMakeFiles/CMakeDirectoryInformation.cmake" + "gtest/CMakeFiles/CMakeDirectoryInformation.cmake" + ) + +# Dependency information for all targets: +SET(CMAKE_DEPEND_INFO_FILES + "CMakeFiles/clean_test_results.dir/DependInfo.cmake" + "CMakeFiles/doxygen.dir/DependInfo.cmake" + "CMakeFiles/run_tests.dir/DependInfo.cmake" + "CMakeFiles/tests.dir/DependInfo.cmake" + "gtest/CMakeFiles/gtest.dir/DependInfo.cmake" + "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake" + ) diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2 new file mode 100644 index 0000000000..2a11fe8ef9 --- /dev/null +++ b/build/CMakeFiles/Makefile2 @@ -0,0 +1,266 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +# The main recursive all target +all: +.PHONY : all + +# The main recursive preinstall target +preinstall: +.PHONY : preinstall + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Target rules for target CMakeFiles/clean_test_results.dir + +# All Build rule for target. +CMakeFiles/clean_test_results.dir/all: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target clean_test_results" +.PHONY : CMakeFiles/clean_test_results.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/clean_test_results.dir/rule + +# Convenience name for target. +clean_test_results: CMakeFiles/clean_test_results.dir/rule +.PHONY : clean_test_results + +# clean rule for target. +CMakeFiles/clean_test_results.dir/clean: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean +.PHONY : CMakeFiles/clean_test_results.dir/clean + +# clean rule for target. +clean: CMakeFiles/clean_test_results.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/doxygen.dir + +# All Build rule for target. +CMakeFiles/doxygen.dir/all: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target doxygen" +.PHONY : CMakeFiles/doxygen.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/doxygen.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/doxygen.dir/rule + +# Convenience name for target. +doxygen: CMakeFiles/doxygen.dir/rule +.PHONY : doxygen + +# clean rule for target. +CMakeFiles/doxygen.dir/clean: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean +.PHONY : CMakeFiles/doxygen.dir/clean + +# clean rule for target. +clean: CMakeFiles/doxygen.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/run_tests.dir + +# All Build rule for target. +CMakeFiles/run_tests.dir/all: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target run_tests" +.PHONY : CMakeFiles/run_tests.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/run_tests.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/run_tests.dir/rule + +# Convenience name for target. +run_tests: CMakeFiles/run_tests.dir/rule +.PHONY : run_tests + +# clean rule for target. +CMakeFiles/run_tests.dir/clean: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean +.PHONY : CMakeFiles/run_tests.dir/clean + +# clean rule for target. +clean: CMakeFiles/run_tests.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/tests.dir + +# All Build rule for target. +CMakeFiles/tests.dir/all: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target tests" +.PHONY : CMakeFiles/tests.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/tests.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/tests.dir/rule + +# Convenience name for target. +tests: CMakeFiles/tests.dir/rule +.PHONY : tests + +# clean rule for target. +CMakeFiles/tests.dir/clean: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean +.PHONY : CMakeFiles/tests.dir/clean + +# clean rule for target. +clean: CMakeFiles/tests.dir/clean +.PHONY : clean + +#============================================================================= +# Directory level rules for directory gtest + +# Convenience name for "all" pass in the directory. +gtest/all: +.PHONY : gtest/all + +# Convenience name for "clean" pass in the directory. +gtest/clean: gtest/CMakeFiles/gtest.dir/clean +gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : gtest/clean + +# Convenience name for "preinstall" pass in the directory. +gtest/preinstall: +.PHONY : gtest/preinstall + +#============================================================================= +# Target rules for target gtest/CMakeFiles/gtest.dir + +# All Build rule for target. +gtest/CMakeFiles/gtest.dir/all: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 1 + @echo "Built target gtest" +.PHONY : gtest/CMakeFiles/gtest.dir/all + +# Build rule for subdir invocation for target. +gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1 + $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : gtest/CMakeFiles/gtest.dir/rule + +# Convenience name for target. +gtest: gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest + +# clean rule for target. +gtest/CMakeFiles/gtest.dir/clean: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean +.PHONY : gtest/CMakeFiles/gtest.dir/clean + +# clean rule for target. +clean: gtest/CMakeFiles/gtest.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target gtest/CMakeFiles/gtest_main.dir + +# All Build rule for target. +gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 2 + @echo "Built target gtest_main" +.PHONY : gtest/CMakeFiles/gtest_main.dir/all + +# Build rule for subdir invocation for target. +gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2 + $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : gtest/CMakeFiles/gtest_main.dir/rule + +# Convenience name for target. +gtest_main: gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest_main + +# clean rule for target. +gtest/CMakeFiles/gtest_main.dir/clean: + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : gtest/CMakeFiles/gtest_main.dir/clean + +# clean rule for target. +clean: gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : clean + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt new file mode 100644 index 0000000000..552e0c540a --- /dev/null +++ b/build/CMakeFiles/TargetDirectories.txt @@ -0,0 +1,6 @@ +/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir +/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir +/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir +/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir +/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir +/home/roman/src/Firmware/build/CMakeFiles/tests.dir diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake new file mode 100644 index 0000000000..7aff3a53c3 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make new file mode 100644 index 0000000000..b3776d0df4 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/build.make @@ -0,0 +1,66 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for clean_test_results. + +# Include the progress variables for this target. +include CMakeFiles/clean_test_results.dir/progress.make + +CMakeFiles/clean_test_results: + /usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results + +clean_test_results: CMakeFiles/clean_test_results +clean_test_results: CMakeFiles/clean_test_results.dir/build.make +.PHONY : clean_test_results + +# Rule to build all files generated by this target. +CMakeFiles/clean_test_results.dir/build: clean_test_results +.PHONY : CMakeFiles/clean_test_results.dir/build + +CMakeFiles/clean_test_results.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake +.PHONY : CMakeFiles/clean_test_results.dir/clean + +CMakeFiles/clean_test_results.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/clean_test_results.dir/depend + diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake new file mode 100644 index 0000000000..46c1cb3380 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/clean_test_results" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache new file mode 100644 index 0000000000..3dccd73172 --- /dev/null +++ b/build/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake new file mode 100644 index 0000000000..7aff3a53c3 --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make new file mode 100644 index 0000000000..f5cc02bb39 --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for doxygen. + +# Include the progress variables for this target. +include CMakeFiles/doxygen.dir/progress.make + +CMakeFiles/doxygen: + +doxygen: CMakeFiles/doxygen +doxygen: CMakeFiles/doxygen.dir/build.make +.PHONY : doxygen + +# Rule to build all files generated by this target. +CMakeFiles/doxygen.dir/build: doxygen +.PHONY : CMakeFiles/doxygen.dir/build + +CMakeFiles/doxygen.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake +.PHONY : CMakeFiles/doxygen.dir/clean + +CMakeFiles/doxygen.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/doxygen.dir/depend + diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake new file mode 100644 index 0000000000..3cf72d90fa --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/doxygen" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks new file mode 100644 index 0000000000..573541ac97 --- /dev/null +++ b/build/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake new file mode 100644 index 0000000000..7aff3a53c3 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make new file mode 100644 index 0000000000..3907c75732 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for run_tests. + +# Include the progress variables for this target. +include CMakeFiles/run_tests.dir/progress.make + +CMakeFiles/run_tests: + +run_tests: CMakeFiles/run_tests +run_tests: CMakeFiles/run_tests.dir/build.make +.PHONY : run_tests + +# Rule to build all files generated by this target. +CMakeFiles/run_tests.dir/build: run_tests +.PHONY : CMakeFiles/run_tests.dir/build + +CMakeFiles/run_tests.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake +.PHONY : CMakeFiles/run_tests.dir/clean + +CMakeFiles/run_tests.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/run_tests.dir/depend + diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake new file mode 100644 index 0000000000..45a3e057b2 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/run_tests" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake new file mode 100644 index 0000000000..7aff3a53c3 --- /dev/null +++ b/build/CMakeFiles/tests.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make new file mode 100644 index 0000000000..720d530001 --- /dev/null +++ b/build/CMakeFiles/tests.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for tests. + +# Include the progress variables for this target. +include CMakeFiles/tests.dir/progress.make + +CMakeFiles/tests: + +tests: CMakeFiles/tests +tests: CMakeFiles/tests.dir/build.make +.PHONY : tests + +# Rule to build all files generated by this target. +CMakeFiles/tests.dir/build: tests +.PHONY : CMakeFiles/tests.dir/build + +CMakeFiles/tests.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake +.PHONY : CMakeFiles/tests.dir/clean + +CMakeFiles/tests.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/tests.dir/depend + diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake new file mode 100644 index 0000000000..a0424cfc7c --- /dev/null +++ b/build/CMakeFiles/tests.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/tests" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/build/CMakeFiles/tests.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake new file mode 100644 index 0000000000..7b55348244 --- /dev/null +++ b/build/CTestTestfile.cmake @@ -0,0 +1,7 @@ +# CMake generated Testfile for +# Source directory: /home/roman/src/Firmware/src +# Build directory: /home/roman/src/Firmware/build +# +# This file includes the relevant testing commands required for +# testing this directory and lists subdirectories to be tested as well. +SUBDIRS(gtest) diff --git a/build/Makefile b/build/Makefile new file mode 100644 index 0000000000..c7d796cc42 --- /dev/null +++ b/build/Makefile @@ -0,0 +1,262 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: install/local +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: install/strip +.PHONY : install/strip/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target test +test: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." + /usr/bin/ctest --force-new-ctest-process $(ARGS) +.PHONY : test + +# Special rule for the target test +test/fast: test +.PHONY : test/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named clean_test_results + +# Build rule for target. +clean_test_results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 clean_test_results +.PHONY : clean_test_results + +# fast build rule for target. +clean_test_results/fast: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build +.PHONY : clean_test_results/fast + +#============================================================================= +# Target rules for targets named doxygen + +# Build rule for target. +doxygen: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 doxygen +.PHONY : doxygen + +# fast build rule for target. +doxygen/fast: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build +.PHONY : doxygen/fast + +#============================================================================= +# Target rules for targets named run_tests + +# Build rule for target. +run_tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 run_tests +.PHONY : run_tests + +# fast build rule for target. +run_tests/fast: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build +.PHONY : run_tests/fast + +#============================================================================= +# Target rules for targets named tests + +# Build rule for target. +tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 tests +.PHONY : tests + +# fast build rule for target. +tests/fast: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build +.PHONY : tests/fast + +#============================================================================= +# Target rules for targets named gtest + +# Build rule for target. +gtest: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 gtest +.PHONY : gtest + +# fast build rule for target. +gtest/fast: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build +.PHONY : gtest/fast + +#============================================================================= +# Target rules for targets named gtest_main + +# Build rule for target. +gtest_main: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 gtest_main +.PHONY : gtest_main + +# fast build rule for target. +gtest_main/fast: + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build +.PHONY : gtest_main/fast + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... clean_test_results" + @echo "... doxygen" + @echo "... edit_cache" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... run_tests" + @echo "... test" + @echo "... tests" + @echo "... gtest" + @echo "... gtest_main" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake new file mode 100644 index 0000000000..1cbfc826b0 --- /dev/null +++ b/build/catkin/catkin_generated/version/package.cmake @@ -0,0 +1,9 @@ +set(_CATKIN_CURRENT_PACKAGE "catkin") +set(catkin_VERSION "0.6.9") +set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") +set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg") +set(catkin_DEPRECATED "") +set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose") +set(catkin_MAINTAINER "Dirk Thomas ") +set(catkin_BUILDTOOL_DEPENDS "cmake") +set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") \ No newline at end of file diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh new file mode 100755 index 0000000000..d6be91db5c --- /dev/null +++ b/build/catkin_generated/env_cached.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup_cached.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup_cached.sh" +exec "$@" diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py new file mode 100644 index 0000000000..7a56cdb7f9 --- /dev/null +++ b/build/catkin_generated/generate_cached_setup.py @@ -0,0 +1,29 @@ +from __future__ import print_function +import argparse +import os +import stat +import sys + +# find the import for catkin's python package - either from source space or from an installed underlay +if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')): + sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python')) +try: + from catkin.environment_cache import generate_environment_script +except ImportError: + # search for catkin package in all workspaces and prepend to path + for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'): + python_path = os.path.join(workspace, 'lib/python2.7/dist-packages') + if os.path.isdir(os.path.join(python_path, 'catkin')): + sys.path.insert(0, python_path) + break + from catkin.environment_cache import generate_environment_script + +code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh') + +output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh' +with open(output_filename, 'w') as f: + #print('Generate script for cached setup "%s"' % output_filename) + f.write('\n'.join(code)) + +mode = os.stat(output_filename).st_mode +os.chmod(output_filename, mode | stat.S_IXUSR) diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall new file mode 100644 index 0000000000..a959c71efc --- /dev/null +++ b/build/catkin_generated/installspace/.rosinstall @@ -0,0 +1,2 @@ +- setup-file: + local-name: /home/roman/src/Firmware/install/setup.sh diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py new file mode 100755 index 0000000000..8db644140c --- /dev/null +++ b/build/catkin_generated/installspace/_setup_util.py @@ -0,0 +1,287 @@ +#!/usr/bin/python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +'''This file generates shell code for the setup.SHELL scripts to set environment variables''' + +from __future__ import print_function +import argparse +import copy +import errno +import os +import platform +import sys + +CATKIN_MARKER_FILE = '.catkin' + +system = platform.system() +IS_DARWIN = (system == 'Darwin') +IS_WINDOWS = (system == 'Windows') + +# subfolder of workspace prepended to CMAKE_PREFIX_PATH +ENV_VAR_SUBFOLDERS = { + 'CMAKE_PREFIX_PATH': '', + 'CPATH': 'include', + 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], + 'PATH': 'bin', + 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], + 'PYTHONPATH': 'lib/python2.7/dist-packages', +} + + +def rollback_env_variables(environ, env_var_subfolders): + ''' + Generate shell code to reset environment variables + by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. + This does not cover modifications performed by environment hooks. + ''' + lines = [] + unmodified_environ = copy.copy(environ) + for key in sorted(env_var_subfolders.keys()): + subfolders = env_var_subfolders[key] + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + value = _rollback_env_variable(unmodified_environ, key, subfolder) + if value is not None: + environ[key] = value + lines.append(assignment(key, value)) + if lines: + lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) + return lines + + +def _rollback_env_variable(environ, name, subfolder): + ''' + For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. + + :param subfolder: str '' or subfoldername that may start with '/' + :returns: the updated value of the environment variable. + ''' + value = environ[name] if name in environ else '' + env_paths = [path for path in value.split(os.pathsep) if path] + value_modified = False + if subfolder: + if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): + subfolder = subfolder[1:] + if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): + subfolder = subfolder[:-1] + for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): + path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path + path_to_remove = None + for env_path in env_paths: + env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path + if env_path_clean == path_to_find: + path_to_remove = env_path + break + if path_to_remove: + env_paths.remove(path_to_remove) + value_modified = True + new_value = os.pathsep.join(env_paths) + return new_value if value_modified else None + + +def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): + ''' + Based on CMAKE_PREFIX_PATH return all catkin workspaces. + + :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` + ''' + # get all cmake prefix paths + env_name = 'CMAKE_PREFIX_PATH' + value = environ[env_name] if env_name in environ else '' + paths = [path for path in value.split(os.pathsep) if path] + # remove non-workspace paths + workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] + return workspaces + + +def prepend_env_variables(environ, env_var_subfolders, workspaces): + ''' + Generate shell code to prepend environment variables + for the all workspaces. + ''' + lines = [] + lines.append(comment('prepend folders of workspaces to environment variables')) + + paths = [path for path in workspaces.split(os.pathsep) if path] + + prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') + lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) + + for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): + subfolder = env_var_subfolders[key] + prefix = _prefix_env_variable(environ, key, paths, subfolder) + lines.append(prepend(environ, key, prefix)) + return lines + + +def _prefix_env_variable(environ, name, paths, subfolders): + ''' + Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. + ''' + value = environ[name] if name in environ else '' + environ_paths = [path for path in value.split(os.pathsep) if path] + checked_paths = [] + for path in paths: + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + path_tmp = path + if subfolder: + path_tmp = os.path.join(path_tmp, subfolder) + # exclude any path already in env and any path we already added + if path_tmp not in environ_paths and path_tmp not in checked_paths: + checked_paths.append(path_tmp) + prefix_str = os.pathsep.join(checked_paths) + if prefix_str != '' and environ_paths: + prefix_str += os.pathsep + return prefix_str + + +def assignment(key, value): + if not IS_WINDOWS: + return 'export %s="%s"' % (key, value) + else: + return 'set %s=%s' % (key, value) + + +def comment(msg): + if not IS_WINDOWS: + return '# %s' % msg + else: + return 'REM %s' % msg + + +def prepend(environ, key, prefix): + if key not in environ or not environ[key]: + return assignment(key, prefix) + if not IS_WINDOWS: + return 'export %s="%s$%s"' % (key, prefix, key) + else: + return 'set %s=%s%%%s%%' % (key, prefix, key) + + +def find_env_hooks(environ, cmake_prefix_path): + ''' + Generate shell code with found environment hooks + for the all workspaces. + ''' + lines = [] + lines.append(comment('found environment hooks in workspaces')) + + generic_env_hooks = [] + generic_env_hooks_workspace = [] + specific_env_hooks = [] + specific_env_hooks_workspace = [] + generic_env_hooks_by_filename = {} + specific_env_hooks_by_filename = {} + generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' + specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None + # remove non-workspace paths + workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] + for workspace in reversed(workspaces): + env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') + if os.path.isdir(env_hook_dir): + for filename in sorted(os.listdir(env_hook_dir)): + if filename.endswith('.%s' % generic_env_hook_ext): + # remove previous env hook with same name if present + if filename in generic_env_hooks_by_filename: + i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) + generic_env_hooks.pop(i) + generic_env_hooks_workspace.pop(i) + # append env hook + generic_env_hooks.append(os.path.join(env_hook_dir, filename)) + generic_env_hooks_workspace.append(workspace) + generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] + elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): + # remove previous env hook with same name if present + if filename in specific_env_hooks_by_filename: + i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) + specific_env_hooks.pop(i) + specific_env_hooks_workspace.pop(i) + # append env hook + specific_env_hooks.append(os.path.join(env_hook_dir, filename)) + specific_env_hooks_workspace.append(workspace) + specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] + env_hooks = generic_env_hooks + specific_env_hooks + env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace + count = len(env_hooks) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) + for i in range(count): + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) + return lines + + +def _parse_arguments(args=None): + parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') + parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') + return parser.parse_known_args(args=args)[0] + + +if __name__ == '__main__': + try: + try: + args = _parse_arguments() + except Exception as e: + print(e, file=sys.stderr) + sys.exit(1) + + # environment at generation time + CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') + # prepend current workspace if not already part of CPP + base_path = os.path.dirname(__file__) + if base_path not in CMAKE_PREFIX_PATH: + CMAKE_PREFIX_PATH.insert(0, base_path) + CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) + + environ = dict(os.environ) + lines = [] + if not args.extend: + lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) + lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) + lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) + print('\n'.join(lines)) + + # need to explicitly flush the output + sys.stdout.flush() + except IOError as e: + # and catch potantial "broken pipe" if stdout is not writable + # which can happen when piping the output to a file but the disk is full + if e.errno == errno.EPIPE: + print(e, file=sys.stderr) + sys.exit(2) + raise + + sys.exit(0) diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh new file mode 100755 index 0000000000..8aa9d244ae --- /dev/null +++ b/build/catkin_generated/installspace/env.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" +exec "$@" diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash new file mode 100644 index 0000000000..ff47af8f30 --- /dev/null +++ b/build/catkin_generated/installspace/setup.bash @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +# generated from catkin/cmake/templates/setup.bash.in + +CATKIN_SHELL=bash + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh new file mode 100644 index 0000000000..95e1571bef --- /dev/null +++ b/build/catkin_generated/installspace/setup.sh @@ -0,0 +1,87 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/template/setup.sh.in + +# Sets various environment variables and sources additional environment hooks. +# It tries it's best to undo changes from a previously sourced setup file before. +# Supported command line options: +# --extend: skips the undoing of changes from a previously sourced setup file + +# since this file is sourced either use the provided _CATKIN_SETUP_DIR +# or fall back to the destination set at configure time +: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install} +_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" +unset _CATKIN_SETUP_DIR + +if [ ! -f "$_SETUP_UTIL" ]; then + echo "Missing Python script: $_SETUP_UTIL" + return 22 +fi + +# detect if running on Darwin platform +_UNAME=`uname -s` +_IS_DARWIN=0 +if [ "$_UNAME" = "Darwin" ]; then + _IS_DARWIN=1 +fi +unset _UNAME + +# make sure to export all environment variables +export CMAKE_PREFIX_PATH +export CPATH +if [ $_IS_DARWIN -eq 0 ]; then + export LD_LIBRARY_PATH +else + export DYLD_LIBRARY_PATH +fi +unset _IS_DARWIN +export PATH +export PKG_CONFIG_PATH +export PYTHONPATH + +# remember type of shell if not already set +if [ -z "$CATKIN_SHELL" ]; then + CATKIN_SHELL=sh +fi + +# invoke Python script to generate necessary exports of environment variables +_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` +if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then + echo "Could not create temporary file: $_SETUP_TMP" + return 1 +fi +CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP +_RC=$? +if [ $_RC -ne 0 ]; then + if [ $_RC -eq 2 ]; then + echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" + else + echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" + fi + unset _RC + unset _SETUP_UTIL + rm -f $_SETUP_TMP + unset _SETUP_TMP + return 1 +fi +unset _RC +unset _SETUP_UTIL +. $_SETUP_TMP +rm -f $_SETUP_TMP +unset _SETUP_TMP + +# source all environment hooks +_i=0 +while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do + eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i + unset _CATKIN_ENVIRONMENT_HOOKS_$_i + eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + # set workspace for environment hook + CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace + . "$_envfile" + unset CATKIN_ENV_HOOK_WORKSPACE + _i=$((_i + 1)) +done +unset _i + +unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh new file mode 100644 index 0000000000..b660717663 --- /dev/null +++ b/build/catkin_generated/installspace/setup.zsh @@ -0,0 +1,8 @@ +#!/usr/bin/env zsh +# generated from catkin/cmake/templates/setup.zsh.in + +CATKIN_SHELL=zsh +_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) +emulate sh # emulate POSIX +. "$_CATKIN_SETUP_DIR/setup.sh" +emulate zsh # back to zsh mode diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake new file mode 100644 index 0000000000..6b0e2dff92 --- /dev/null +++ b/build/catkin_generated/order_packages.cmake @@ -0,0 +1,10 @@ +# generated from catkin/cmake/em/order_packages.cmake.em + +set(CATKIN_ORDERED_PACKAGES "") +set(CATKIN_ORDERED_PACKAGE_PATHS "") +set(CATKIN_ORDERED_PACKAGES_IS_META "") +set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") + +set(CATKIN_MESSAGE_GENERATORS ) + +set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in") diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py new file mode 100644 index 0000000000..5cb1765802 --- /dev/null +++ b/build/catkin_generated/order_packages.py @@ -0,0 +1,5 @@ +# generated from catkin/cmake/template/order_packages.context.py.in +source_root_dir = "/home/roman/src/Firmware/src" +whitelisted_packages = "".split(';') if "" != "" else [] +blacklisted_packages = "".split(';') if "" != "" else [] +underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else [] diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh new file mode 100755 index 0000000000..e03bce4ba5 --- /dev/null +++ b/build/catkin_generated/setup_cached.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env sh +# generated from catkin/python/catkin/environment_cache.py + +# based on a snapshot of the environment before and after calling the setup script +# it emulates the modifications of the setup script without recurring computations + +# new environment variables + +# modified environment variables +export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" +export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH" +export CPATH="/home/roman/src/Firmware/devel/include:$CPATH" +export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib" +export PATH="/home/roman/src/Firmware/devel/bin:$PATH" +export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH" +export PWD="/home/roman/src/Firmware/build" +export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH" +export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES" +export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH" +export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" \ No newline at end of file diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp new file mode 100644 index 0000000000..26882f02aa --- /dev/null +++ b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp @@ -0,0 +1,250 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +from __future__ import print_function +import os +import sys + +import distutils.core +try: + import setuptools +except ImportError: + pass + +from argparse import ArgumentParser + + +def _get_locations(pkgs, package_dir): + """ + based on setuptools logic and the package_dir dict, builds a dict + of location roots for each pkg in pkgs. + See http://docs.python.org/distutils/setupscript.html + + :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents) + """ + # package_dir contains a dict {package_name: relativepath} + # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'} + # + # '' means where to look for any package unless a parent package + # is listed so package bar.pot is expected at lib2/bar/pot, + # whereas package sup.dee is expected at src/sup/dee + # + # if package_dir does not state anything about a package, + # setuptool expects the package folder to be in the root of the + # project + locations = {} + allprefix = package_dir.get('', '') + for pkg in pkgs: + parent_location = None + splits = pkg.split('.') + # we iterate over compound name from parent to child + # so once we found parent, children just append to their parent + for key_len in range(len(splits)): + key = '.'.join(splits[:key_len + 1]) + if key not in locations: + if key in package_dir: + locations[key] = package_dir[key] + elif parent_location is not None: + locations[key] = parent_location + else: + locations[key] = allprefix + parent_location = locations[key] + return locations + + +def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules): + """ + Generates lines to add to a cmake file which will set variables + + :param version: str, format 'int.int.int' + :param scripts: [list of str]: relative paths to scripts + :param package_dir: {modulename: path} + :pkgs: [list of str] python_packages declared in catkin package + :modules: [list of str] python modules + """ + prefix = '%s_SETUP_PY' % package_name + result = [] + result.append(r'set(%s_VERSION "%s")' % (prefix, version)) + result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts))) + + # Remove packages with '.' separators. + # + # setuptools allows specifying submodules in other folders than + # their parent + # + # The symlink approach of catkin does not work with such submodules. + # In the common case, this does not matter as the submodule is + # within the containing module. We verify this assumption, and if + # it passes, we remove submodule packages. + locations = _get_locations(pkgs, package_dir) + for pkgname, location in locations.items(): + if not '.' in pkgname: + continue + splits = pkgname.split('.') + # hack: ignore write-combining setup.py files for msg and srv files + if splits[1] in ['msg', 'srv']: + continue + # check every child has the same root folder as its parent + parent_name = '.'.join(splits[:1]) + if location != locations[parent_name]: + raise RuntimeError( + "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name])) + + # If checks pass, remove all submodules + pkgs = [p for p in pkgs if '.' not in p] + + resolved_pkgs = [] + for pkg in pkgs: + resolved_pkgs += [os.path.join(locations[pkg], pkg)] + + result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs))) + result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/"))) + + # skip modules which collide with package names + filtered_modules = [] + for modname in modules: + splits = modname.split('.') + # check all parents too + equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))] + if any(equals_package): + continue + filtered_modules.append(modname) + module_locations = _get_locations(filtered_modules, package_dir) + + result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules]))) + result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/"))) + + return result + + +def _create_mock_setup_function(package_name, outfile): + """ + Creates a function to call instead of distutils.core.setup or + setuptools.setup, which just captures some args and writes them + into a file that can be used from cmake + + :param package_name: name of the package + :param outfile: filename that cmake will use afterwards + :returns: a function to replace disutils.core.setup and setuptools.setup + """ + + def setup(*args, **kwargs): + ''' + Checks kwargs and writes a scriptfile + ''' + if 'version' not in kwargs: + sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name) + raise RuntimeError("version not found in setup.py") + version = kwargs['version'] + package_dir = kwargs.get('package_dir', {}) + + pkgs = kwargs.get('packages', []) + scripts = kwargs.get('scripts', []) + modules = kwargs.get('py_modules', []) + + unsupported_args = [ + 'entry_points', + 'exclude_package_data', + 'ext_modules ', + 'ext_package', + 'include_package_data', + 'namespace_packages', + 'setup_requires', + 'use_2to3', + 'zip_safe'] + used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs] + if used_unsupported_args: + sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name)) + + result = generate_cmake_file(package_name=package_name, + version=version, + scripts=scripts, + package_dir=package_dir, + pkgs=pkgs, + modules=modules) + with open(outfile, 'w') as out: + out.write('\n'.join(result)) + + return setup + + +def main(): + """ + Script main, parses arguments and invokes Dummy.setup indirectly. + """ + parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.') + parser.add_argument('package_name', help='Name of catkin package') + parser.add_argument('setupfile_path', help='Full path to setup.py') + parser.add_argument('outfile', help='Where to write result to') + + args = parser.parse_args() + + # print("%s" % sys.argv) + # PACKAGE_NAME = sys.argv[1] + # OUTFILE = sys.argv[3] + # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE), + # file=sys.stderr) + + # print("executing %s" % args.setupfile_path) + + # be sure you're in the directory containing + # setup.py so the sys.path manipulation works, + # so the import of __version__ works + os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path))) + + # patch setup() function of distutils and setuptools for the + # context of evaluating setup.py + try: + fake_setup = _create_mock_setup_function(package_name=args.package_name, + outfile=args.outfile) + + distutils_backup = distutils.core.setup + distutils.core.setup = fake_setup + try: + setuptools_backup = setuptools.setup + setuptools.setup = fake_setup + except NameError: + pass + + with open(args.setupfile_path, 'r') as fh: + exec(fh.read()) + finally: + distutils.core.setup = distutils_backup + try: + setuptools.setup = setuptools_backup + except NameError: + pass + +if __name__ == '__main__': + main() diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp new file mode 100644 index 0000000000..087d4d802e --- /dev/null +++ b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp @@ -0,0 +1,56 @@ +# generated from catkin/cmake/em/order_packages.cmake.em +@{ +import os +try: + from catkin_pkg.cmake import get_metapackage_cmake_template_path +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +try: + from catkin_pkg.topological_order import topological_order +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +try: + from catkin_pkg.package import InvalidPackage +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +# vars defined in order_packages.context.py.in +try: + ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces) +except InvalidPackage as e: + print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"')) + ordered_packages = [] +fatal_error = False +}@ + +set(CATKIN_ORDERED_PACKAGES "") +set(CATKIN_ORDERED_PACKAGE_PATHS "") +set(CATKIN_ORDERED_PACKAGES_IS_META "") +set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") +@[for path, package in ordered_packages]@ +@[if path is None]@ +message(FATAL_ERROR "Circular dependency in subset of packages:\n@package") +@{ +fatal_error = True +}@ +@[elif package.name != 'catkin']@ +list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)") +list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))") +list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))") +list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')") +@{ +deprecated = [e for e in package.exports if e.tagname == 'deprecated'] +}@ +@[if deprecated]@ +message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')") +@[end if]@ +@[end if]@ +@[end for]@ + +@[if not fatal_error]@ +@{ +message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]] +}@ +set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators))) +@[end if]@ + +set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))") diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp new file mode 100644 index 0000000000..7c2708ac3e --- /dev/null +++ b/build/catkin_generated/stamps/Project/package.xml.stamp @@ -0,0 +1,37 @@ + + + catkin + 0.6.9 + Low-level build system macros and infrastructure for ROS. + Dirk Thomas + BSD + + http://www.ros.org/wiki/catkin + https://github.com/ros/catkin/issues + https://github.com/ros/catkin + + Troy Straszheim + Morten Kjaergaard + Brian Gerkey + Dirk Thomas + + cmake + cmake + + python-argparse + python-catkin-pkg + + python-empy + + gtest + python-empy + python-nose + + python-mock + python-nose + + + + + + diff --git a/build/catkin_make.cache b/build/catkin_make.cache new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/build/catkin_make.cache @@ -0,0 +1 @@ + diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake new file mode 100644 index 0000000000..982fd705c6 --- /dev/null +++ b/build/cmake_install.cmake @@ -0,0 +1,140 @@ +# Install script for directory: /home/roman/src/Firmware/src + +# Set the install prefix +IF(NOT DEFINED CMAKE_INSTALL_PREFIX) + SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") +ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) +STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + IF(BUILD_TYPE) + STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + ELSE(BUILD_TYPE) + SET(CMAKE_INSTALL_CONFIG_NAME "") + ENDIF(BUILD_TYPE) + MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + +# Set the component getting installed. +IF(NOT CMAKE_INSTALL_COMPONENT) + IF(COMPONENT) + MESSAGE(STATUS "Install component: \"${COMPONENT}\"") + SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + ELSE(COMPONENT) + SET(CMAKE_INSTALL_COMPONENT) + ENDIF(COMPONENT) +ENDIF(NOT CMAKE_INSTALL_COMPONENT) + +# Install shared libraries without execute permission? +IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + SET(CMAKE_INSTALL_SO_NO_EXE "1") +ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + + if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") + file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") + endif() + if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin") + file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "") + endif() +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/_setup_util.py") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/env.sh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.bash") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.sh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.zsh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/.rosinstall") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_LOCAL_ONLY) + # Include the install script for each subdirectory. + INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake") + +ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY) + +IF(CMAKE_INSTALL_COMPONENT) + SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") +ELSE(CMAKE_INSTALL_COMPONENT) + SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") +ENDIF(CMAKE_INSTALL_COMPONENT) + +FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "") +FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) + FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n") +ENDFOREACH(file) diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 0000000000..06b7c63c13 --- /dev/null +++ b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake @@ -0,0 +1,16 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Relative path conversion top directories. +SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest") +SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") + +# Force unix paths in dependencies. +SET(CMAKE_FORCE_UNIX_PATHS 1) + + +# The C and CXX include file regular expressions for this directory. +SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") +SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") +SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) +SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake new file mode 100644 index 0000000000..40234804c6 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake @@ -0,0 +1,27 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + "CXX" + ) +# The set of files for implicit dependencies of each language: +SET(CMAKE_DEPENDS_CHECK_CXX + "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" + ) +SET(CMAKE_CXX_COMPILER_ID "GNU") + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "GTEST_CREATE_SHARED_LIBRARY=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + "/usr/src/gtest/include" + "/usr/src/gtest" + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make new file mode 100644 index 0000000000..744feec6d6 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/build.make @@ -0,0 +1,102 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Include any dependencies generated for this target. +include gtest/CMakeFiles/gtest.dir/depend.make + +# Include the progress variables for this target. +include gtest/CMakeFiles/gtest.dir/progress.make + +# Include the compile flags for this target's objects. +include gtest/CMakeFiles/gtest.dir/flags.make + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires: +.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build +.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o + +# Object files for target gtest +gtest_OBJECTS = \ +"CMakeFiles/gtest.dir/src/gtest-all.cc.o" + +# External object files for target gtest +gtest_EXTERNAL_OBJECTS = + +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so" + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE) + +# Rule to build all files generated by this target. +gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so +.PHONY : gtest/CMakeFiles/gtest.dir/build + +gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires +.PHONY : gtest/CMakeFiles/gtest.dir/requires + +gtest/CMakeFiles/gtest.dir/clean: + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake +.PHONY : gtest/CMakeFiles/gtest.dir/clean + +gtest/CMakeFiles/gtest.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : gtest/CMakeFiles/gtest.dir/depend + diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake new file mode 100644 index 0000000000..015a1ee758 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake @@ -0,0 +1,10 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/gtest.dir/src/gtest-all.cc.o" + "libgtest.pdb" + "libgtest.so" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang CXX) + INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make new file mode 100644 index 0000000000..37ac348dbd --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/depend.make @@ -0,0 +1,2 @@ +# Empty dependencies file for gtest. +# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make new file mode 100644 index 0000000000..fa55300acd --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/flags.make @@ -0,0 +1,8 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# compile CXX with /usr/bin/c++ +CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra + +CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS + diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt new file mode 100644 index 0000000000..8745cedd2c --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/link.txt @@ -0,0 +1 @@ +/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make new file mode 100644 index 0000000000..781c7de277 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/progress.make @@ -0,0 +1,2 @@ +CMAKE_PROGRESS_1 = 1 + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake new file mode 100644 index 0000000000..9f3beee6e1 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake @@ -0,0 +1,28 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + "CXX" + ) +# The set of files for implicit dependencies of each language: +SET(CMAKE_DEPENDS_CHECK_CXX + "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + ) +SET(CMAKE_CXX_COMPILER_ID "GNU") + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "GTEST_CREATE_SHARED_LIBRARY=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake" + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + "/usr/src/gtest/include" + "/usr/src/gtest" + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make new file mode 100644 index 0000000000..67a7ec0180 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/build.make @@ -0,0 +1,103 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Include any dependencies generated for this target. +include gtest/CMakeFiles/gtest_main.dir/depend.make + +# Include the progress variables for this target. +include gtest/CMakeFiles/gtest_main.dir/progress.make + +# Include the compile flags for this target's objects. +include gtest/CMakeFiles/gtest_main.dir/flags.make + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires: +.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build +.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o + +# Object files for target gtest_main +gtest_main_OBJECTS = \ +"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + +# External object files for target gtest_main +gtest_main_EXTERNAL_OBJECTS = + +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make +gtest/libgtest_main.so: gtest/libgtest.so +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so" + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE) + +# Rule to build all files generated by this target. +gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so +.PHONY : gtest/CMakeFiles/gtest_main.dir/build + +gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires +.PHONY : gtest/CMakeFiles/gtest_main.dir/requires + +gtest/CMakeFiles/gtest_main.dir/clean: + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake +.PHONY : gtest/CMakeFiles/gtest_main.dir/clean + +gtest/CMakeFiles/gtest_main.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : gtest/CMakeFiles/gtest_main.dir/depend + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake new file mode 100644 index 0000000000..c8fe838192 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake @@ -0,0 +1,10 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + "libgtest_main.pdb" + "libgtest_main.so" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang CXX) + INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make new file mode 100644 index 0000000000..1d67c1ab52 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/depend.make @@ -0,0 +1,2 @@ +# Empty dependencies file for gtest_main. +# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make new file mode 100644 index 0000000000..93ab70f12f --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/flags.make @@ -0,0 +1,8 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# compile CXX with /usr/bin/c++ +CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra + +CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt new file mode 100644 index 0000000000..d35b7f61ac --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/link.txt @@ -0,0 +1 @@ +/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make new file mode 100644 index 0000000000..164e1d26ca --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/progress.make @@ -0,0 +1,2 @@ +CMAKE_PROGRESS_1 = 2 + diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks new file mode 100644 index 0000000000..573541ac97 --- /dev/null +++ b/build/gtest/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake new file mode 100644 index 0000000000..bd4b57a8bf --- /dev/null +++ b/build/gtest/CTestTestfile.cmake @@ -0,0 +1,6 @@ +# CMake generated Testfile for +# Source directory: /usr/src/gtest +# Build directory: /home/roman/src/Firmware/build/gtest +# +# This file includes the relevant testing commands required for +# testing this directory and lists subdirectories to be tested as well. diff --git a/build/gtest/Makefile b/build/gtest/Makefile new file mode 100644 index 0000000000..bc8c23a004 --- /dev/null +++ b/build/gtest/Makefile @@ -0,0 +1,262 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: install/local +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: install/strip +.PHONY : install/strip/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target test +test: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." + /usr/bin/ctest --force-new-ctest-process $(ARGS) +.PHONY : test + +# Special rule for the target test +test/fast: test +.PHONY : test/fast + +# The main all target +all: cmake_check_build_system + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Convenience name for target. +gtest/CMakeFiles/gtest.dir/rule: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest/CMakeFiles/gtest.dir/rule + +# Convenience name for target. +gtest: gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest + +# fast build rule for target. +gtest/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build +.PHONY : gtest/fast + +# Convenience name for target. +gtest/CMakeFiles/gtest_main.dir/rule: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest/CMakeFiles/gtest_main.dir/rule + +# Convenience name for target. +gtest_main: gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest_main + +# fast build rule for target. +gtest_main/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build +.PHONY : gtest_main/fast + +src/gtest-all.o: src/gtest-all.cc.o +.PHONY : src/gtest-all.o + +# target to build an object file +src/gtest-all.cc.o: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o +.PHONY : src/gtest-all.cc.o + +src/gtest-all.i: src/gtest-all.cc.i +.PHONY : src/gtest-all.i + +# target to preprocess a source file +src/gtest-all.cc.i: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i +.PHONY : src/gtest-all.cc.i + +src/gtest-all.s: src/gtest-all.cc.s +.PHONY : src/gtest-all.s + +# target to generate assembly for a file +src/gtest-all.cc.s: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s +.PHONY : src/gtest-all.cc.s + +src/gtest_main.o: src/gtest_main.cc.o +.PHONY : src/gtest_main.o + +# target to build an object file +src/gtest_main.cc.o: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o +.PHONY : src/gtest_main.cc.o + +src/gtest_main.i: src/gtest_main.cc.i +.PHONY : src/gtest_main.i + +# target to preprocess a source file +src/gtest_main.cc.i: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i +.PHONY : src/gtest_main.cc.i + +src/gtest_main.s: src/gtest_main.cc.s +.PHONY : src/gtest_main.s + +# target to generate assembly for a file +src/gtest_main.cc.s: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s +.PHONY : src/gtest_main.cc.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... edit_cache" + @echo "... gtest" + @echo "... gtest_main" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... test" + @echo "... src/gtest-all.o" + @echo "... src/gtest-all.i" + @echo "... src/gtest-all.s" + @echo "... src/gtest_main.o" + @echo "... src/gtest_main.i" + @echo "... src/gtest_main.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake new file mode 100644 index 0000000000..f51b656514 --- /dev/null +++ b/build/gtest/cmake_install.cmake @@ -0,0 +1,34 @@ +# Install script for directory: /usr/src/gtest + +# Set the install prefix +IF(NOT DEFINED CMAKE_INSTALL_PREFIX) + SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") +ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) +STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + IF(BUILD_TYPE) + STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + ELSE(BUILD_TYPE) + SET(CMAKE_INSTALL_CONFIG_NAME "") + ENDIF(BUILD_TYPE) + MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + +# Set the component getting installed. +IF(NOT CMAKE_INSTALL_COMPONENT) + IF(COMPONENT) + MESSAGE(STATUS "Install component: \"${COMPONENT}\"") + SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + ELSE(COMPONENT) + SET(CMAKE_INSTALL_COMPONENT) + ENDIF(COMPONENT) +ENDIF(NOT CMAKE_INSTALL_COMPONENT) + +# Install shared libraries without execute permission? +IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + SET(CMAKE_INSTALL_SO_NO_EXE "1") +ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + diff --git a/devel/.catkin b/devel/.catkin new file mode 100644 index 0000000000..50d43da7e7 --- /dev/null +++ b/devel/.catkin @@ -0,0 +1 @@ +/home/roman/src/Firmware/src \ No newline at end of file diff --git a/devel/.rosinstall b/devel/.rosinstall new file mode 100644 index 0000000000..fda964e6cf --- /dev/null +++ b/devel/.rosinstall @@ -0,0 +1,2 @@ +- setup-file: + local-name: /home/roman/src/Firmware/devel/setup.sh diff --git a/devel/_setup_util.py b/devel/_setup_util.py new file mode 100755 index 0000000000..8db644140c --- /dev/null +++ b/devel/_setup_util.py @@ -0,0 +1,287 @@ +#!/usr/bin/python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +'''This file generates shell code for the setup.SHELL scripts to set environment variables''' + +from __future__ import print_function +import argparse +import copy +import errno +import os +import platform +import sys + +CATKIN_MARKER_FILE = '.catkin' + +system = platform.system() +IS_DARWIN = (system == 'Darwin') +IS_WINDOWS = (system == 'Windows') + +# subfolder of workspace prepended to CMAKE_PREFIX_PATH +ENV_VAR_SUBFOLDERS = { + 'CMAKE_PREFIX_PATH': '', + 'CPATH': 'include', + 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], + 'PATH': 'bin', + 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], + 'PYTHONPATH': 'lib/python2.7/dist-packages', +} + + +def rollback_env_variables(environ, env_var_subfolders): + ''' + Generate shell code to reset environment variables + by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. + This does not cover modifications performed by environment hooks. + ''' + lines = [] + unmodified_environ = copy.copy(environ) + for key in sorted(env_var_subfolders.keys()): + subfolders = env_var_subfolders[key] + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + value = _rollback_env_variable(unmodified_environ, key, subfolder) + if value is not None: + environ[key] = value + lines.append(assignment(key, value)) + if lines: + lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) + return lines + + +def _rollback_env_variable(environ, name, subfolder): + ''' + For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. + + :param subfolder: str '' or subfoldername that may start with '/' + :returns: the updated value of the environment variable. + ''' + value = environ[name] if name in environ else '' + env_paths = [path for path in value.split(os.pathsep) if path] + value_modified = False + if subfolder: + if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): + subfolder = subfolder[1:] + if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): + subfolder = subfolder[:-1] + for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): + path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path + path_to_remove = None + for env_path in env_paths: + env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path + if env_path_clean == path_to_find: + path_to_remove = env_path + break + if path_to_remove: + env_paths.remove(path_to_remove) + value_modified = True + new_value = os.pathsep.join(env_paths) + return new_value if value_modified else None + + +def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): + ''' + Based on CMAKE_PREFIX_PATH return all catkin workspaces. + + :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` + ''' + # get all cmake prefix paths + env_name = 'CMAKE_PREFIX_PATH' + value = environ[env_name] if env_name in environ else '' + paths = [path for path in value.split(os.pathsep) if path] + # remove non-workspace paths + workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] + return workspaces + + +def prepend_env_variables(environ, env_var_subfolders, workspaces): + ''' + Generate shell code to prepend environment variables + for the all workspaces. + ''' + lines = [] + lines.append(comment('prepend folders of workspaces to environment variables')) + + paths = [path for path in workspaces.split(os.pathsep) if path] + + prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') + lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) + + for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): + subfolder = env_var_subfolders[key] + prefix = _prefix_env_variable(environ, key, paths, subfolder) + lines.append(prepend(environ, key, prefix)) + return lines + + +def _prefix_env_variable(environ, name, paths, subfolders): + ''' + Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. + ''' + value = environ[name] if name in environ else '' + environ_paths = [path for path in value.split(os.pathsep) if path] + checked_paths = [] + for path in paths: + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + path_tmp = path + if subfolder: + path_tmp = os.path.join(path_tmp, subfolder) + # exclude any path already in env and any path we already added + if path_tmp not in environ_paths and path_tmp not in checked_paths: + checked_paths.append(path_tmp) + prefix_str = os.pathsep.join(checked_paths) + if prefix_str != '' and environ_paths: + prefix_str += os.pathsep + return prefix_str + + +def assignment(key, value): + if not IS_WINDOWS: + return 'export %s="%s"' % (key, value) + else: + return 'set %s=%s' % (key, value) + + +def comment(msg): + if not IS_WINDOWS: + return '# %s' % msg + else: + return 'REM %s' % msg + + +def prepend(environ, key, prefix): + if key not in environ or not environ[key]: + return assignment(key, prefix) + if not IS_WINDOWS: + return 'export %s="%s$%s"' % (key, prefix, key) + else: + return 'set %s=%s%%%s%%' % (key, prefix, key) + + +def find_env_hooks(environ, cmake_prefix_path): + ''' + Generate shell code with found environment hooks + for the all workspaces. + ''' + lines = [] + lines.append(comment('found environment hooks in workspaces')) + + generic_env_hooks = [] + generic_env_hooks_workspace = [] + specific_env_hooks = [] + specific_env_hooks_workspace = [] + generic_env_hooks_by_filename = {} + specific_env_hooks_by_filename = {} + generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' + specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None + # remove non-workspace paths + workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] + for workspace in reversed(workspaces): + env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') + if os.path.isdir(env_hook_dir): + for filename in sorted(os.listdir(env_hook_dir)): + if filename.endswith('.%s' % generic_env_hook_ext): + # remove previous env hook with same name if present + if filename in generic_env_hooks_by_filename: + i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) + generic_env_hooks.pop(i) + generic_env_hooks_workspace.pop(i) + # append env hook + generic_env_hooks.append(os.path.join(env_hook_dir, filename)) + generic_env_hooks_workspace.append(workspace) + generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] + elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): + # remove previous env hook with same name if present + if filename in specific_env_hooks_by_filename: + i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) + specific_env_hooks.pop(i) + specific_env_hooks_workspace.pop(i) + # append env hook + specific_env_hooks.append(os.path.join(env_hook_dir, filename)) + specific_env_hooks_workspace.append(workspace) + specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] + env_hooks = generic_env_hooks + specific_env_hooks + env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace + count = len(env_hooks) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) + for i in range(count): + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) + return lines + + +def _parse_arguments(args=None): + parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') + parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') + return parser.parse_known_args(args=args)[0] + + +if __name__ == '__main__': + try: + try: + args = _parse_arguments() + except Exception as e: + print(e, file=sys.stderr) + sys.exit(1) + + # environment at generation time + CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') + # prepend current workspace if not already part of CPP + base_path = os.path.dirname(__file__) + if base_path not in CMAKE_PREFIX_PATH: + CMAKE_PREFIX_PATH.insert(0, base_path) + CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) + + environ = dict(os.environ) + lines = [] + if not args.extend: + lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) + lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) + lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) + print('\n'.join(lines)) + + # need to explicitly flush the output + sys.stdout.flush() + except IOError as e: + # and catch potantial "broken pipe" if stdout is not writable + # which can happen when piping the output to a file but the disk is full + if e.errno == errno.EPIPE: + print(e, file=sys.stderr) + sys.exit(2) + raise + + sys.exit(0) diff --git a/devel/env.sh b/devel/env.sh new file mode 100755 index 0000000000..8aa9d244ae --- /dev/null +++ b/devel/env.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" +exec "$@" diff --git a/devel/setup.bash b/devel/setup.bash new file mode 100644 index 0000000000..ff47af8f30 --- /dev/null +++ b/devel/setup.bash @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +# generated from catkin/cmake/templates/setup.bash.in + +CATKIN_SHELL=bash + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/devel/setup.sh b/devel/setup.sh new file mode 100644 index 0000000000..7d80d638a6 --- /dev/null +++ b/devel/setup.sh @@ -0,0 +1,87 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/template/setup.sh.in + +# Sets various environment variables and sources additional environment hooks. +# It tries it's best to undo changes from a previously sourced setup file before. +# Supported command line options: +# --extend: skips the undoing of changes from a previously sourced setup file + +# since this file is sourced either use the provided _CATKIN_SETUP_DIR +# or fall back to the destination set at configure time +: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel} +_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" +unset _CATKIN_SETUP_DIR + +if [ ! -f "$_SETUP_UTIL" ]; then + echo "Missing Python script: $_SETUP_UTIL" + return 22 +fi + +# detect if running on Darwin platform +_UNAME=`uname -s` +_IS_DARWIN=0 +if [ "$_UNAME" = "Darwin" ]; then + _IS_DARWIN=1 +fi +unset _UNAME + +# make sure to export all environment variables +export CMAKE_PREFIX_PATH +export CPATH +if [ $_IS_DARWIN -eq 0 ]; then + export LD_LIBRARY_PATH +else + export DYLD_LIBRARY_PATH +fi +unset _IS_DARWIN +export PATH +export PKG_CONFIG_PATH +export PYTHONPATH + +# remember type of shell if not already set +if [ -z "$CATKIN_SHELL" ]; then + CATKIN_SHELL=sh +fi + +# invoke Python script to generate necessary exports of environment variables +_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` +if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then + echo "Could not create temporary file: $_SETUP_TMP" + return 1 +fi +CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP +_RC=$? +if [ $_RC -ne 0 ]; then + if [ $_RC -eq 2 ]; then + echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" + else + echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" + fi + unset _RC + unset _SETUP_UTIL + rm -f $_SETUP_TMP + unset _SETUP_TMP + return 1 +fi +unset _RC +unset _SETUP_UTIL +. $_SETUP_TMP +rm -f $_SETUP_TMP +unset _SETUP_TMP + +# source all environment hooks +_i=0 +while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do + eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i + unset _CATKIN_ENVIRONMENT_HOOKS_$_i + eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + # set workspace for environment hook + CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace + . "$_envfile" + unset CATKIN_ENV_HOOK_WORKSPACE + _i=$((_i + 1)) +done +unset _i + +unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/devel/setup.zsh b/devel/setup.zsh new file mode 100644 index 0000000000..b660717663 --- /dev/null +++ b/devel/setup.zsh @@ -0,0 +1,8 @@ +#!/usr/bin/env zsh +# generated from catkin/cmake/templates/setup.zsh.in + +CATKIN_SHELL=zsh +_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) +emulate sh # emulate POSIX +. "$_CATKIN_SETUP_DIR/setup.sh" +emulate zsh # back to zsh mode diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 2423e47b4f..4d7487c2bc 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 +Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 diff --git a/uavcan b/uavcan index c7872def16..6980ee8240 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 +Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 From 5ecb2262d1578d4af4e8e5da74df85ab9c9fc19d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 11:05:26 +0200 Subject: [PATCH 008/122] removed devel --- devel/.catkin | 1 - devel/.rosinstall | 2 - devel/_setup_util.py | 287 ------------------------------------------- devel/env.sh | 16 --- devel/setup.bash | 8 -- devel/setup.sh | 87 ------------- devel/setup.zsh | 8 -- 7 files changed, 409 deletions(-) delete mode 100644 devel/.catkin delete mode 100644 devel/.rosinstall delete mode 100755 devel/_setup_util.py delete mode 100755 devel/env.sh delete mode 100644 devel/setup.bash delete mode 100644 devel/setup.sh delete mode 100644 devel/setup.zsh diff --git a/devel/.catkin b/devel/.catkin deleted file mode 100644 index 50d43da7e7..0000000000 --- a/devel/.catkin +++ /dev/null @@ -1 +0,0 @@ -/home/roman/src/Firmware/src \ No newline at end of file diff --git a/devel/.rosinstall b/devel/.rosinstall deleted file mode 100644 index fda964e6cf..0000000000 --- a/devel/.rosinstall +++ /dev/null @@ -1,2 +0,0 @@ -- setup-file: - local-name: /home/roman/src/Firmware/devel/setup.sh diff --git a/devel/_setup_util.py b/devel/_setup_util.py deleted file mode 100755 index 8db644140c..0000000000 --- a/devel/_setup_util.py +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -'''This file generates shell code for the setup.SHELL scripts to set environment variables''' - -from __future__ import print_function -import argparse -import copy -import errno -import os -import platform -import sys - -CATKIN_MARKER_FILE = '.catkin' - -system = platform.system() -IS_DARWIN = (system == 'Darwin') -IS_WINDOWS = (system == 'Windows') - -# subfolder of workspace prepended to CMAKE_PREFIX_PATH -ENV_VAR_SUBFOLDERS = { - 'CMAKE_PREFIX_PATH': '', - 'CPATH': 'include', - 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], - 'PATH': 'bin', - 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], - 'PYTHONPATH': 'lib/python2.7/dist-packages', -} - - -def rollback_env_variables(environ, env_var_subfolders): - ''' - Generate shell code to reset environment variables - by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. - This does not cover modifications performed by environment hooks. - ''' - lines = [] - unmodified_environ = copy.copy(environ) - for key in sorted(env_var_subfolders.keys()): - subfolders = env_var_subfolders[key] - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - value = _rollback_env_variable(unmodified_environ, key, subfolder) - if value is not None: - environ[key] = value - lines.append(assignment(key, value)) - if lines: - lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) - return lines - - -def _rollback_env_variable(environ, name, subfolder): - ''' - For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. - - :param subfolder: str '' or subfoldername that may start with '/' - :returns: the updated value of the environment variable. - ''' - value = environ[name] if name in environ else '' - env_paths = [path for path in value.split(os.pathsep) if path] - value_modified = False - if subfolder: - if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): - subfolder = subfolder[1:] - if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): - subfolder = subfolder[:-1] - for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): - path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path - path_to_remove = None - for env_path in env_paths: - env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path - if env_path_clean == path_to_find: - path_to_remove = env_path - break - if path_to_remove: - env_paths.remove(path_to_remove) - value_modified = True - new_value = os.pathsep.join(env_paths) - return new_value if value_modified else None - - -def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): - ''' - Based on CMAKE_PREFIX_PATH return all catkin workspaces. - - :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` - ''' - # get all cmake prefix paths - env_name = 'CMAKE_PREFIX_PATH' - value = environ[env_name] if env_name in environ else '' - paths = [path for path in value.split(os.pathsep) if path] - # remove non-workspace paths - workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] - return workspaces - - -def prepend_env_variables(environ, env_var_subfolders, workspaces): - ''' - Generate shell code to prepend environment variables - for the all workspaces. - ''' - lines = [] - lines.append(comment('prepend folders of workspaces to environment variables')) - - paths = [path for path in workspaces.split(os.pathsep) if path] - - prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') - lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) - - for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): - subfolder = env_var_subfolders[key] - prefix = _prefix_env_variable(environ, key, paths, subfolder) - lines.append(prepend(environ, key, prefix)) - return lines - - -def _prefix_env_variable(environ, name, paths, subfolders): - ''' - Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. - ''' - value = environ[name] if name in environ else '' - environ_paths = [path for path in value.split(os.pathsep) if path] - checked_paths = [] - for path in paths: - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - path_tmp = path - if subfolder: - path_tmp = os.path.join(path_tmp, subfolder) - # exclude any path already in env and any path we already added - if path_tmp not in environ_paths and path_tmp not in checked_paths: - checked_paths.append(path_tmp) - prefix_str = os.pathsep.join(checked_paths) - if prefix_str != '' and environ_paths: - prefix_str += os.pathsep - return prefix_str - - -def assignment(key, value): - if not IS_WINDOWS: - return 'export %s="%s"' % (key, value) - else: - return 'set %s=%s' % (key, value) - - -def comment(msg): - if not IS_WINDOWS: - return '# %s' % msg - else: - return 'REM %s' % msg - - -def prepend(environ, key, prefix): - if key not in environ or not environ[key]: - return assignment(key, prefix) - if not IS_WINDOWS: - return 'export %s="%s$%s"' % (key, prefix, key) - else: - return 'set %s=%s%%%s%%' % (key, prefix, key) - - -def find_env_hooks(environ, cmake_prefix_path): - ''' - Generate shell code with found environment hooks - for the all workspaces. - ''' - lines = [] - lines.append(comment('found environment hooks in workspaces')) - - generic_env_hooks = [] - generic_env_hooks_workspace = [] - specific_env_hooks = [] - specific_env_hooks_workspace = [] - generic_env_hooks_by_filename = {} - specific_env_hooks_by_filename = {} - generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' - specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None - # remove non-workspace paths - workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] - for workspace in reversed(workspaces): - env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') - if os.path.isdir(env_hook_dir): - for filename in sorted(os.listdir(env_hook_dir)): - if filename.endswith('.%s' % generic_env_hook_ext): - # remove previous env hook with same name if present - if filename in generic_env_hooks_by_filename: - i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) - generic_env_hooks.pop(i) - generic_env_hooks_workspace.pop(i) - # append env hook - generic_env_hooks.append(os.path.join(env_hook_dir, filename)) - generic_env_hooks_workspace.append(workspace) - generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] - elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): - # remove previous env hook with same name if present - if filename in specific_env_hooks_by_filename: - i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) - specific_env_hooks.pop(i) - specific_env_hooks_workspace.pop(i) - # append env hook - specific_env_hooks.append(os.path.join(env_hook_dir, filename)) - specific_env_hooks_workspace.append(workspace) - specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] - env_hooks = generic_env_hooks + specific_env_hooks - env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace - count = len(env_hooks) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) - for i in range(count): - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) - return lines - - -def _parse_arguments(args=None): - parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') - parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') - return parser.parse_known_args(args=args)[0] - - -if __name__ == '__main__': - try: - try: - args = _parse_arguments() - except Exception as e: - print(e, file=sys.stderr) - sys.exit(1) - - # environment at generation time - CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') - # prepend current workspace if not already part of CPP - base_path = os.path.dirname(__file__) - if base_path not in CMAKE_PREFIX_PATH: - CMAKE_PREFIX_PATH.insert(0, base_path) - CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) - - environ = dict(os.environ) - lines = [] - if not args.extend: - lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) - lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) - lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) - print('\n'.join(lines)) - - # need to explicitly flush the output - sys.stdout.flush() - except IOError as e: - # and catch potantial "broken pipe" if stdout is not writable - # which can happen when piping the output to a file but the disk is full - if e.errno == errno.EPIPE: - print(e, file=sys.stderr) - sys.exit(2) - raise - - sys.exit(0) diff --git a/devel/env.sh b/devel/env.sh deleted file mode 100755 index 8aa9d244ae..0000000000 --- a/devel/env.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" -exec "$@" diff --git a/devel/setup.bash b/devel/setup.bash deleted file mode 100644 index ff47af8f30..0000000000 --- a/devel/setup.bash +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/setup.bash.in - -CATKIN_SHELL=bash - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/devel/setup.sh b/devel/setup.sh deleted file mode 100644 index 7d80d638a6..0000000000 --- a/devel/setup.sh +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/template/setup.sh.in - -# Sets various environment variables and sources additional environment hooks. -# It tries it's best to undo changes from a previously sourced setup file before. -# Supported command line options: -# --extend: skips the undoing of changes from a previously sourced setup file - -# since this file is sourced either use the provided _CATKIN_SETUP_DIR -# or fall back to the destination set at configure time -: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel} -_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" -unset _CATKIN_SETUP_DIR - -if [ ! -f "$_SETUP_UTIL" ]; then - echo "Missing Python script: $_SETUP_UTIL" - return 22 -fi - -# detect if running on Darwin platform -_UNAME=`uname -s` -_IS_DARWIN=0 -if [ "$_UNAME" = "Darwin" ]; then - _IS_DARWIN=1 -fi -unset _UNAME - -# make sure to export all environment variables -export CMAKE_PREFIX_PATH -export CPATH -if [ $_IS_DARWIN -eq 0 ]; then - export LD_LIBRARY_PATH -else - export DYLD_LIBRARY_PATH -fi -unset _IS_DARWIN -export PATH -export PKG_CONFIG_PATH -export PYTHONPATH - -# remember type of shell if not already set -if [ -z "$CATKIN_SHELL" ]; then - CATKIN_SHELL=sh -fi - -# invoke Python script to generate necessary exports of environment variables -_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` -if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then - echo "Could not create temporary file: $_SETUP_TMP" - return 1 -fi -CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP -_RC=$? -if [ $_RC -ne 0 ]; then - if [ $_RC -eq 2 ]; then - echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" - else - echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" - fi - unset _RC - unset _SETUP_UTIL - rm -f $_SETUP_TMP - unset _SETUP_TMP - return 1 -fi -unset _RC -unset _SETUP_UTIL -. $_SETUP_TMP -rm -f $_SETUP_TMP -unset _SETUP_TMP - -# source all environment hooks -_i=0 -while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do - eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i - unset _CATKIN_ENVIRONMENT_HOOKS_$_i - eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - # set workspace for environment hook - CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace - . "$_envfile" - unset CATKIN_ENV_HOOK_WORKSPACE - _i=$((_i + 1)) -done -unset _i - -unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/devel/setup.zsh b/devel/setup.zsh deleted file mode 100644 index b660717663..0000000000 --- a/devel/setup.zsh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env zsh -# generated from catkin/cmake/templates/setup.zsh.in - -CATKIN_SHELL=zsh -_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) -emulate sh # emulate POSIX -. "$_CATKIN_SETUP_DIR/setup.sh" -emulate zsh # back to zsh mode From 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 23 Sep 2014 16:55:19 +0200 Subject: [PATCH 009/122] Adapted math library for use of PX4 and ROS as shared library. First version, it works but some things might still be ugly --- src/lib/mathlib/math/Limits.hpp | 5 ++++- src/lib/mathlib/math/Matrix.hpp | 20 ++++++++++++++++++++ src/lib/mathlib/math/Quaternion.hpp | 4 ++++ src/lib/mathlib/math/Vector.hpp | 11 +++++++++++ 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd669..713cb51b56 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,10 @@ #pragma once +#ifdef CONFIG_ARCH_ARM #include +#endif + #include namespace math { @@ -84,4 +87,4 @@ float __EXPORT degrees(float radians); double __EXPORT degrees(double radians); -} \ No newline at end of file +} diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ca931e2daf..8fb3caa0d1 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,7 +44,15 @@ #define MATRIX_HPP #include +#include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#include +#define M_PI_2_F 1.570769 +#endif namespace math { @@ -65,7 +73,11 @@ public: /** * struct for using arm_math functions */ + #ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; + #else + eigen_matrix_instance arm_mat; + #endif /** * trivial ctor @@ -352,8 +364,16 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { + #ifdef CONFIG_ARCH_ARM Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + #else + //probably nicer if this could go into a function like "eigen_mat_mult" or so + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::VectorXf Product = Me * Vec; + Vector res(Product.data()); + #endif return res; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 21d05c7ef1..d8acc44434 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,7 +44,11 @@ #define QUATERNION_HPP #include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#endif + #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 0ddf776155..51a84d27eb 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -45,7 +45,13 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#include +#endif namespace math { @@ -65,7 +71,12 @@ public: /** * struct for using arm_math functions, represents column vector */ + #ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_col; + #else + eigen_matrix_instance arm_col; + #endif + /** * trivial ctor From 0553771f4fd6fbdba43669a8f17185ed61f96a51 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:52:28 +0200 Subject: [PATCH 010/122] Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 926a8db2ac..0019ef94d9 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,7 +45,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_PitchController::ECL_PitchController() : _last_run(0), @@ -61,8 +68,8 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _bodyrate_setpoint(0.0f) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } @@ -75,7 +82,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -138,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } From cfe14d78c5a9d2f80ebc0282e4bc400dcba6a795 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:53:23 +0200 Subject: [PATCH 011/122] Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 39b9f9d03c..973e15d987 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,8 +51,9 @@ #include #include +#ifdef CONFIG_ARM_ARCH #include - +#endif class __EXPORT ECL_PitchController //XXX: create controller superclass { public: @@ -129,7 +130,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H From 9f9fab400efbf8ad9cd44a0ff82cb9a97706134b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:57:20 +0200 Subject: [PATCH 012/122] Adapted for shared library use with ROS --- src/lib/ecl/ecl.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index aa3c5000a4..d6098dbfea 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -36,8 +36,11 @@ * Adapter / shim layer for system calls needed by ECL * */ - +#ifdef CONFIG_ARCH_ARM #include - #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time + +#else +#include +#endif From 5aefe11975c0fa04ec0a921ffb54d7cf8da5c39a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 09:01:30 +0200 Subject: [PATCH 013/122] Had to add definition of PI is used with ROS because cannot share math library yet-needs to be solved --- src/lib/mathlib/math/Limits.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a0..c593936ce6 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -45,7 +45,7 @@ namespace math { - +#define M_PI_F 3.14159265358979323846 float __EXPORT min(float val1, float val2) { @@ -143,4 +143,4 @@ double __EXPORT degrees(double radians) return (radians / M_PI) * 180.0; } -} \ No newline at end of file +} From 77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 09:11:44 +0200 Subject: [PATCH 014/122] Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 ++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 ++++- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0019ef94d9..b840206d58 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -75,7 +75,7 @@ ECL_PitchController::ECL_PitchController() : ECL_PitchController::~ECL_PitchController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 94bd26f03d..1b9925f632 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,7 +45,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_RollController::ECL_RollController() : _last_run(0), @@ -60,20 +67,20 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { } ECL_RollController::~ECL_RollController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -101,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0799dbe03b..84e6e9fe4f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,7 +51,10 @@ #include #include + +#ifdef CONIG_ARCH_ARM #include +#endif class __EXPORT ECL_RollController //XXX: create controller superclass { @@ -120,7 +123,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b80651..5b023fa8f3 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,7 +44,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_YawController::ECL_YawController() : _last_run(0), @@ -59,13 +66,13 @@ ECL_YawController::ECL_YawController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { } ECL_YawController::~ECL_YawController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -76,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -120,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index a360c14b89..61657e95be 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,7 +50,10 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include +#endif class __EXPORT ECL_YawController //XXX: create controller superclass { @@ -121,7 +124,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; From 2b8a9b632555708731d93f4aa7945d19e83d3134 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 10:29:28 +0200 Subject: [PATCH 015/122] Restored performance counter functionality, ROS package used own source file for function definitions but per_counter.h stays the same --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 +++--- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 ++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 8 ++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 6 ++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 8 ++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 6 ++---- 6 files changed, 17 insertions(+), 22 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b840206d58..8ee8b9c682 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -69,13 +69,13 @@ ECL_PitchController::ECL_PitchController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } ECL_PitchController::~ECL_PitchController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) @@ -145,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 973e15d987..39b9f9d03c 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,9 +51,8 @@ #include #include -#ifdef CONFIG_ARM_ARCH #include -#endif + class __EXPORT ECL_PitchController //XXX: create controller superclass { public: @@ -130,7 +129,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1b9925f632..6707a11ba7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -67,20 +67,20 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")), { } ECL_RollController::~ECL_RollController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -108,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 84e6e9fe4f..dbcabd8478 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,10 +51,8 @@ #include #include - -#ifdef CONIG_ARCH_ARM #include -#endif + class __EXPORT ECL_RollController //XXX: create controller superclass { @@ -123,7 +121,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5b023fa8f3..7ff8641756 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,13 +66,13 @@ ECL_YawController::ECL_YawController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f), - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { } ECL_YawController::~ECL_YawController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -83,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -127,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 61657e95be..c9e80930f3 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,10 +50,8 @@ #include #include - -#ifdef CONFIG_ARCH_ARM #include -#endif + class __EXPORT ECL_YawController //XXX: create controller superclass { @@ -124,7 +122,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; From d7cf6c4319cd9522286f0a7abb73e24a3050a6f7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 13:45:38 +0200 Subject: [PATCH 016/122] Use namespace std so that we can use the isfinite function, which should be available from math.h but the compiler gives an error that the function is undeclared --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 8ee8b9c682..fa0f98e16f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,7 +51,8 @@ #else #include #include -#define isfinite std::isfinite +//#define isfinite std::isfinite +using namespace std; #endif ECL_PitchController::ECL_PitchController() : @@ -68,7 +69,7 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6707a11ba7..640940c99a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -51,7 +51,7 @@ #else #include #include -#define isfinite std::isfinite +using namespace std; #endif ECL_RollController::ECL_RollController() : @@ -67,7 +67,7 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7ff8641756..5f01cecf9a 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -50,7 +50,7 @@ #else #include #include -#define isfinite std::isfinite +using namespace std; #endif ECL_YawController::ECL_YawController() : From f347e87391d35d43bdf4bdf51323ea0c53e9ead7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 15:54:34 +0200 Subject: [PATCH 017/122] Added base class for fixed wing attitude controller -> still working on it --- .../fw_att_control/fw_att_control_base.cpp | 50 +++++++ .../fw_att_control/fw_att_control_base.h | 124 ++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp create mode 100644 src/modules/fw_att_control/fw_att_control_base.h diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp new file mode 100644 index 0000000000..4f61f02ef7 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,50 @@ +/* + * fw_att_control_base.cpp + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#include "fw_att_control_base.h" + + +FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : + + _task_should_exit(false), + _task_running(false), + _control_task(-1), + + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), +/* states */ + _setpoint_valid(false), + _debug(false) +{ + /* safely initialize structs */ + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + + + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() +{ + +} + +void FixedwingAttitudeControlBase::control_attitude() +{ + +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h new file mode 100644 index 0000000000..9a2d9b1956 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,124 @@ +/* + * fw_att_control_base.h + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControlBase(); + + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + + + 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 manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_ff; + float y_roll_feedforward; + float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + } _parameters; /**< local copies of interesting parameters */ + + + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + + + +}; + + + +#endif /* FW_ATT_CONTROL_BASE_H_ */ From 81a5aeb6f5cb022e6987a1651658d9b62359cccb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 09:11:54 +0200 Subject: [PATCH 018/122] Restored to original PX4 version since the hrt_time functions are now also implemented in ROS --- src/lib/ecl/ecl.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index d6098dbfea..662e3a39f5 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -36,11 +36,8 @@ * Adapter / shim layer for system calls needed by ECL * */ -#ifdef CONFIG_ARCH_ARM + #include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time -#else -#include -#endif From 96b22f1ba8980ea8e1cbc405a808ae920102f064 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:20:15 +0200 Subject: [PATCH 019/122] Adapted uORB topic files to work with ROS (data stuctures are used but not the uORB functionality) --- src/modules/uORB/topics/actuator_controls.h | 4 ++++ src/modules/uORB/topics/airspeed.h | 4 ++++ src/modules/uORB/topics/manual_control_setpoint.h | 6 ++++-- src/modules/uORB/topics/parameter_update.h | 6 +++++- src/modules/uORB/topics/vehicle_attitude.h | 4 ++++ src/modules/uORB/topics/vehicle_attitude_setpoint.h | 4 ++++ src/modules/uORB/topics/vehicle_control_mode.h | 4 ++++ src/modules/uORB/topics/vehicle_global_position.h | 5 ++++- src/modules/uORB/topics/vehicle_rates_setpoint.h | 4 ++++ src/modules/uORB/topics/vehicle_status.h | 4 ++++ 10 files changed, 41 insertions(+), 4 deletions(-) diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index e768ab2f61..6c641dbcec 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -47,7 +47,9 @@ #define TOPIC_ACTUATOR_CONTROLS_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ @@ -70,9 +72,11 @@ struct actuator_controls_s { */ /* actuator control sets; this list can be expanded as more controllers emerge */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); +#endif #endif diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index d2ee754cdf..4c115a8113 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,7 +40,9 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #include /** @@ -63,6 +65,8 @@ struct airspeed_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(airspeed); +#endif #endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc3..af5df69792 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -41,8 +41,9 @@ #define TOPIC_MANUAL_CONTROL_SETPOINT_H_ #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" - +#endif /** * Switch position */ @@ -106,6 +107,7 @@ struct manual_control_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(manual_control_setpoint); - +#endif #endif diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 68964deb0a..7afb78d49d 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -40,7 +40,9 @@ #define TOPIC_PARAMETER_UPDATE_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -56,6 +58,8 @@ struct parameter_update_s { * @} */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(parameter_update); +#endif -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 40328af146..7780988c84 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -44,7 +44,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -87,6 +89,8 @@ struct vehicle_attitude_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8446e9c6e2..8b5a761433 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -42,7 +42,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -82,6 +84,8 @@ struct vehicle_attitude_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude_setpoint); +#endif #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b56..b071e0fa3e 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,7 +48,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #include "vehicle_status.h" /** @@ -89,6 +91,8 @@ struct vehicle_control_mode_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_control_mode); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b8936..e8f010924d 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,7 +45,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -81,6 +83,7 @@ struct vehicle_global_position_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_global_position); - +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 9f8b412a7d..cbfab89d60 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -41,7 +41,9 @@ #define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -62,6 +64,8 @@ struct vehicle_rates_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_rates_setpoint); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b683bf98ad..973987f5c3 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -53,7 +53,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @{ @@ -234,6 +236,8 @@ struct vehicle_status_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_status); +#endif #endif From af290826d9957a75ff62dbd1451988228c603a45 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:24:04 +0200 Subject: [PATCH 020/122] Fixed so that when working with ROS the same err.h is included --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 +--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 +-- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index fa0f98e16f..d1f79f0ead 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,13 +45,11 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include -//#define isfinite std::isfinite using namespace std; #endif diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 640940c99a..30176f92fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,11 +45,10 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include using namespace std; #endif diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5f01cecf9a..1b4d8486c5 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,11 +44,10 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include using namespace std; #endif From a69ae3493d99250bd5a46ba1ac5f11fcf2d05069 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:24:39 +0200 Subject: [PATCH 021/122] Adapted so that can be used with ROS --- src/drivers/drv_accel.h | 64 ++++++++++++++++++++++------------------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966bd..49b104d380 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -42,12 +42,13 @@ #include #include - +#ifdef CONFIG_ARCH_ARM #include "drv_sensor.h" #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" +#endif /** * accel report structure. Reads from the device must be in multiples of this * structure. @@ -81,46 +82,49 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +#ifdef CONFIG_ARCH_ARM + ORB_DECLARE(sensor_accel0); + ORB_DECLARE(sensor_accel1); + ORB_DECLARE(sensor_accel2); -/* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ + /* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ -#define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) + #define _ACCELIOCBASE (0x2100) + #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) -/** set the accel internal sample rate to at least (arg) Hz */ -#define ACCELIOCSSAMPLERATE _ACCELIOC(0) + /** set the accel internal sample rate to at least (arg) Hz */ + #define ACCELIOCSSAMPLERATE _ACCELIOC(0) -/** return the accel internal sample rate in Hz */ -#define ACCELIOCGSAMPLERATE _ACCELIOC(1) + /** return the accel internal sample rate in Hz */ + #define ACCELIOCGSAMPLERATE _ACCELIOC(1) -/** set the accel internal lowpass filter to no lower than (arg) Hz */ -#define ACCELIOCSLOWPASS _ACCELIOC(2) + /** set the accel internal lowpass filter to no lower than (arg) Hz */ + #define ACCELIOCSLOWPASS _ACCELIOC(2) -/** return the accel internal lowpass filter in Hz */ -#define ACCELIOCGLOWPASS _ACCELIOC(3) + /** return the accel internal lowpass filter in Hz */ + #define ACCELIOCGLOWPASS _ACCELIOC(3) -/** set the accel scaling constants to the structure pointed to by (arg) */ -#define ACCELIOCSSCALE _ACCELIOC(5) + /** set the accel scaling constants to the structure pointed to by (arg) */ + #define ACCELIOCSSCALE _ACCELIOC(5) -/** get the accel scaling constants into the structure pointed to by (arg) */ -#define ACCELIOCGSCALE _ACCELIOC(6) + /** get the accel scaling constants into the structure pointed to by (arg) */ + #define ACCELIOCGSCALE _ACCELIOC(6) -/** set the accel measurement range to handle at least (arg) g */ -#define ACCELIOCSRANGE _ACCELIOC(7) + /** set the accel measurement range to handle at least (arg) g */ + #define ACCELIOCSRANGE _ACCELIOC(7) -/** get the current accel measurement range in g */ -#define ACCELIOCGRANGE _ACCELIOC(8) + /** get the current accel measurement range in g */ + #define ACCELIOCGRANGE _ACCELIOC(8) -/** get the result of a sensor self-test */ -#define ACCELIOCSELFTEST _ACCELIOC(9) + /** get the result of a sensor self-test */ + #define ACCELIOCSELFTEST _ACCELIOC(9) + +#endif #endif /* _DRV_ACCEL_H */ From 6329ca1a70b220b99ea793f416aedc8ab6fbccff Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:25:57 +0200 Subject: [PATCH 022/122] Adapted so that this header can also be used in a ROS environment --- src/modules/systemlib/err.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index ca13d62655..2a201ee80b 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -67,6 +67,7 @@ #include +#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT const char *getprogname(void); @@ -86,4 +87,8 @@ __EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, __END_DECLS +#else //we are using ROS (should make a variable!!!) +#include +#define warnx ROS_WARN +#endif #endif From 43d9ebc231501fe2d3fe6541f1079d2d019a2698 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:28:49 +0200 Subject: [PATCH 023/122] Added control_attitude function and cleaned up --- .../fw_att_control/fw_att_control_base.cpp | 225 ++++++++++++++++-- 1 file changed, 207 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 4f61f02ef7..7218e4e9bb 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -6,23 +6,23 @@ */ #include "fw_att_control_base.h" +#include +#include +#include +#include +using namespace std; FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), + _task_should_exit(false), _task_running(false), _control_task(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), - _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ - _setpoint_valid(false), - _debug(false) -{ + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; _accel = {}; @@ -37,14 +37,203 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : } - - -FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() -{ +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { } -void FixedwingAttitudeControlBase::control_attitude() -{ +void FixedwingAttitudeControlBase::control_attitude() { + bool lock_integrator = false; + static int loop_counter = 0; + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is not updating, we assume the normal average speed */ + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _parameters.airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); + + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_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 { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; + att_sp.thrust = throttle_sp; + + } + + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { + if (_debug && loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } + } + + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, + speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, + _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!isfinite(roll_u)) { + _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + + if (_debug && loop_counter % 10 == 0) { + warnx("roll_u %.4f", (double) roll_u); + } + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; + if (!isfinite(pitch_u)) { + _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, (double) airspeed_scaling, + (double) roll_sp, (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); + } + } + + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + if (!isfinite(yaw_u)) { + _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double) yaw_u); + } + } + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + if (_debug && loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double) throttle_sp); + } + } + } else { + perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); + } + } } From 43056258839c67590dec811e923553fad66a0f4b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:29:30 +0200 Subject: [PATCH 024/122] Added control_attitude function --- src/modules/fw_att_control/fw_att_control_base.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 9a2d9b1956..6e071fe201 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -114,6 +114,8 @@ protected: ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + void control_attitude(); + From 020a8292155cd2821cb66a9709a8c7c18524bdac Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:00:15 +0200 Subject: [PATCH 025/122] Adapted for shared library use with ROS --- src/lib/mathlib/math/Matrix.hpp | 114 +++++++++++++++++++------------- 1 file changed, 68 insertions(+), 46 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 8fb3caa0d1..34fc4604ac 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -54,10 +54,9 @@ #define M_PI_2_F 1.570769 #endif -namespace math -{ +namespace math { -template +template class __EXPORT Matrix; // MxN matrix with float elements @@ -73,19 +72,19 @@ public: /** * struct for using arm_math functions */ - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; - #else +#else eigen_matrix_instance arm_mat; - #endif +#endif /** * trivial ctor * Initializes the elements to zero. */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} + MatrixBase() : + data {}, + arm_mat {M, N, &data[0][0]} { } @@ -95,19 +94,19 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} + MatrixBase(const float d[M][N]) : + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -159,9 +158,9 @@ public: */ bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; return true; } @@ -171,9 +170,9 @@ public: */ bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; return false; } @@ -193,8 +192,8 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; return res; } @@ -206,16 +205,16 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } Matrix &operator +=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; return *static_cast*>(this); } @@ -227,16 +226,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } Matrix &operator -=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; return *static_cast*>(this); } @@ -248,16 +247,17 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; return res; + } Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; return *static_cast*>(this); } @@ -266,16 +266,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; return *static_cast*>(this); } @@ -285,27 +285,49 @@ public: */ template Matrix operator *(const Matrix &m) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map >(m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; + Matrix res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Me.transposeInPlace(); + Matrix res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Matrix res(MyInverse.data()); + return res; +#endif } /** @@ -323,7 +345,7 @@ public: unsigned int n = (M < N) ? M : N; for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + data[i][i] = 1; } void print(void) { @@ -331,7 +353,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", data[i][j]); printf(" ]\n"); } @@ -364,16 +386,16 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); - #else +#else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); Eigen::VectorXf Product = Me * Vec; Vector res(Product.data()); - #endif +#endif return res; } }; @@ -405,8 +427,8 @@ public: */ Vector<3> operator *(const Vector<3> &v) const { Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); return res; } From c8b1c5b119e6552e8e5420ca8fecd24b2a3ad4a2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:01:11 +0200 Subject: [PATCH 026/122] Adapted for shared library use with ROS --- src/lib/geo/geo.h | 6 +- src/lib/geo_lookup/geo_mag_declination.h | 4 + .../fw_att_control/fw_att_control_main.cpp | 403 +++++++++--------- src/modules/uORB/topics/actuator_armed.h | 6 +- 4 files changed, 219 insertions(+), 200 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 2311e0a7c9..ff2d92389f 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,14 +45,16 @@ #pragma once +#ifdef CONFIG_ARCH_ARM #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" __BEGIN_DECLS - +#endif #include "geo_lookup/geo_mag_declination.h" #include +#include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ @@ -276,4 +278,6 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +#ifdef CONFIG_ARCH_ARM __END_DECLS +#endif diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index 0ac062d6d4..d79b784121 100644 --- a/src/lib/geo_lookup/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -40,8 +40,12 @@ #pragma once +#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT float get_mag_declination(float lat, float lon); __END_DECLS +#else +float get_mag_declination(float lat, float lon); +#endif diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c60d8d3489..5cff390e2c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,8 +83,7 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl -{ +class FixedwingAttitudeControl { public: /** * Constructor @@ -101,54 +100,56 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { return _task_running; } + bool task_running() { + return _task_running; + } private: - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ - 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 _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) */ - 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 manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ + 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 manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _debug; /**< if set to true, print debug output */ + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -182,14 +183,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ - } _parameters; /**< local copies of interesting parameters */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -228,75 +229,71 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ - - - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; + } _parameter_handles; /**< handles for interesting parameters */ + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); - + void vehicle_manual_poll(); /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control -{ +namespace att_control { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -304,39 +301,28 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), + _task_should_exit(false), _task_running(false), _control_task(-1), -/* subscriptions */ - _att_sub(-1), - _accel_sub(-1), - _airspeed_sub(-1), - _vcontrol_mode_sub(-1), - _params_sub(-1), - _manual_sub(-1), - _global_pos_sub(-1), - _vehicle_status_sub(-1), + /* subscriptions */ + _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( + -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( + -1), -/* publications */ - _rate_sp_pub(-1), - _attitude_sp_pub(-1), - _actuators_0_pub(-1), - _actuators_1_pub(-1), + /* publications */ + _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( + -1), -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), - _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ - _setpoint_valid(false), - _debug(false) -{ + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; _accel = {}; @@ -349,7 +335,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; - _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -390,8 +375,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() -{ +FixedwingAttitudeControl::~FixedwingAttitudeControl() { if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -419,9 +403,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() att_control::g_control = nullptr; } -int -FixedwingAttitudeControl::parameters_update() -{ +int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -429,21 +411,26 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, + &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, + &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, + &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, + &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, + &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -453,16 +440,19 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, + &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, + &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians( + _parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians( + _parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); - /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -492,9 +482,7 @@ FixedwingAttitudeControl::parameters_update() return OK; } -void -FixedwingAttitudeControl::vehicle_control_mode_poll() -{ +void FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -502,13 +490,12 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, + &_vcontrol_mode); } } -void -FixedwingAttitudeControl::vehicle_manual_poll() -{ +void FixedwingAttitudeControl::vehicle_manual_poll() { bool manual_updated; /* get pilots inputs */ @@ -520,9 +507,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -533,9 +518,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() } } -void -FixedwingAttitudeControl::vehicle_accel_poll() -{ +void FixedwingAttitudeControl::vehicle_accel_poll() { /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -545,9 +528,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() } } -void -FixedwingAttitudeControl::vehicle_setpoint_poll() -{ +void FixedwingAttitudeControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -558,21 +539,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } } -void -FixedwingAttitudeControl::global_pos_poll() -{ +void FixedwingAttitudeControl::global_pos_poll() { /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, + &_global_pos); } } -void -FixedwingAttitudeControl::vehicle_status_poll() -{ +void FixedwingAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -582,15 +560,11 @@ FixedwingAttitudeControl::vehicle_status_poll() } } -void -FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ +void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { att_control::g_control->task_main(); } -void -FixedwingAttitudeControl::task_main() -{ +void FixedwingAttitudeControl::task_main() { /* inform about start */ warnx("Initializing.."); @@ -667,7 +641,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -721,8 +694,8 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); @@ -740,16 +713,20 @@ FixedwingAttitudeControl::task_main() * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; - if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_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; + pitch_sp = _att_sp.pitch_body + + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -775,10 +752,12 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max + - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -797,11 +776,13 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), + _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise( + ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -816,11 +797,17 @@ FixedwingAttitudeControl::task_main() float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; - } else { + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } @@ -829,74 +816,94 @@ FixedwingAttitudeControl::task_main() /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, + _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : + _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("roll_u %.4f", (double)roll_u); + warnx("roll_u %.4f", (double) roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, + _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), - (double)airspeed, (double)airspeed_scaling, - (double)roll_sp, (double)pitch_sp, - (double)_roll_ctrl.get_desired_rate(), - (double)_pitch_ctrl.get_desired_rate(), - (double)_att_sp.roll_body); + warnx( + "pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, + (double) airspeed_scaling, (double) roll_sp, + (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); } } - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, + _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : + _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("yaw_u %.4f", (double)yaw_u); + warnx("yaw_u %.4f", (double) yaw_u); } } /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + _actuators.control[3] = + (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double)throttle_sp); + warnx("throttle_sp %.4f", (double) throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + warnx( + "Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); } } @@ -913,11 +920,13 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, + &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), + &rates_sp); } } else { @@ -939,16 +948,19 @@ FixedwingAttitudeControl::task_main() if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, + &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), + &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + 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], @@ -956,7 +968,8 @@ FixedwingAttitudeControl::task_main() } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), + &_actuators_airframe); } } @@ -972,18 +985,14 @@ FixedwingAttitudeControl::task_main() _exit(0); } -int -FixedwingAttitudeControl::start() -{ +int FixedwingAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, 2048, + (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); @@ -993,8 +1002,7 @@ FixedwingAttitudeControl::start() return OK; } -int fw_att_control_main(int argc, char *argv[]) -{ +int fw_att_control_main(int argc, char *argv[]) { if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1015,7 +1023,8 @@ int fw_att_control_main(int argc, char *argv[]) } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr + || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 0f6c9aca17..9ec9d10aba 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -42,8 +42,9 @@ #define TOPIC_ACTUATOR_ARMED_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" - +#endif /** * @addtogroup topics * @{ @@ -64,6 +65,7 @@ struct actuator_armed_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_armed); - +#endif #endif From 4fdf8e1ff26d37513c003ea1e92445fae81cc2cb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 23 Oct 2014 16:59:21 +0200 Subject: [PATCH 027/122] updated from remote --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 + ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 63 ++ ROMFS/px4fmu_common/mixers/FMU_quadshot.mix | 15 + Tools/tests-host/sf0x_test.cpp | 65 ++ Tools/tests-host/st24_test.cpp | 76 ++ src/drivers/sf0x/sf0x_parser.cpp | 155 +++ src/drivers/sf0x/sf0x_parser.h | 51 + src/lib/rc/module.mk | 40 + src/lib/rc/st24.c | 253 +++++ src/lib/rc/st24.h | 163 ++++ src/modules/bottle_drop/bottle_drop.cpp | 907 ++++++++++++++++++ src/modules/bottle_drop/bottle_drop_params.c | 131 +++ src/modules/bottle_drop/module.mk | 41 + .../mc_att_control/mc_att_control_base.cpp | 283 ++++++ .../mc_att_control/mc_att_control_base.h | 172 ++++ src/modules/navigator/datalinkloss.cpp | 227 +++++ src/modules/navigator/datalinkloss.h | 98 ++ src/modules/navigator/datalinkloss_params.c | 126 +++ src/modules/navigator/enginefailure.cpp | 149 +++ src/modules/navigator/enginefailure.h | 83 ++ src/modules/navigator/gpsfailure.cpp | 178 ++++ src/modules/navigator/gpsfailure.h | 97 ++ src/modules/navigator/gpsfailure_params.c | 97 ++ src/modules/navigator/rcloss.cpp | 182 ++++ src/modules/navigator/rcloss.h | 88 ++ src/modules/navigator/rcloss_params.c | 60 ++ .../uORB/topics/multirotor_motor_limits.h | 69 ++ src/modules/uORB/topics/vtol_vehicle_status.h | 66 ++ src/modules/vtol_att_control/module.mk | 40 + .../vtol_att_control_main.cpp | 642 +++++++++++++ 30 files changed, 4632 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix create mode 100644 Tools/tests-host/sf0x_test.cpp create mode 100644 Tools/tests-host/st24_test.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.h create mode 100644 src/lib/rc/module.mk create mode 100644 src/lib/rc/st24.c create mode 100644 src/lib/rc/st24.h create mode 100644 src/modules/bottle_drop/bottle_drop.cpp create mode 100644 src/modules/bottle_drop/bottle_drop_params.c create mode 100644 src/modules/bottle_drop/module.mk create mode 100644 src/modules/mc_att_control/mc_att_control_base.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_base.h create mode 100644 src/modules/navigator/datalinkloss.cpp create mode 100644 src/modules/navigator/datalinkloss.h create mode 100644 src/modules/navigator/datalinkloss_params.c create mode 100644 src/modules/navigator/enginefailure.cpp create mode 100644 src/modules/navigator/enginefailure.h create mode 100644 src/modules/navigator/gpsfailure.cpp create mode 100644 src/modules/navigator/gpsfailure.h create mode 100644 src/modules/navigator/gpsfailure_params.c create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h create mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h create mode 100644 src/modules/vtol_att_control/module.mk create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps new file mode 100644 index 0000000000..23ade6d78b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -0,0 +1,15 @@ +#!nsh +# +# Standard apps for vtol: +# att & pos estimator, att & pos control. +# + +attitude_estimator_ekf start +#ekf_att_pos_estimator start +position_estimator_inav start + +vtol_att_control start +mc_att_control start +mc_pos_control start +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults new file mode 100644 index 0000000000..f0ea9add07 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -0,0 +1,63 @@ +#!nsh + +set VEHICLE_TYPE vtol + +if [ $DO_AUTOCONFIG == yes ] +then + #Default parameters for MC + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILTMAX_AIR 45.0 + param set MPC_TILTMAX_LND 15.0 + param set MPC_LAND_SPEED 1.0 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + + param set NAV_ACCEPT_RAD 2.0 + + # + # Default parameters for FW + # + param set NAV_LAND_ALT 90 + param set NAV_RTL_ALT 100 + param set NAV_RTL_LAND_T -1 + param set NAV_ACCEPT_RAD 50 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1075 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix new file mode 100644 index 0000000000..b077ff30ac --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix @@ -0,0 +1,15 @@ +#!nsh +#Quadshot mixer for PX4FMU +#=========================== +R: 4v 10000 10000 10000 0 + +#mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp new file mode 100644 index 0000000000..82d19fcbe1 --- /dev/null +++ b/Tools/tests-host/sf0x_test.cpp @@ -0,0 +1,65 @@ + +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char *argv[]) +{ + warnx("SF0X test started"); + + int ret = 0; + + const char LINE_MAX = 20; + char _linebuf[LINE_MAX]; + _linebuf[0] = '\0'; + + const char *lines[] = {"0.01\r\n", + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; + + enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; + float dist_m; + char _parserbuf[LINE_MAX]; + unsigned _parsebuf_index = 0; + + for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { + + printf("\n%s", _linebuf); + + int parse_ret; + + for (int i = 0; i < strlen(lines[l]); i++) { + parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); + + if (parse_ret == 0) { + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); + } + } + + printf("%s", lines[l]); + + } + + warnx("test finished"); + + return ret; +} diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp new file mode 100644 index 0000000000..25a9355e2e --- /dev/null +++ b/Tools/tests-host/st24_test.cpp @@ -0,0 +1,76 @@ + +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) +{ + warnx("ST24 test started"); + + if (argc < 2) { + errx(1, "Need a filename for the input file"); + } + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1], "rt"); + + if (!fp) { + errx(1, "failed opening file"); + } + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + // warnx("FRAME RESET\n\n"); + } + + uint8_t b = static_cast(x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + uint16_t channels[20]; + + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { + warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + + for (unsigned i = 0; i < channel_count; i++) { + + int16_t val = channels[i]; + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + } + } + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + + } else { + warnx("Test aborted, errno: %d", ret); + } + +} diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 0000000000..8e73b0ad35 --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * 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 sf0x_parser.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include "sf0x_parser.h" +#include +#include + +//#define SF0X_DEBUG + +#ifdef SF0X_DEBUG +#include + +const char *parser_state[] = { + "0_UNSYNC", + "1_SYNC", + "2_GOT_DIGIT0", + "3_GOT_DOT", + "4_GOT_DIGIT1", + "5_GOT_DIGIT2", + "6_GOT_CARRIAGE_RETURN" +}; +#endif + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char *end; + + switch (*state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + } + +#ifdef SF0X_DEBUG + printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +} \ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 0000000000..20892d50eb --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 sf0x_parser.cpp + * @author Lorenz Meier + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist); \ No newline at end of file diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 0000000000..e089c69658 --- /dev/null +++ b/src/lib/rc/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Yuntec ST24 transmitter protocol decoder +# + +SRCS = st24.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c new file mode 100644 index 0000000000..e8a791b8f6 --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * 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 st24.h + * + * RC protocol implementation for Yuneec ST24 transmitter. + * + * @author Lorenz Meier + */ + +#include +#include +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED = 0, + ST24_DECODE_STATE_GOT_STX1, + ST24_DECODE_STATE_GOT_STX2, + ST24_DECODE_STATE_GOT_LEN, + ST24_DECODE_STATE_GOT_TYPE, + ST24_DECODE_STATE_GOT_DATA +}; + +const char *decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA" + }; + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 4096.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + +static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; +static unsigned _rxlen; + +static ReceiverFcPacket _rxpacket; + +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} + + +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) +{ + + int ret = 1; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { + ret = 3; + } + + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + ret = 0; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 12) ? max_chan_count : 12; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 24) ? max_chan_count : 24; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + ret = 2; + } + break; + + default: + ret = 2; + break; + } + + } else { + /* decoding failed */ + ret = 4; + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 0000000000..454234601d --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 st24.h + * + * RC protocol definition for Yuneec ST24 transmitter + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +__BEGIN_DECLS + +#define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 + +enum ST24_PACKET_TYPE { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +}; + +#pragma pack(push, 1) +typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data +} ReceiverFcPacket; + +/** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) +} ChannelData12; + +/** + * RC Channel data (12 channels). + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) +} ChannelData24; + +/** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; /// + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + + void open_bay(); + void close_bay(); + void drop(); + void lock_release(); + +private: + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ + int _mavlink_fd; + + int _command_sub; + int _wind_estimate_sub; + struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _drop_approval; + hrt_abstime _doors_opened; + hrt_abstime _drop_time; + + float _alt_clearance; + + struct position_s { + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m + } _target_position, _drop_position; + + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Set the actuators + */ + int actuators_publish(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1), + _command_sub(-1), + _wind_estimate_sub(-1), + _command {}, + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(70.0f), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("drop state: %d", _drop_state); +} + +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); + + usleep(500 * 1000); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + _actuators.control[2] = 1.0f; + + _drop_time = hrt_absolute_time(); + actuators_publish(); + + warnx("dropping now"); + + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; + actuators_publish(); + + warnx("closing release"); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + +void +BottleDrop::task_main() +{ + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + float ground_distance = _alt_clearance; // Replace by closer estimate in loop + + // constant + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + unsigned counter = 0; + + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &m); + param_get(param_surface, &A); + + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + struct wind_estimate_s wind; + + // wakeup source(s) + struct pollfd fds[1]; + + // Setup of loop + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + // Whatever state the bay is in, we want it closed on startup + lock_release(); + close_bay(); + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + if (_global_pos.timestamp == 0) { + continue; + } + + const unsigned sleeptime_us = 9500; + + hrt_abstime last_run = hrt_absolute_time(); + float dt_runs = sleeptime_us / 1e6f; + + // switch to faster updates during the drop + while (_drop_state > DROP_STATE_INIT) { + + // Get wind estimate + orb_check(_wind_estimate_sub, &updated); + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } + + // Get vehicle position + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + // Get parameter updates + orb_check(parameter_update_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + // update all parameters + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + orb_check(_command_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); + ground_distance = _global_pos.alt - _target_position.alt; + + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } + } + + switch (_drop_state) { + + case DROP_STATE_TARGET_VALID: + { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // compute drop vector + x = groundspeed_body * t_signal + x; + + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = _target_position.alt + _alt_clearance; + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt; + + // Save WPs in datamanager + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + + } else { + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); + } + + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + + _drop_state = DROP_STATE_TARGET_SET; + } + break; + + case DROP_STATE_TARGET_SET: + { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door && + fabsf(approach_error) < math::radians(20.0f)) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } + } + } + break; + + case DROP_STATE_BAY_OPEN: + { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + } + } + } + break; + + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + break; + } + + counter++; + + // update_actuators(); + + // run at roughly 100 Hz + usleep(sleeptime_us); + + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; + last_run = hrt_absolute_time(); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) +{ + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + open_bay(); + drop(); + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + + } else if (cmd->param1 > 0.5f) { + open_bay(); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + + } else { + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + break; + + default: + _drop_approval = false; + warnx("param1 val unknown"); + break; + } + + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + _drop_state = DROP_STATE_TARGET_VALID; + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); + map_projection_init(&ref, _target_position.lat, _target_position.lon); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + + if (cmd->param1 < 0) { + + // Clear internal states + _drop_approval = false; + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + _onboard_mission.current_seq = -1; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + default: + break; + } +} + +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} + +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) +{ + bottle_drop::g_bottle_drop->task_main(); +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "not running"); + } + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 0000000000..51ebfb9a19 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 bottle_drop_params.c + * Bottle drop parameters + * + * @author Dominik Juchli + */ + +#include +#include + +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 0.08 + * @max 1.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 0000000000..6b18fff556 --- /dev/null +++ b/src/modules/bottle_drop/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013, 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. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.cpp \ + bottle_drop_params.c diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp new file mode 100644 index 0000000000..d4270b1530 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -0,0 +1,283 @@ +/* + * mc_att_control_base.cpp + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#include "mc_att_control_base.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + + _task_should_exit(false), _control_task(-1), + + _actuators_0_circuit_breaker_enabled(false), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +} + +void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) { + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled + || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.z; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi( + _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + } + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x + * _params.man_pitch_max; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, + _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + +// /* publish the attitude setpoint if needed */ +// if (publish_att_sp) { +// _v_att_sp.timestamp = hrt_absolute_time(); +// +// if (_att_sp_pub > 0) { +// orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, +// &_v_att_sp); +// +// } else { +// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), +// &_v_att_sp); +// } +// } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h new file mode 100644 index 0000000000..d75088b85b --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -0,0 +1,172 @@ +/* + * mc_att_control_base.h + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* Copyright (c) 2013, 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 mc_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase { +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + + + + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + 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 */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + } _params; + + + /** + * Attitude controller. + */ + //void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void vehicle_attitude_setpoint_poll(); //provisional + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 0000000000..66f1c8c731 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 datalinkloss.cpp + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), + _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), + _dll_state(DLL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset DLL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _dll_state = DLL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + _dll_state = DLL_STATE_NONE; + updateParams(); + advance_dll(); + set_dll_item(); +} + +void +DataLinkLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_dll(); + set_dll_item(); + } +} + +void +DataLinkLoss::set_dll_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_commsholdalt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_airfieldhomealt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +DataLinkLoss::advance_dll() +{ + switch (_dll_state) { + case DLL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _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(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } + } + break; + case DLL_STATE_FLYTOCOMMSHOLDWP: + warnx("fly to airfield home"); + 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(); + break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + 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(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 0000000000..31e0e39078 --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,98 @@ +/*************************************************************************** + * + * Copyright (c) 2013-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 datalinkloss.h + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; + control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 + } _dll_state; + + /** + * Set the DLL item + */ + void set_dll_item(); + + /** + * Move to next DLL item + */ + void advance_dll(); + +}; +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 0000000000..6780c0c88c --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 datalinkloss_params.c + * + * Parameters for DLL + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * Data Link Loss parameters, accessible via MAVLink + */ + +/** + * Comms hold wait time + * + * The amount of time in seconds the system should wait at the comms hold waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); + +/** + * Comms hold Lat + * + * Latitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); + +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); + +/** + * Comms hold alt + * + * Altitude of comms hold waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); + +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group DLL + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 0000000000..e007b208b2 --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_NONE; + advance_ef(); + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 0000000000..2c48c2fce2 --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * Copyright (c) 2013-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 enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 0000000000..cd55f60b06 --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: + set_gpsf_item(); + advance_gpsf(); + break; + default: + break; + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + + switch (_gpsf_state) { + case GPSF_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("gps fail: request flight termination"); + } + default: + break; + } + + reset_mission_item_reached(); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + updateParams(); + + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); + break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 0000000000..e9e72babf8 --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,97 @@ +/*************************************************************************** + * + * Copyright (c) 2013-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 gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include +#include + +#include +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, + } _gpsf_state; + + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 0000000000..39d179eed7 --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 0000000000..5564a1c42e --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("rc not recovered: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + 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(); + } + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + 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(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 0000000000..bcb74d8778 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * Copyright (c) 2013-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 rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 0000000000..f1a01c73b5 --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering + * + * @unit seconds + * @min -1.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 0000000000..44c51e4d98 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012-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 multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h new file mode 100644 index 0000000000..24ecca9faa --- /dev/null +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -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 +#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 diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk new file mode 100644 index 0000000000..c349c23401 --- /dev/null +++ b/src/modules/vtol_att_control/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013, 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. +# +############################################################################ + +# +# VTOL attitude controller +# + +MODULE_COMMAND = vtol_att_control + +SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp new file mode 100644 index 0000000000..38fa4eec18 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -0,0 +1,642 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drivers/drv_pwm_output.h" +#include +#include + +#include + +#include + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + + struct { + float min_pwm_mc; //pwm value for idle in mc mode + } _params; + + struct { + param_t min_pwm_mc; + } _params_handles; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_control_output(); //write mc_att_control results to actuator message + void fill_fw_att_control_output(); //write fw_att_control results to actuator message + void set_idle_fw(); + void set_idle_mc(); +}; + +namespace VTOL_att_control +{ +VtolAttitudeControl *g_control; +} + +/** +* Constructor +*/ +VtolAttitudeControl::VtolAttitudeControl() : + _task_should_exit(false), + _control_task(-1), + + //init subscription handlers + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + + //init publication handlers + _actuators_0_pub(-1), + _actuators_1_pub(-1), + _vtol_vehicle_status_pub(-1), + + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) +{ + + flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ + + memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); + memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); + memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); + memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); + memset(&_armed, 0, sizeof(_armed)); + + _params_handles.min_pwm_mc = param_find("PWM_MIN"); + + /* fetch initial parameter values */ + parameters_update(); +} + +/** +* Destructor +*/ +VtolAttitudeControl::~VtolAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + VTOL_att_control::g_control = nullptr; +} + +/** +* Check for changes in vehicle control mode. +*/ +void VtolAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check if vehicle control mode has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +/** +* Check for changes in manual inputs. +*/ +void VtolAttitudeControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} +/** +* Check for arming status updates. +*/ +void VtolAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/** +* Check for inputs from mc attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_mc_poll() +{ + bool updated; + orb_check(_actuator_inputs_mc, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); + } +} + +/** +* Check for inputs from fw attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_fw_poll() +{ + bool updated; + orb_check(_actuator_inputs_fw, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); + } +} + +/** +* Check for parameter updates. +*/ +void +VtolAttitudeControl::parameters_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +/** +* Update parameters. +*/ +int +VtolAttitudeControl::parameters_update() +{ + /* idle pwm */ + float v; + param_get(_params_handles.min_pwm_mc, &v); + _params.min_pwm_mc = v; + + return OK; +} + +/** +* Prepare message to acutators with data from mc attitude controller. +*/ +void VtolAttitudeControl::fill_mc_att_control_output() +{ + _actuators_out_0.control[0] = _actuators_mc_in.control[0]; + _actuators_out_0.control[1] = _actuators_mc_in.control[1]; + _actuators_out_0.control[2] = _actuators_mc_in.control[2]; + _actuators_out_0.control[3] = _actuators_mc_in.control[3]; + //set neutral position for elevons + _actuators_out_1.control[0] = 0; //roll elevon + _actuators_out_1.control[1] = 0; //pitch elevon +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void VtolAttitudeControl::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0.control[0] = 0; + _actuators_out_0.control[1] = 0; + _actuators_out_0.control[2] = 0; + _actuators_out_0.control[3] = _actuators_fw_in.control[3]; + /*controls for the elevons */ + _actuators_out_1.control[0] = _actuators_fw_in.control[0]; /*roll elevon*/ + _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolAttitudeControl::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolAttitudeControl::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = 1100; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +void +VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + VTOL_att_control::g_control->task_main(); +} + +void VtolAttitudeControl::task_main() +{ + warnx("started"); + fflush(stdout); + + /* do subscriptions */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _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)); + + _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); + _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + + parameters_update(); /*initialize parameter cache/* + + /* wakeup source*/ + struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + + fds[0].fd = _actuator_inputs_mc; + fds[0].events = POLLIN; + fds[1].fd = _actuator_inputs_fw; + fds[1].events = POLLIN; + fds[2].fd = _params_sub; + fds[2].events = POLLIN; + + while (!_task_should_exit) { + /*Advertise/Publish vtol vehicle status*/ + if (_vtol_vehicle_status_pub > 0) { + orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); + + } else { + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); + } + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + if (fds[2].revents & POLLIN) { //parameters were updated, read them now + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + vehicle_manual_poll(); //Check for changes in manual inputs. + arming_status_poll(); //Check for arming status updates. + actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + parameters_update_poll(); + + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + _vtol_vehicle_status.vtol_in_rw_mode = true; + + if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ + set_idle_mc(); + flag_idle_mc = true; + } + + /* got data from mc_att_controller */ + if (fds[0].revents & POLLIN) { + vehicle_manual_poll(); /* update remote input */ + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + + fill_mc_att_control_output(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + + if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + _vtol_vehicle_status.vtol_in_rw_mode = false; + + if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ + set_idle_fw(); + flag_idle_mc = false; + } + + if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); //update remote input + + fill_fw_att_control_output(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + } + + warnx("exit"); + _control_task = -1; + _exit(0); +} + +int +VtolAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("vtol_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +int vtol_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: vtol_att_control {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (VTOL_att_control::g_control != nullptr) { + errx(1, "already running"); + } + + VTOL_att_control::g_control = new VtolAttitudeControl; + + if (VTOL_att_control::g_control == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != VTOL_att_control::g_control->start()) { + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (VTOL_att_control::g_control == nullptr) { + errx(1, "not running"); + } + + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (VTOL_att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} From b168ba92e0be91aec9db5aaccbcc427cfd067c7b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 29 Oct 2014 15:57:45 +0100 Subject: [PATCH 028/122] added setter functions into base class. used when integrating the base class into a gazebo environment --- .../mc_att_control/mc_att_control_base.cpp | 57 +++++++++++++++++++ .../mc_att_control/mc_att_control_base.h | 7 +++ 2 files changed, 64 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d4270b1530..d39bae4dc9 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -281,3 +281,60 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } } + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { + // check if this is consistent !!! + math::Vector<3> quat; + quat(0) = attitude(0); + quat(1) = attitude(1); + quat(2) = attitude(2); + quat(3) = attitude(3); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; + +} + +void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); + +} + +void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = control_attitude_thrust_reference(3); + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d75088b85b..995838bb27 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -166,6 +166,13 @@ protected: void vehicle_attitude_setpoint_poll(); //provisional + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion& attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + + + }; From c51ec3411850d9de0794d4e584838d4a137dafcf Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:50:20 +0100 Subject: [PATCH 029/122] added initialisation of parameters, added assignment of actuator controls, cleaned up --- .../mc_att_control/mc_att_control_base.cpp | 53 ++++++++++++++++--- 1 file changed, 45 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d39bae4dc9..fee1174582 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -51,6 +51,27 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); + + // setup standard gains + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { @@ -282,13 +303,21 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { +void MulticopterAttitudeControlBase::set_actuator_controls() { + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + //_actuators.timestamp = hrt_absolute_time(); +} + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { // check if this is consistent !!! - math::Vector<3> quat; - quat(0) = attitude(0); - quat(1) = attitude(1); - quat(2) = attitude(2); - quat(3) = attitude(3); + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); _v_att.q[0] = quat(0); _v_att.q[1] = quat(1); @@ -322,11 +351,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = control_attitude_thrust_reference(3); + _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; - Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); _v_att_sp.R_body[0][0] = Rot_sp(0,0); _v_att_sp.R_body[1][0] = Rot_sp(1,0); _v_att_sp.R_body[2][0] = Rot_sp(2,0); @@ -338,3 +367,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.R_body[2][2] = Rot_sp(2,2); } + +void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} + From 48cdbf3d0cb8aed7dcdddde872a64096e7c9a595 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:51:12 +0100 Subject: [PATCH 030/122] added more setter and getter functions --- .../mc_att_control/mc_att_control_base.h | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 995838bb27..8f1cf015ea 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,6 +76,8 @@ #include //#include #include +#include + /** @@ -105,6 +107,14 @@ public: * * @return OK on success. */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -161,15 +171,10 @@ protected: /** * Attitude rates controller. */ - void control_attitude(float dt); - void control_attitude_rates(float dt); - + void vehicle_attitude_setpoint_poll(); //provisional - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion& attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + From b5e34eac4faae4b0bc605c6d2d36ed65740c781d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 15:40:22 +0100 Subject: [PATCH 031/122] fixed typo --- src/modules/mc_att_control/mc_att_control_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index fee1174582..b07a1a6be7 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -351,7 +351,7 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; From c9a7850b93643eee7a7ddb145972c4cb36f5db66 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:10:01 +0100 Subject: [PATCH 032/122] do not include eigen lib from local source --- src/lib/mathlib/math/Matrix.hpp | 2 +- src/lib/mathlib/math/Vector.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 34fc4604ac..832be66dbf 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +#include #define M_PI_2_F 1.570769 #endif diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 51a84d27eb..b0b03980d3 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +#include #endif namespace math From f72f0db9963049879eb0dce9a9ef37baadd45dd2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:10:43 +0100 Subject: [PATCH 033/122] include correct eigen lib header --- src/modules/mc_att_control/mc_att_control_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 8f1cf015ea..5a08bba798 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,7 +76,7 @@ #include //#include #include -#include +//#include From a2ec2ec857f5b7584628c2a923c85d0b62b082dd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:58:56 +0100 Subject: [PATCH 034/122] cleaned up --- .../mc_att_control/mc_att_control_base.h | 58 ++----------------- 1 file changed, 6 insertions(+), 52 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 5a08bba798..515fb0c14a 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -1,14 +1,7 @@ -/* - * mc_att_control_base.h - * - * Created on: Sep 25, 2014 - * Author: roman - */ - #ifndef MC_ATT_CONTROL_BASE_H_ #define MC_ATT_CONTROL_BASE_H_ -/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +/* 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 @@ -40,21 +33,10 @@ ****************************************************************************/ /** - * @file mc_att_control_main.cpp - * Multicopter attitude controller. + * @file mc_att_control_base.h + * + * @author Roman Bapst * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * - * The controller has two loops: P loop for angular error and PD loop for angular rate error. - * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. - * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, - * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. - * These two approaches fused seamlessly with weight depending on angular error. - * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. - * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. - * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ #include @@ -74,18 +56,10 @@ #include #include #include -//#include #include -//#include -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f @@ -98,7 +72,7 @@ public: MulticopterAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~MulticopterAttitudeControlBase(); @@ -109,6 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator void set_attitude(const Eigen::Quaternion attitude); void set_attitude_rates(const Eigen::Vector3d& angular_rate); @@ -120,10 +95,6 @@ protected: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - - - - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ @@ -146,8 +117,6 @@ protected: bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - - struct { math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ @@ -161,24 +130,9 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - - /** - * Attitude controller. - */ - //void control_attitude(float dt); - - /** - * Attitude rates controller. - */ void vehicle_attitude_setpoint_poll(); //provisional - - - - - }; #endif /* MC_ATT_CONTROL_BASE_H_ */ From 0acdf5927b9e2dd2cc7375008af2d00cd7f9ba0e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:01:59 +0100 Subject: [PATCH 035/122] cleaned up --- .../mc_att_control/mc_att_control_base.cpp | 44 +++++++++++++++---- 1 file changed, 35 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index b07a1a6be7..baf2bfe65c 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * mc_att_control_base.cpp +/* 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 mc_att_control_base.h + * + * @author Roman Bapst * - * Created on: Sep 25, 2014 - * Author: roman */ #include "mc_att_control_base.h" @@ -308,11 +339,9 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - //_actuators.timestamp = hrt_absolute_time(); } void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - // check if this is consistent !!! math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -336,7 +365,6 @@ void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion Date: Mon, 10 Nov 2014 17:06:14 +0100 Subject: [PATCH 036/122] cleaned up --- .../fw_att_control/fw_att_control_base.cpp | 39 ++++++++++-- .../fw_att_control/fw_att_control_base.h | 59 +++++++++++++------ 2 files changed, 75 insertions(+), 23 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 7218e4e9bb..d8ba159693 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * fw_att_control_base.cpp +/* 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 mc_att_control_base.cpp + * + * @author Roman Bapst * - * Created on: Sep 24, 2014 - * Author: roman */ #include "fw_att_control_base.h" diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6e071fe201..6b2efc46b2 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -1,13 +1,44 @@ -/* - * fw_att_control_base.h - * - * Created on: Sep 24, 2014 - * Author: roman - */ - #ifndef FW_ATT_CONTROL_BASE_H_ #define FW_ATT_CONTROL_BASE_H_ +/* 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 fw_att_control_base.h + * + * @author Roman Bapst + * + */ + #include #include #include @@ -22,8 +53,8 @@ #include #include #include -#include +#include #include class FixedwingAttitudeControlBase @@ -35,7 +66,7 @@ public: FixedwingAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~FixedwingAttitudeControlBase(); @@ -46,8 +77,6 @@ protected: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -107,20 +136,12 @@ protected: } _parameters; /**< local copies of interesting parameters */ - - - ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; void control_attitude(); - - - }; - - #endif /* FW_ATT_CONTROL_BASE_H_ */ From 403fe886845b8e3da150bdc45e47c4e4b29cea96 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:24:48 +0100 Subject: [PATCH 037/122] deleted folder that has found its way into the repo --- build/CATKIN_IGNORE | 0 build/CMakeCache.txt | 450 ------------------ .../CMakeFiles/2.8.12.2/CMakeCCompiler.cmake | 56 --- .../2.8.12.2/CMakeCXXCompiler.cmake | 57 --- .../2.8.12.2/CMakeDetermineCompilerABI_C.bin | Bin 8547 -> 0 bytes .../CMakeDetermineCompilerABI_CXX.bin | Bin 8560 -> 0 bytes build/CMakeFiles/2.8.12.2/CMakeSystem.cmake | 15 - .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c | 389 --------------- build/CMakeFiles/2.8.12.2/CompilerIdC/a.out | Bin 8643 -> 0 bytes .../CompilerIdCXX/CMakeCXXCompilerId.cpp | 377 --------------- build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 8652 -> 0 bytes .../CMakeDirectoryInformation.cmake | 16 - build/CMakeFiles/CMakeError.log | 53 --- build/CMakeFiles/CMakeOutput.log | 293 ------------ build/CMakeFiles/CMakeRuleHashes.txt | 5 - build/CMakeFiles/Makefile.cmake | 150 ------ build/CMakeFiles/Makefile2 | 266 ----------- build/CMakeFiles/TargetDirectories.txt | 6 - .../clean_test_results.dir/DependInfo.cmake | 20 - .../clean_test_results.dir/build.make | 66 --- .../clean_test_results.dir/cmake_clean.cmake | 8 - .../clean_test_results.dir/progress.make | 1 - build/CMakeFiles/cmake.check_cache | 1 - build/CMakeFiles/doxygen.dir/DependInfo.cmake | 20 - build/CMakeFiles/doxygen.dir/build.make | 65 --- .../CMakeFiles/doxygen.dir/cmake_clean.cmake | 8 - build/CMakeFiles/doxygen.dir/progress.make | 1 - build/CMakeFiles/progress.marks | 1 - .../CMakeFiles/run_tests.dir/DependInfo.cmake | 20 - build/CMakeFiles/run_tests.dir/build.make | 65 --- .../run_tests.dir/cmake_clean.cmake | 8 - build/CMakeFiles/run_tests.dir/progress.make | 1 - build/CMakeFiles/tests.dir/DependInfo.cmake | 20 - build/CMakeFiles/tests.dir/build.make | 65 --- build/CMakeFiles/tests.dir/cmake_clean.cmake | 8 - build/CMakeFiles/tests.dir/progress.make | 1 - build/CTestTestfile.cmake | 7 - build/Makefile | 262 ---------- .../catkin_generated/version/package.cmake | 9 - build/catkin_generated/env_cached.sh | 16 - .../catkin_generated/generate_cached_setup.py | 29 -- .../catkin_generated/installspace/.rosinstall | 2 - .../installspace/_setup_util.py | 287 ----------- build/catkin_generated/installspace/env.sh | 16 - .../catkin_generated/installspace/setup.bash | 8 - build/catkin_generated/installspace/setup.sh | 87 ---- build/catkin_generated/installspace/setup.zsh | 8 - build/catkin_generated/order_packages.cmake | 10 - build/catkin_generated/order_packages.py | 5 - build/catkin_generated/setup_cached.sh | 20 - .../Project/interrogate_setup_dot_py.py.stamp | 250 ---------- .../Project/order_packages.cmake.em.stamp | 56 --- .../stamps/Project/package.xml.stamp | 37 -- build/catkin_make.cache | 1 - build/cmake_install.cmake | 140 ------ .../CMakeDirectoryInformation.cmake | 16 - .../CMakeFiles/gtest.dir/DependInfo.cmake | 27 -- build/gtest/CMakeFiles/gtest.dir/build.make | 102 ---- .../CMakeFiles/gtest.dir/cmake_clean.cmake | 10 - build/gtest/CMakeFiles/gtest.dir/depend.make | 2 - build/gtest/CMakeFiles/gtest.dir/flags.make | 8 - build/gtest/CMakeFiles/gtest.dir/link.txt | 1 - .../gtest/CMakeFiles/gtest.dir/progress.make | 2 - .../gtest_main.dir/DependInfo.cmake | 28 -- .../CMakeFiles/gtest_main.dir/build.make | 103 ---- .../gtest_main.dir/cmake_clean.cmake | 10 - .../CMakeFiles/gtest_main.dir/depend.make | 2 - .../CMakeFiles/gtest_main.dir/flags.make | 8 - .../gtest/CMakeFiles/gtest_main.dir/link.txt | 1 - .../CMakeFiles/gtest_main.dir/progress.make | 2 - build/gtest/CMakeFiles/progress.marks | 1 - build/gtest/CTestTestfile.cmake | 6 - build/gtest/Makefile | 262 ---------- build/gtest/cmake_install.cmake | 34 -- 74 files changed, 4387 deletions(-) delete mode 100644 build/CATKIN_IGNORE delete mode 100644 build/CMakeCache.txt delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out delete mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake delete mode 100644 build/CMakeFiles/CMakeError.log delete mode 100644 build/CMakeFiles/CMakeOutput.log delete mode 100644 build/CMakeFiles/CMakeRuleHashes.txt delete mode 100644 build/CMakeFiles/Makefile.cmake delete mode 100644 build/CMakeFiles/Makefile2 delete mode 100644 build/CMakeFiles/TargetDirectories.txt delete mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/clean_test_results.dir/build.make delete mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make delete mode 100644 build/CMakeFiles/cmake.check_cache delete mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/doxygen.dir/build.make delete mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/doxygen.dir/progress.make delete mode 100644 build/CMakeFiles/progress.marks delete mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/run_tests.dir/build.make delete mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/run_tests.dir/progress.make delete mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/tests.dir/build.make delete mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/tests.dir/progress.make delete mode 100644 build/CTestTestfile.cmake delete mode 100644 build/Makefile delete mode 100644 build/catkin/catkin_generated/version/package.cmake delete mode 100755 build/catkin_generated/env_cached.sh delete mode 100644 build/catkin_generated/generate_cached_setup.py delete mode 100644 build/catkin_generated/installspace/.rosinstall delete mode 100755 build/catkin_generated/installspace/_setup_util.py delete mode 100755 build/catkin_generated/installspace/env.sh delete mode 100644 build/catkin_generated/installspace/setup.bash delete mode 100644 build/catkin_generated/installspace/setup.sh delete mode 100644 build/catkin_generated/installspace/setup.zsh delete mode 100644 build/catkin_generated/order_packages.cmake delete mode 100644 build/catkin_generated/order_packages.py delete mode 100755 build/catkin_generated/setup_cached.sh delete mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp delete mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp delete mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp delete mode 100644 build/catkin_make.cache delete mode 100644 build/cmake_install.cmake delete mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt delete mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make delete mode 100644 build/gtest/CMakeFiles/progress.marks delete mode 100644 build/gtest/CTestTestfile.cmake delete mode 100644 build/gtest/Makefile delete mode 100644 build/gtest/cmake_install.cmake diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt deleted file mode 100644 index 8fc5c4b4a8..0000000000 --- a/build/CMakeCache.txt +++ /dev/null @@ -1,450 +0,0 @@ -# This is the CMakeCache file. -# For build in directory: /home/roman/src/Firmware/build -# It was generated by CMake: /usr/bin/cmake -# You can edit this file to change values found and used by cmake. -# If you do not want to change any of the values, simply exit the editor. -# If you do want to change a value, simply edit, save, and exit the editor. -# The syntax for the file is as follows: -# KEY:TYPE=VALUE -# KEY is the name of a variable in the cache. -# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. -# VALUE is the current value for the KEY. - -######################## -# EXTERNAL cache entries -######################## - -//Build shared libraries (DLLs). -BUILD_SHARED_LIBS:BOOL=ON - -//List of ';' separated packages to exclude -CATKIN_BLACKLIST_PACKAGES:STRING= - -//catkin devel space -CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel - -//Catkin enable testing -CATKIN_ENABLE_TESTING:BOOL=ON - -//Catkin skip testing -CATKIN_SKIP_TESTING:BOOL=OFF - -//List of ';' separated packages to build -CATKIN_WHITELIST_PACKAGES:STRING= - -//Path to a program. -CMAKE_AR:FILEPATH=/usr/bin/ar - -//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or -// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel. -CMAKE_BUILD_TYPE:STRING= - -//Enable/Disable color output during build. -CMAKE_COLOR_MAKEFILE:BOOL=ON - -//CXX compiler. -CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ - -//Flags used by the compiler during all build types. -CMAKE_CXX_FLAGS:STRING= - -//Flags used by the compiler during debug builds. -CMAKE_CXX_FLAGS_DEBUG:STRING=-g - -//Flags used by the compiler during release minsize builds. -CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG - -//Flags used by the compiler during release builds (/MD /Ob1 /Oi -// /Ot /Oy /Gs will produce slightly less optimized but smaller -// files). -CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG - -//Flags used by the compiler during Release with Debug Info builds. -CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG - -//C compiler. -CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc - -//Flags used by the compiler during all build types. -CMAKE_C_FLAGS:STRING= - -//Flags used by the compiler during debug builds. -CMAKE_C_FLAGS_DEBUG:STRING=-g - -//Flags used by the compiler during release minsize builds. -CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG - -//Flags used by the compiler during release builds (/MD /Ob1 /Oi -// /Ot /Oy /Gs will produce slightly less optimized but smaller -// files). -CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG - -//Flags used by the compiler during Release with Debug Info builds. -CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG - -//Flags used by the linker. -CMAKE_EXE_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Enable/Disable output of compile commands during generation. -CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF - -//Install path prefix, prepended onto install directories. -CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install - -//Path to a program. -CMAKE_LINKER:FILEPATH=/usr/bin/ld - -//Path to a program. -CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make - -//Flags used by the linker during the creation of modules. -CMAKE_MODULE_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Path to a program. -CMAKE_NM:FILEPATH=/usr/bin/nm - -//Path to a program. -CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy - -//Path to a program. -CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump - -//Value Computed by CMake -CMAKE_PROJECT_NAME:STATIC=Project - -//Path to a program. -CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib - -//Flags used by the linker during the creation of dll's. -CMAKE_SHARED_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//If set, runtime paths are not added when installing shared libraries, -// but are added when building. -CMAKE_SKIP_INSTALL_RPATH:BOOL=NO - -//If set, runtime paths are not added when using shared libraries. -CMAKE_SKIP_RPATH:BOOL=NO - -//Flags used by the linker during the creation of static libraries. -CMAKE_STATIC_LINKER_FLAGS:STRING= - -//Flags used by the linker during debug builds. -CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Path to a program. -CMAKE_STRIP:FILEPATH=/usr/bin/strip - -//If true, cmake will use relative paths in makefiles and projects. -CMAKE_USE_RELATIVE_PATHS:BOOL=OFF - -//If this value is on, makefiles will be generated without the -// .SILENT directive, and all commands will be echoed to the console -// during the make. This is useful for debugging only. With Visual -// Studio IDE projects all commands are done without /nologo. -CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE - -//Path to a program. -DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND - -//Path to a program. -EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy - -//Empy script -EMPY_SCRIPT:STRING=/usr/bin/empy - -//Path to a file. -GTEST_INCLUDE_DIR:PATH=/usr/include - -//Path to a library. -GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND - -//Path to a library. -GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND - -//Path to a library. -GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND - -//Path to a library. -GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND - -//lsb_release executable was found -LSB_FOUND:BOOL=TRUE - -//Path to a program. -LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release - -//Path to a program. -NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7 - -//Path to a program. -PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python - -//Specify specific Python version to use ('major.minor' or 'major') -PYTHON_VERSION:STRING= - -//Value Computed by CMake -Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build - -//Value Computed by CMake -Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src - -//Path to a library. -RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so - -//Enable debian style python package layout -SETUPTOOLS_DEB_LAYOUT:BOOL=ON - -//LSB Distrib tag -UBUNTU:BOOL=TRUE - -//LSB Distrib - codename tag -UBUNTU_TRUSTY:BOOL=TRUE - -//Path to a file. -_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h - -//Path to a file. -_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc - -//The directory containing a CMake configuration file for catkin. -catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake - -//Value Computed by CMake -gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest - -//Dependencies for the target -gtest_LIB_DEPENDS:STATIC=general;-lpthread; - -//Value Computed by CMake -gtest_SOURCE_DIR:STATIC=/usr/src/gtest - -//Build gtest's sample programs. -gtest_build_samples:BOOL=OFF - -//Build all of gtest's own tests. -gtest_build_tests:BOOL=OFF - -//Disable uses of pthreads in gtest. -gtest_disable_pthreads:BOOL=OFF - -//Use shared (DLL) run-time lib even when Google Test is built -// as static lib. -gtest_force_shared_crt:BOOL=OFF - -//Dependencies for the target -gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest; - - -######################## -# INTERNAL cache entries -######################## - -//catkin environment -CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh -CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results -//ADVANCED property for variable: CMAKE_AR -CMAKE_AR-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_BUILD_TOOL -CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1 -//What is the target build tool cmake is generating for. -CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make -//This is the directory where this CMakeCache.txt was created -CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build -//Major version of cmake used to create the current loaded cache -CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 -//Minor version of cmake used to create the current loaded cache -CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 -//Patch version of cmake used to create the current loaded cache -CMAKE_CACHE_PATCH_VERSION:INTERNAL=12 -//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE -CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 -//Path to CMake executable. -CMAKE_COMMAND:INTERNAL=/usr/bin/cmake -//Path to cpack program executable. -CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack -//Path to ctest program executable. -CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest -//ADVANCED property for variable: CMAKE_CXX_COMPILER -CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS -CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG -CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL -CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE -CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO -CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_COMPILER -CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS -CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG -CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL -CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE -CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO -CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//Executable file format -CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS -CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG -CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL -CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE -CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS -CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 -//Name of generator. -CMAKE_GENERATOR:INTERNAL=Unix Makefiles -//Name of generator toolset. -CMAKE_GENERATOR_TOOLSET:INTERNAL= -//Have symbol pthread_create -CMAKE_HAVE_LIBC_CREATE:INTERNAL= -//Have library pthreads -CMAKE_HAVE_PTHREADS_CREATE:INTERNAL= -//Have library pthread -CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1 -//Have include pthread.h -CMAKE_HAVE_PTHREAD_H:INTERNAL=1 -//Start directory with the top level CMakeLists.txt file for this -// project -CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src -//Install .so files without execute permission. -CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 -//ADVANCED property for variable: CMAKE_LINKER -CMAKE_LINKER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MAKE_PROGRAM -CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS -CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG -CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL -CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE -CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_NM -CMAKE_NM-ADVANCED:INTERNAL=1 -//number of local generators -CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2 -//ADVANCED property for variable: CMAKE_OBJCOPY -CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_OBJDUMP -CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_RANLIB -CMAKE_RANLIB-ADVANCED:INTERNAL=1 -//Path to CMake installation. -CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS -CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG -CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL -CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE -CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH -CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SKIP_RPATH -CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS -CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG -CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL -CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE -CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STRIP -CMAKE_STRIP-ADVANCED:INTERNAL=1 -//uname command -CMAKE_UNAME:INTERNAL=/bin/uname -//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS -CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE -CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 -//Details about finding PythonInterp -FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()] -//Details about finding Threads -FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()] -GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE -GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include -GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest -GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest -GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main -//ADVANCED property for variable: GTEST_INCLUDE_DIR -GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_LIBRARY -GTEST_LIBRARY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_LIBRARY_DEBUG -GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_MAIN_LIBRARY -GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG -GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: PYTHON_EXECUTABLE -PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1 -//This needs to be in PYTHONPATH when 'setup.py install' is called. -// And it needs to match. But setuptools won't tell us where -// it will install things. -PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages - diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake deleted file mode 100644 index 83254ce491..0000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake +++ /dev/null @@ -1,56 +0,0 @@ -set(CMAKE_C_COMPILER "/usr/bin/cc") -set(CMAKE_C_COMPILER_ARG1 "") -set(CMAKE_C_COMPILER_ID "GNU") -set(CMAKE_C_COMPILER_VERSION "4.8.2") -set(CMAKE_C_PLATFORM_ID "Linux") - -set(CMAKE_AR "/usr/bin/ar") -set(CMAKE_RANLIB "/usr/bin/ranlib") -set(CMAKE_LINKER "/usr/bin/ld") -set(CMAKE_COMPILER_IS_GNUCC 1) -set(CMAKE_C_COMPILER_LOADED 1) -set(CMAKE_C_COMPILER_WORKS TRUE) -set(CMAKE_C_ABI_COMPILED TRUE) -set(CMAKE_COMPILER_IS_MINGW ) -set(CMAKE_COMPILER_IS_CYGWIN ) -if(CMAKE_COMPILER_IS_CYGWIN) - set(CYGWIN 1) - set(UNIX 1) -endif() - -set(CMAKE_C_COMPILER_ENV_VAR "CC") - -if(CMAKE_COMPILER_IS_MINGW) - set(MINGW 1) -endif() -set(CMAKE_C_COMPILER_ID_RUN 1) -set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) -set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) -set(CMAKE_C_LINKER_PREFERENCE 10) - -# Save compiler ABI information. -set(CMAKE_C_SIZEOF_DATA_PTR "8") -set(CMAKE_C_COMPILER_ABI "ELF") -set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") - -if(CMAKE_C_SIZEOF_DATA_PTR) - set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") -endif() - -if(CMAKE_C_COMPILER_ABI) - set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") -endif() - -if(CMAKE_C_LIBRARY_ARCHITECTURE) - set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") -endif() - - - - -set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") -set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") -set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") - - - diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake deleted file mode 100644 index c4373d570c..0000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake +++ /dev/null @@ -1,57 +0,0 @@ -set(CMAKE_CXX_COMPILER "/usr/bin/c++") -set(CMAKE_CXX_COMPILER_ARG1 "") -set(CMAKE_CXX_COMPILER_ID "GNU") -set(CMAKE_CXX_COMPILER_VERSION "4.8.2") -set(CMAKE_CXX_PLATFORM_ID "Linux") - -set(CMAKE_AR "/usr/bin/ar") -set(CMAKE_RANLIB "/usr/bin/ranlib") -set(CMAKE_LINKER "/usr/bin/ld") -set(CMAKE_COMPILER_IS_GNUCXX 1) -set(CMAKE_CXX_COMPILER_LOADED 1) -set(CMAKE_CXX_COMPILER_WORKS TRUE) -set(CMAKE_CXX_ABI_COMPILED TRUE) -set(CMAKE_COMPILER_IS_MINGW ) -set(CMAKE_COMPILER_IS_CYGWIN ) -if(CMAKE_COMPILER_IS_CYGWIN) - set(CYGWIN 1) - set(UNIX 1) -endif() - -set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") - -if(CMAKE_COMPILER_IS_MINGW) - set(MINGW 1) -endif() -set(CMAKE_CXX_COMPILER_ID_RUN 1) -set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) -set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) -set(CMAKE_CXX_LINKER_PREFERENCE 30) -set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) - -# Save compiler ABI information. -set(CMAKE_CXX_SIZEOF_DATA_PTR "8") -set(CMAKE_CXX_COMPILER_ABI "ELF") -set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") - -if(CMAKE_CXX_SIZEOF_DATA_PTR) - set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") -endif() - -if(CMAKE_CXX_COMPILER_ABI) - set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") -endif() - -if(CMAKE_CXX_LIBRARY_ARCHITECTURE) - set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") -endif() - - - - -set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") -set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") -set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") - - - diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin deleted file mode 100755 index 2f2ebe478334290d81ee498f150d9bab78baa7e7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8547 zcmeHMeQaCR6+iaNheH#`?FyunQE#JLV9Fycp)(*x9M|b{>$F{)z_wy;o%kg&_@lF* zOUufr$f#yP8T%0HDug=3ANhm*Ip{Vt(gjhZF{x}~LhB|Wx@>J}M-d&05Y3&=edoNq zJUbH;5`XnqHP`pNbM86!5EjE%9;57GjC|=D`-)<{8d7?<2X>>K_cr1l1>e_XD&&2UQO zMQok1GWgq~hOB_yFD`qCk4u*O#*CFgJ3lEx#JG+0EjBI}sDE5i6>iF9hu8Jol*@GG zvW3c?u04@;UF&-Ea#3HaY{K@Dcf;l(rHRL}gT`e!fLa6i{rvhPFK(P%(X%PF@1?gk zKJ|y5ANiia^@06Cxn^9Cq5T2)!c|KG4GnESXnb%yRRkom*1ZH@xIpE zezxTKO{b>*`o~kxtQ|gi{;ltP^V<6!KD_VO2j362E`R5lqaBS$-~ID@G@G^Pa%O%Z zmH@!Ag(QW-BK*Hxgnn!h`nOeIIv^6V*CiuO^iB``X+>WFy&n3AYQLj~{~-Wa3hmHK z>xtLRhyTuT_*SM`EijrDQ-c;;;>rCfa&>uB}G z=M&X8?lr3K7!&`vuyrtTd;OL#9oiJo{;d(SC zW`jm`<^g|d`Y0+u!!D*?#!vF?_Er0lb`)#Oe{5DWsxwCQ_r|facN)GE#v8L0e`@+S zw(y^>3>|BQ({UTX>iSl2lw zEFXF~9NY;;bw-{@o%ybmvV9etDN8A_jp{?^jB4fWtpeA#-a8X_T%zz|J{@? z{xMTb7vc|ST!5$Aw_vpZBsTYNxy|K9JGvt&o0uNX?uaM*Qvkk|+U?!6kDt3F+6uo$p82P`!ihQ+&_k@Bf*TMPxZd_a?scO*Po(RKDxX()TNBfJLR?K{fX=Z@ztCvg!*;Vix`sOSIcfLj1o2C%^LSpM`Xqh- ztibnJFLqNu4|vY)j195aZQ;(L;Yz`(gnRUezP4-4trb;WbAwF)(93)CR(crpvQ@In zasHJpSY~M#=!K$X>e0SLmz5r)+E}5Y4_C6eOjkAosy3c3kAt4sTPW|%+oe^qzw9x%Iux<|7-zeDi*g#Ah~556BV)_CMC@;i$f#B-G!UU@t(eJEv-^|N0eA_Cuf+0WUZ z7@Gl&(%B*5eEwV&}Iy_>K+$DIo#&;4Zm-u~ll0lUib9LE9_NICAIJjQmf z{1Y^;9CtbHM~J}joZ+4Ssz-h&#g&~N{S1De;_{~+d4898lH{NDOc={C4twNzpX#BG za=d1E?LR{DJbwI+(o6Dq{dL2uf6^n*?|J;r$M1Ozul^T2@{{xqGD-R`vLJ!yk1>T; z9oLr}sdX=Y5OcW%>P; z*C*@edHAPCKfi;_Vy9f|ox$>4zUz_aa|0{rWI+PUaf$PTOVqKS&K;d(Alu1cImSn1 zBg^yqTZ9C?{2q4zN&4B(87%LO-z?81KL4|BH|}Bm%t-K5Rh@mujnfpkx@qF@T@Y>L z{%~AF|E#CKW0l?7CQsvD{r?LZ_?-RJ4GsLxKIw)AnzR48p+SSQueqT?qqASRp+S?g z54oX1v$Marp+UgeH{8&m#n}(s(BKLu&by)2d5=YC(CWlxH#BH-;;kE6z4utv&ip&G z8ybY1xaWojOPzS;h6c-=IOT>0pL61m8yYNk;))v@T-L2Z6x0C&2 zRMBh4^B}du_d&+b6@Tr#;A>~Se&N31gLc5@O9UrgP=7uCHx>Udem42+{D?Q-vBSri#)Wu5 z(QETKkNE67jyFH@6CZ9xwfUpNN^fo6=w_b0~di`aRR?BVkepIgpSf4zC7 zk5v1$c`Hf$e6C|O`LSQ~>z1+1Uq9b;wZHy&esvLgT(wi1pMsH+RkkXlqxuL4u`#wy zBoo^Qg@F0=k+AY&1QSSQ;QWn?Bt)ht#&X5sbWUWfVyP_Bl|3+0%f_*v7RPy<~An2;9YAVmPfHgLdzTb?+iuqmHoLP$YCG-(HwUr*u zf)M@Nq62Xe-@G2PSE^h58*j%>OPIFGl#60KUC8825bM9SIXaMtfpD{_L>mY(I1uAj zHYB(7MU!GnfB*LQpcstyCF7_^<>fw67S5GxTQnhOr}DhXzTRGUzU)(H?YMLE$vZov M_a>@?ti0lyfbasU7T diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin deleted file mode 100755 index 16c737f2604406275900604b68676d59840e9fbb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8560 zcmeHMZ){w}5udx~e@E=QO9-t=()d~u)qwo4Lr4t~jnC(E-lgY+IyR(*%jWDm#~1F8 zd-o3PkU~^9sHYGbRYgss3Zbe}^MOiyqO?>csS!bls(}_nOOc`kS4jveh?H6t@m8~M z=K1;ErJ_Qr`XRq4-rL!k+1amof)H4|IbM zdSErIga!bpS2PPfB*EnzsphhY_%r~h1AuO?Md4bk|x?6b_U zOXNk|HEX8ucONP0YqG}0<$B`dlI6ZWYo^f7Pm3Ti4pM(t`Ys>P__(Ah+>*_VY*>Fw zHr0{Ml3C;pDD^j($%s5OA?>sLPZ;2&=I&ZfP- z=XbU~@$T5sWa2cg4;&B5H3Gol;Ql7~;&NY8U0uuf>YxYPaH_GN-sl?__xK`jK7aNH z`AWsOZ;2kBdI2^tV>=X8~X-1RzN3 ziPz0TYFsUFxY1f?Y3lfHqO%`h9HxHVcH+D)gBBPdJsU~jfTAyfAo&x<7l1lNaSuI_1=!$!AOu={yogN+lNG z@_-nmz9QLVsgy2RCe$=wb3E1?5$p7I`fX}(JaW$!{#meEt@Vy5ugLgtcf3OFa{+7J z;J(1U!)T@TfqRI1LWCT2C%-X$vB@}C|Bf#N#)0Ez{kfX~z?goqX~xnhZ+qPr0=f22 zz7Pc3dO)tl`ZzY8KP{z{YhS@SZvFG8q?BFn$11PwG%9C|>32W4YbbW|IL^3n@&qn8 zzJ|zKrQPzfqZe(3_Zf4m&`DE+t;TCV4v0wjXKF zVU77u%xOkt)~Ni>IB{W<;W=fzHdpp0W`1o6ziwVi7?qbS`7CW^jDQ^qq4JApmo=ZNl>57efABU{q%1sl@q*q?Ca+IA4-dqK?Y9 zFB+Bd>66h7fxZ6V_q|bn z>*HF~A39VQ^|wD(Z}>YOZ7}@Z(~SfEo~*w+?C%Ww+k5??UVm$^KiKPUQg%KK0LSK~ zbndsSxQeT|ivM?n=>7&=68PTFWi!$FUe`i(zPDY^?etz+Np*f7t)O}oAkM zskS>`)jY^o!Z0#zGNmc~I(P322dIjb~_R?7qFVkuL|FRF+l=*4t4i30R%B5Q)K zF7LXTo-{#M&jh_#P;UV}Jub$I$y{2Dr-}k7QpXA zMwd(8qS&G}-5d6B_vJ z^S}uWuCe{R6IzXUEJA~3+b=tzL5uBgozQBKRomfL>C2@S5Z{e=@6e9ra*PH1@Br~T=KRwJ&C(BOJ|KRTgRor=~zsDlr2 zJ1zpAxt<#K<2%ZW@p;!hTqpW*7d=dL_xtT0rGLRr-XCL%Uj01}kskX!Ncgehubvl- zj@BC2Da9WE41gl|9cKCC0ALst!ATjktCpURl%Az9?z8wAn7XX>-4M0(w5WkEt6D#+ zm7Z4Ubj|zCie9~b){`DSM;V(*5Bq1vc15pVR~e6$H*x1draYX1T#P3ay*j`1ln0NZ z>ip5qmHz5{$}beX8n2QbKA#wWzJfjHJhk&m|D4=JBjbuP7EqN)%htHEt(~>JT|6}f)HCGgCZW=J|qMzq?Lr36Qh_| zDgh^Y93&x91+hC@7)fSD$}AL1B3Yh<(L!z_n>N!aeZ%cvSy!7$<*=-$rby?_;uKhq z(^NT^n*u>kRaH|7sYR@jf#m(^$c`P6LT)0HO&7zxF@1Dm0>n4tT^lm_v4U^}^O4%E zh{=%*2+=&vsX_B^d^_9yVD>9jDv0r9K7|>zzHe*|55yuMoNOvR2SN-D zL|Dt___p3~Tx{#_-yR(jL*d?d6#G%xyib+8bEfPljc4Wi+!?{19%q*AGw1qvEDI?= OEA9AS<^8PW;lBYd8IS4! diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake deleted file mode 100644 index 0616b7c1ee..0000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake +++ /dev/null @@ -1,15 +0,0 @@ -set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic") -set(CMAKE_HOST_SYSTEM_NAME "Linux") -set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic") -set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") - - - -set(CMAKE_SYSTEM "Linux-3.13.0-24-generic") -set(CMAKE_SYSTEM_NAME "Linux") -set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic") -set(CMAKE_SYSTEM_PROCESSOR "x86_64") - -set(CMAKE_CROSSCOMPILING "FALSE") - -set(CMAKE_SYSTEM_LOADED 1) diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c deleted file mode 100644 index cba81d4a6b..0000000000 --- a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c +++ /dev/null @@ -1,389 +0,0 @@ -#ifdef __cplusplus -# error "A C++ compiler has been selected for C." -#endif - -/* Version number components: V=Version, R=Revision, P=Patch - Version date components: YYYY=Year, MM=Month, DD=Day */ - -#if defined(__18CXX) -# define ID_VOID_MAIN -#endif - -#if defined(__INTEL_COMPILER) || defined(__ICC) -# define COMPILER_ID "Intel" - /* __INTEL_COMPILER = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) -# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) -# if defined(__INTEL_COMPILER_BUILD_DATE) - /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ -# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) -# endif - -#elif defined(__PATHCC__) -# define COMPILER_ID "PathScale" -# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) -# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) -# if defined(__PATHCC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) -# endif - -#elif defined(__clang__) -# define COMPILER_ID "Clang" -# define COMPILER_VERSION_MAJOR DEC(__clang_major__) -# define COMPILER_VERSION_MINOR DEC(__clang_minor__) -# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) - -#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) -# define COMPILER_ID "Embarcadero" -# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) -# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) -# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) - -#elif defined(__BORLANDC__) -# define COMPILER_ID "Borland" - /* __BORLANDC__ = 0xVRR */ -# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) -# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) - -#elif defined(__WATCOMC__) -# define COMPILER_ID "Watcom" - /* __WATCOMC__ = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) -# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) - -#elif defined(__SUNPRO_C) -# define COMPILER_ID "SunPro" -# if __SUNPRO_C >= 0x5100 - /* __SUNPRO_C = 0xVRRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) -# else - /* __SUNPRO_C = 0xVRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) -# endif - -#elif defined(__HP_cc) -# define COMPILER_ID "HP" - /* __HP_cc = VVRRPP */ -# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) -# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) -# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) - -#elif defined(__DECC) -# define COMPILER_ID "Compaq" - /* __DECC_VER = VVRRTPPPP */ -# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) -# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) -# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) - -#elif defined(__IBMC__) -# if defined(__COMPILER_VER__) -# define COMPILER_ID "zOS" -# else -# if __IBMC__ >= 800 -# define COMPILER_ID "XL" -# else -# define COMPILER_ID "VisualAge" -# endif - /* __IBMC__ = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) -# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) -# endif - -#elif defined(__PGI) -# define COMPILER_ID "PGI" -# define COMPILER_VERSION_MAJOR DEC(__PGIC__) -# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) -# if defined(__PGIC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) -# endif - -#elif defined(_CRAYC) -# define COMPILER_ID "Cray" -# define COMPILER_VERSION_MAJOR DEC(_RELEASE) -# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) - -#elif defined(__TI_COMPILER_VERSION__) -# define COMPILER_ID "TI" - /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ -# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) -# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) -# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) - -#elif defined(__TINYC__) -# define COMPILER_ID "TinyCC" - -#elif defined(__SCO_VERSION__) -# define COMPILER_ID "SCO" - -#elif defined(__GNUC__) -# define COMPILER_ID "GNU" -# define COMPILER_VERSION_MAJOR DEC(__GNUC__) -# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) -# if defined(__GNUC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) -# endif - -#elif defined(_MSC_VER) -# define COMPILER_ID "MSVC" - /* _MSC_VER = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) -# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) -# if defined(_MSC_FULL_VER) -# if _MSC_VER >= 1400 - /* _MSC_FULL_VER = VVRRPPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) -# else - /* _MSC_FULL_VER = VVRRPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) -# endif -# endif -# if defined(_MSC_BUILD) -# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) -# endif - -/* Analog VisualDSP++ >= 4.5.6 */ -#elif defined(__VISUALDSPVERSION__) -# define COMPILER_ID "ADSP" - /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ -# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) -# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) - -/* Analog VisualDSP++ < 4.5.6 */ -#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) -# define COMPILER_ID "ADSP" - -/* IAR Systems compiler for embedded systems. - http://www.iar.com */ -#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) -# define COMPILER_ID "IAR" - -/* sdcc, the small devices C compiler for embedded systems, - http://sdcc.sourceforge.net */ -#elif defined(SDCC) -# define COMPILER_ID "SDCC" - /* SDCC = VRP */ -# define COMPILER_VERSION_MAJOR DEC(SDCC/100) -# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) -# define COMPILER_VERSION_PATCH DEC(SDCC % 10) - -#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) -# define COMPILER_ID "MIPSpro" -# if defined(_SGI_COMPILER_VERSION) - /* _SGI_COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) -# else - /* _COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) -# endif - -/* This compiler is either not known or is too old to define an - identification macro. Try to identify the platform and guess that - it is the native compiler. */ -#elif defined(__sgi) -# define COMPILER_ID "MIPSpro" - -#elif defined(__hpux) || defined(__hpua) -# define COMPILER_ID "HP" - -#else /* unknown compiler */ -# define COMPILER_ID "" - -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; - -/* Identify known platforms by name. */ -#if defined(__linux) || defined(__linux__) || defined(linux) -# define PLATFORM_ID "Linux" - -#elif defined(__CYGWIN__) -# define PLATFORM_ID "Cygwin" - -#elif defined(__MINGW32__) -# define PLATFORM_ID "MinGW" - -#elif defined(__APPLE__) -# define PLATFORM_ID "Darwin" - -#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) -# define PLATFORM_ID "Windows" - -#elif defined(__FreeBSD__) || defined(__FreeBSD) -# define PLATFORM_ID "FreeBSD" - -#elif defined(__NetBSD__) || defined(__NetBSD) -# define PLATFORM_ID "NetBSD" - -#elif defined(__OpenBSD__) || defined(__OPENBSD) -# define PLATFORM_ID "OpenBSD" - -#elif defined(__sun) || defined(sun) -# define PLATFORM_ID "SunOS" - -#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) -# define PLATFORM_ID "AIX" - -#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) -# define PLATFORM_ID "IRIX" - -#elif defined(__hpux) || defined(__hpux__) -# define PLATFORM_ID "HP-UX" - -#elif defined(__HAIKU__) -# define PLATFORM_ID "Haiku" - -#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) -# define PLATFORM_ID "BeOS" - -#elif defined(__QNX__) || defined(__QNXNTO__) -# define PLATFORM_ID "QNX" - -#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) -# define PLATFORM_ID "Tru64" - -#elif defined(__riscos) || defined(__riscos__) -# define PLATFORM_ID "RISCos" - -#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) -# define PLATFORM_ID "SINIX" - -#elif defined(__UNIX_SV__) -# define PLATFORM_ID "UNIX_SV" - -#elif defined(__bsdos__) -# define PLATFORM_ID "BSDOS" - -#elif defined(_MPRAS) || defined(MPRAS) -# define PLATFORM_ID "MP-RAS" - -#elif defined(__osf) || defined(__osf__) -# define PLATFORM_ID "OSF1" - -#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) -# define PLATFORM_ID "SCO_SV" - -#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) -# define PLATFORM_ID "ULTRIX" - -#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) -# define PLATFORM_ID "Xenix" - -#else /* unknown platform */ -# define PLATFORM_ID "" - -#endif - -/* For windows compilers MSVC and Intel we can determine - the architecture of the compiler being used. This is because - the compilers do not have flags that can change the architecture, - but rather depend on which compiler is being used -*/ -#if defined(_WIN32) && defined(_MSC_VER) -# if defined(_M_IA64) -# define ARCHITECTURE_ID "IA64" - -# elif defined(_M_X64) || defined(_M_AMD64) -# define ARCHITECTURE_ID "x64" - -# elif defined(_M_IX86) -# define ARCHITECTURE_ID "X86" - -# elif defined(_M_ARM) -# define ARCHITECTURE_ID "ARM" - -# elif defined(_M_MIPS) -# define ARCHITECTURE_ID "MIPS" - -# elif defined(_M_SH) -# define ARCHITECTURE_ID "SHx" - -# else /* unknown architecture */ -# define ARCHITECTURE_ID "" -# endif - -#else -# define ARCHITECTURE_ID "" -#endif - -/* Convert integer to decimal digit literals. */ -#define DEC(n) \ - ('0' + (((n) / 10000000)%10)), \ - ('0' + (((n) / 1000000)%10)), \ - ('0' + (((n) / 100000)%10)), \ - ('0' + (((n) / 10000)%10)), \ - ('0' + (((n) / 1000)%10)), \ - ('0' + (((n) / 100)%10)), \ - ('0' + (((n) / 10)%10)), \ - ('0' + ((n) % 10)) - -/* Convert integer to hex digit literals. */ -#define HEX(n) \ - ('0' + ((n)>>28 & 0xF)), \ - ('0' + ((n)>>24 & 0xF)), \ - ('0' + ((n)>>20 & 0xF)), \ - ('0' + ((n)>>16 & 0xF)), \ - ('0' + ((n)>>12 & 0xF)), \ - ('0' + ((n)>>8 & 0xF)), \ - ('0' + ((n)>>4 & 0xF)), \ - ('0' + ((n) & 0xF)) - -/* Construct a string literal encoding the version number components. */ -#ifdef COMPILER_VERSION_MAJOR -char const info_version[] = { - 'I', 'N', 'F', 'O', ':', - 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', - COMPILER_VERSION_MAJOR, -# ifdef COMPILER_VERSION_MINOR - '.', COMPILER_VERSION_MINOR, -# ifdef COMPILER_VERSION_PATCH - '.', COMPILER_VERSION_PATCH, -# ifdef COMPILER_VERSION_TWEAK - '.', COMPILER_VERSION_TWEAK, -# endif -# endif -# endif - ']','\0'}; -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; -char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; - - - -/*--------------------------------------------------------------------------*/ - -#ifdef ID_VOID_MAIN -void main() {} -#else -int main(int argc, char* argv[]) -{ - int require = 0; - require += info_compiler[argc]; - require += info_platform[argc]; - require += info_arch[argc]; -#ifdef COMPILER_VERSION_MAJOR - require += info_version[argc]; -#endif - (void)argv; - return require; -} -#endif diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out deleted file mode 100755 index 33a3d2b580160e3c32c235a17c6d2c6dc266b182..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8643 zcmeHMZ){sv6+iY%nsiBRr?il^uvlY7S+%^iSytRlqjp@U&y|ywG+8&b>5UURiJAWt z`*}+SgF@IOE9+JXgtEbheL$i}d_<-2fwg2>KLAo%4QU9F%0#NRYo(%Xptym>4Vzg$uO2&Kdc-64^VysR6mycEHiA0X%UAO z%oKi`Sg4X#y-bo_mfDug`|5(3BFZO1fKv5Sd8@U{ZK@wj3c_RA%*gJp$Fix8Y$jix z?3fJg?%3U>mkN5PvI+CYwEO!8RY}~At<)}?0Mr=3`0u=RxufOa;FsR~)zp0H+TQP; z?)%Zp$mRe41x*03u?FD_c8|J{&5Vqq*}Fjq{1zoMSu{m1naP6?Phk8Z-5= z<{%iaoHy&QJq`fI^t+97%(Nh%=xh#x+}_n31b+@2| zUAN+uOUI4MhsN~ZZyy?nUpzlw9|q&%g<&Nw-l_}3wO>jp?~M7|_5qwb5*UU-qvKFu z7>wyfpHW$O(K|GE4hulT?hL(yQSyqv?Ih~XVTjXTSk#Qlf>HUcapA^Z!!vJOS}c2q z=6-FFFPnFUjLLf!|3l)ReEMfiL4b10pix=4_AM#p(0lbiYzl(s<2kIKG5z$j>w^I1 zgSH8q2Hy$#hQX*T$oZk`-;h%J+u(XhN{M+?UcPBm%2zJN?x|8|Hg`4!VJ3Fdm^D1} z{%x@7Ozhs!+}P>Jkw`^Go{3lPMh4@RKSl-; zr?$^F24T=R)q(PG;*o`2e>N`M*RY4LZC;Ync;%M)pug>VXdfghHxiY*`zkUbH~qz! zp7R)cJ}Te9jz2mSITjg;9FK^(u`!*~e>{i7;$|Zc4bO^ERE>CF&%q~03%L`SY`S=K zf8XHH$}}tD#S7_TDO1QF-D#mqr;iYSbPidJ6WOFWRw(9LjqP>p$_kN(4b zDZh@2h%fM}H@ab+H{0CMXvL+_rz}aC#`tbu;0K@udXwE zq3MQRUpVUvMSMFWzV>cku-n(#?F)4K8kLpRcx2n9oOBDcnluDL&_M{6Cju0h7$qxye5BQF(mU zIY;piP<|njcn(0|c>}STBm>W%3bfzmwO8#4V0phm$4U4cC7-17SwBo+wwudaPw6?l zB(3^{DZQjHx94pNhbaEj0-s~uxJq{1UCkakkM@%sC%HcweKOcSI8x4=oM6a1CQo9W^S(DMZ|tw*}!9cFTzLgV?eK2pwPQyrNU zsL({RGy!^QDqot)S<)<8F|<)#5fDYti|K3<3G{U$Yl5E1XH3w|^rQ*;SSFtVy;w*k z%_QjQ2{Beo=F(y!Ra_2P43R7rlT#LhzrQwGRAwY|nNiTk3#R(9O0jYrDV0DUE#z|P zyyE%vd~ScY{fw9BK85-0kA{iQ_2ly1 z^~X~t_6hUZzfBUK{UP$v=ezl@Q@e6Kc|ABoeD?DUcmG#h{CV;>^RDt4d~e0_2QEI} zTXI2{T^+_e##=5vuV3#|gRviHxb1(N_}qS1X#KlF{O>c7!2QWMe|j*L)x7PVWx?9sv9fKCkDkbYJRD?+$-J_EUEQ z-rOL9pOF7ox}Rk}uV;K;%lEQ8KDm4z2UlI?^L^bSR?0En8O&$-k&Dm!2`-?Wi3H}c z#Qwn{YTHlyk#;h0H9q5CWFzzOx(#uN_*@RdH61`w`8>}V%y;{5=Cj0S0WRAKlgsCX z1XoandG4$LZ6JrKnY0zM=FHUICXs;ViXt3U151i280o%_zp+S@Fzn##a z+4jp$Xt?Wlof8lm_-sGwga&@wKRTg7!1jAiXwYK&Gbc3IX!|KAH293|f1J=@lkHcW z&|tIeFPzZev$h{_Lc>!4%`Yc3Xa%0PPH3>jo{vswRjZ;k59;6_I2~62*IZ8x`*C`B zVFT=RwZBgKVV6EadiS~Rh{}K2PM#lQN?+ZcrzjsjCo8tw%Z{=&%3(Kjc|DDRG z1!fzpb0p(WN?*GlYvr?`h#RZM!(Ek6b-!B;Uv2+J54L}GzqfnvooNH$YYBqgOvuA8 zhbu9l^wr};cyOs`tr{m|D*x(vlvVm_%uqh=aX7n*JwNpTw0K)#r)xa_0`-o0%KMEu zrEi5WrRR0!614+97a`PLH~{=j*|P)AR(!2e}Ey_RS%KzV#53g&C2Cvor_&$YD zcl5YN_113xpx3IeJD*2aN#CRLul9!t%m@5{4?T!Z^$YNv(zk-)vL8l^X2~p%jp?Hx z#DQqPNW>2h2mz^dF+H9sndzc1b7C}G$frwCLnTD2AjY$Wkz`h+%tEmwlI2Oj7n^L_ zOsDkSd%oORJCVS{taUUM>AYE-0_!lFD(7-jAn0hTLITggE0RWglh3E4QF<7PrdKIkU7nR};tmfEx}wpKOJ25?4O$3Xv)m#6&Wm z%BDfY4~mkR%H&14lukh=KUNU@pyf1FT^xus*4L_<>24 & 0x00FF) -# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) -# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) - -#elif defined(__BORLANDC__) -# define COMPILER_ID "Borland" - /* __BORLANDC__ = 0xVRR */ -# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) -# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) - -#elif defined(__WATCOMC__) -# define COMPILER_ID "Watcom" - /* __WATCOMC__ = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) -# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) - -#elif defined(__SUNPRO_CC) -# define COMPILER_ID "SunPro" -# if __SUNPRO_CC >= 0x5100 - /* __SUNPRO_CC = 0xVRRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) -# else - /* __SUNPRO_CC = 0xVRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) -# endif - -#elif defined(__HP_aCC) -# define COMPILER_ID "HP" - /* __HP_aCC = VVRRPP */ -# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) -# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) -# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) - -#elif defined(__DECCXX) -# define COMPILER_ID "Compaq" - /* __DECCXX_VER = VVRRTPPPP */ -# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) -# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) -# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) - -#elif defined(__IBMCPP__) -# if defined(__COMPILER_VER__) -# define COMPILER_ID "zOS" -# else -# if __IBMCPP__ >= 800 -# define COMPILER_ID "XL" -# else -# define COMPILER_ID "VisualAge" -# endif - /* __IBMCPP__ = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) -# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) -# endif - -#elif defined(__PGI) -# define COMPILER_ID "PGI" -# define COMPILER_VERSION_MAJOR DEC(__PGIC__) -# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) -# if defined(__PGIC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) -# endif - -#elif defined(_CRAYC) -# define COMPILER_ID "Cray" -# define COMPILER_VERSION_MAJOR DEC(_RELEASE) -# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) - -#elif defined(__TI_COMPILER_VERSION__) -# define COMPILER_ID "TI" - /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ -# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) -# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) -# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) - -#elif defined(__SCO_VERSION__) -# define COMPILER_ID "SCO" - -#elif defined(__GNUC__) -# define COMPILER_ID "GNU" -# define COMPILER_VERSION_MAJOR DEC(__GNUC__) -# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) -# if defined(__GNUC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) -# endif - -#elif defined(_MSC_VER) -# define COMPILER_ID "MSVC" - /* _MSC_VER = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) -# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) -# if defined(_MSC_FULL_VER) -# if _MSC_VER >= 1400 - /* _MSC_FULL_VER = VVRRPPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) -# else - /* _MSC_FULL_VER = VVRRPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) -# endif -# endif -# if defined(_MSC_BUILD) -# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) -# endif - -/* Analog VisualDSP++ >= 4.5.6 */ -#elif defined(__VISUALDSPVERSION__) -# define COMPILER_ID "ADSP" - /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ -# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) -# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) - -/* Analog VisualDSP++ < 4.5.6 */ -#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) -# define COMPILER_ID "ADSP" - -/* IAR Systems compiler for embedded systems. - http://www.iar.com */ -#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) -# define COMPILER_ID "IAR" - -#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) -# define COMPILER_ID "MIPSpro" -# if defined(_SGI_COMPILER_VERSION) - /* _SGI_COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) -# else - /* _COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) -# endif - -/* This compiler is either not known or is too old to define an - identification macro. Try to identify the platform and guess that - it is the native compiler. */ -#elif defined(__sgi) -# define COMPILER_ID "MIPSpro" - -#elif defined(__hpux) || defined(__hpua) -# define COMPILER_ID "HP" - -#else /* unknown compiler */ -# define COMPILER_ID "" - -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; - -/* Identify known platforms by name. */ -#if defined(__linux) || defined(__linux__) || defined(linux) -# define PLATFORM_ID "Linux" - -#elif defined(__CYGWIN__) -# define PLATFORM_ID "Cygwin" - -#elif defined(__MINGW32__) -# define PLATFORM_ID "MinGW" - -#elif defined(__APPLE__) -# define PLATFORM_ID "Darwin" - -#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) -# define PLATFORM_ID "Windows" - -#elif defined(__FreeBSD__) || defined(__FreeBSD) -# define PLATFORM_ID "FreeBSD" - -#elif defined(__NetBSD__) || defined(__NetBSD) -# define PLATFORM_ID "NetBSD" - -#elif defined(__OpenBSD__) || defined(__OPENBSD) -# define PLATFORM_ID "OpenBSD" - -#elif defined(__sun) || defined(sun) -# define PLATFORM_ID "SunOS" - -#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) -# define PLATFORM_ID "AIX" - -#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) -# define PLATFORM_ID "IRIX" - -#elif defined(__hpux) || defined(__hpux__) -# define PLATFORM_ID "HP-UX" - -#elif defined(__HAIKU__) -# define PLATFORM_ID "Haiku" - -#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) -# define PLATFORM_ID "BeOS" - -#elif defined(__QNX__) || defined(__QNXNTO__) -# define PLATFORM_ID "QNX" - -#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) -# define PLATFORM_ID "Tru64" - -#elif defined(__riscos) || defined(__riscos__) -# define PLATFORM_ID "RISCos" - -#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) -# define PLATFORM_ID "SINIX" - -#elif defined(__UNIX_SV__) -# define PLATFORM_ID "UNIX_SV" - -#elif defined(__bsdos__) -# define PLATFORM_ID "BSDOS" - -#elif defined(_MPRAS) || defined(MPRAS) -# define PLATFORM_ID "MP-RAS" - -#elif defined(__osf) || defined(__osf__) -# define PLATFORM_ID "OSF1" - -#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) -# define PLATFORM_ID "SCO_SV" - -#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) -# define PLATFORM_ID "ULTRIX" - -#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) -# define PLATFORM_ID "Xenix" - -#else /* unknown platform */ -# define PLATFORM_ID "" - -#endif - -/* For windows compilers MSVC and Intel we can determine - the architecture of the compiler being used. This is because - the compilers do not have flags that can change the architecture, - but rather depend on which compiler is being used -*/ -#if defined(_WIN32) && defined(_MSC_VER) -# if defined(_M_IA64) -# define ARCHITECTURE_ID "IA64" - -# elif defined(_M_X64) || defined(_M_AMD64) -# define ARCHITECTURE_ID "x64" - -# elif defined(_M_IX86) -# define ARCHITECTURE_ID "X86" - -# elif defined(_M_ARM) -# define ARCHITECTURE_ID "ARM" - -# elif defined(_M_MIPS) -# define ARCHITECTURE_ID "MIPS" - -# elif defined(_M_SH) -# define ARCHITECTURE_ID "SHx" - -# else /* unknown architecture */ -# define ARCHITECTURE_ID "" -# endif - -#else -# define ARCHITECTURE_ID "" -#endif - -/* Convert integer to decimal digit literals. */ -#define DEC(n) \ - ('0' + (((n) / 10000000)%10)), \ - ('0' + (((n) / 1000000)%10)), \ - ('0' + (((n) / 100000)%10)), \ - ('0' + (((n) / 10000)%10)), \ - ('0' + (((n) / 1000)%10)), \ - ('0' + (((n) / 100)%10)), \ - ('0' + (((n) / 10)%10)), \ - ('0' + ((n) % 10)) - -/* Convert integer to hex digit literals. */ -#define HEX(n) \ - ('0' + ((n)>>28 & 0xF)), \ - ('0' + ((n)>>24 & 0xF)), \ - ('0' + ((n)>>20 & 0xF)), \ - ('0' + ((n)>>16 & 0xF)), \ - ('0' + ((n)>>12 & 0xF)), \ - ('0' + ((n)>>8 & 0xF)), \ - ('0' + ((n)>>4 & 0xF)), \ - ('0' + ((n) & 0xF)) - -/* Construct a string literal encoding the version number components. */ -#ifdef COMPILER_VERSION_MAJOR -char const info_version[] = { - 'I', 'N', 'F', 'O', ':', - 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', - COMPILER_VERSION_MAJOR, -# ifdef COMPILER_VERSION_MINOR - '.', COMPILER_VERSION_MINOR, -# ifdef COMPILER_VERSION_PATCH - '.', COMPILER_VERSION_PATCH, -# ifdef COMPILER_VERSION_TWEAK - '.', COMPILER_VERSION_TWEAK, -# endif -# endif -# endif - ']','\0'}; -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; -char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; - - - -/*--------------------------------------------------------------------------*/ - -int main(int argc, char* argv[]) -{ - int require = 0; - require += info_compiler[argc]; - require += info_platform[argc]; -#ifdef COMPILER_VERSION_MAJOR - require += info_version[argc]; -#endif - (void)argv; - return require; -} diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out deleted file mode 100755 index db35dbde0e2fe3ccc9c963b6592aa3f3f3eb7206..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8652 zcmeHMZ){sv6+iY%nsiBRw`(D7S!0zIWfgf#vMji(*4as&zEnN-WDilIy=Ab6mi$hQRl*V= zs4|d;z6N|&dB8WMey|La9|lS0VG?;w#A_lR%XxB)CASaThX4DizJ6*S%e|HxwnSaT z!38sczfCl%q}48yWV@AZOXhuc!Aub4Lm@y~hsfUD`sD_-k0mAH!E|c4qy52jqB)() z7AKo0LmkZ>?Rp`nw<({ne$?I9H=rzWKQ>UmYy?nq0L%T~ZTiWcUqAPwue>7PY2JPP zXE#4J@>S&W|NjMb0I;C~;XZprT}Y>fN6_sK5CVUTf*H@7A`?$#L5Q(TE=xHA_QiTS zyF{DbraugzT7*90Kd}3=xFxj~%RWtJ7X8*d!?Cfa763E^tbClV$Y%r{`A&Uf`dY1V zsybR91mlHSv-3 z^oK})?DUSA+8_)Vr<+k8jypt+vNG>yxN+w@oQp$P{sCa?!JbMT{lL61x`kLBTJT(D~ zBe+L|?nt@Q;Th&5R_X)XPx^1&mJ)pcU!dF9u-{+zOzlZ{Y|HMQ58b~Vv$0N$(Z9VT z?w6!YVtJ=8@HKDL*YL6y^#xB?MSV>#RU5vR7uOiR z&~#0&FP!#;BEFW0uc^})?DREs`U0K4TIJ^t(f=hWo$GxSt60SyfdzB=E zzfoD%Q^Aa8oln+L`a-pp-b`seUu~r{pO3asx`ED9e9q(Ye=Nxys+^&7lYQnRdwkY8 zN9FfWeIb%~4nW{}1F?xD1J9od^u5h{ulgo{zJ0{xmuo1mw%DHC)vIcb7En#!g?&*u_x zGY)!kT#V-9nWPv`_>{%q-%pO@l^gL)Y6SGLoT>g;R;(I_3kA?ea+ypr zYr4Dq{{pTnoI^Z+`Tm6ORd`)7c)l?{383aO^Z9;)?=7s`lV$LI1EbZ&H)%bZR3UzE zJK^TzH})+lb8Mg2`x(k$|9SoMI%D(!sI|^~jtA!`!yrYB7ua^@^SXST?je}Z@r45f z#}kIT|4sm?wak28zaa`R9H-cS)?@h;fQk*A=J*gMKKH-df5x+PpTc~OM?=KtcCvkU z`|;F@W5RrnZ`2LUY`xtKf zXI=amio-KBAUByv;PGS3;t`nE0D|{xz7KTkyVJh`0KbFJ`*{Q1m%8=c>9@##8g9Uw z8$|F!^8ZZtv&`rHjPGmtUY6%4+vj=kp36Sp*DYeB9P^#Qe3tLK`20P=2AY^iU>-{x zADp7L|MWf5L=N7K&-fSl$b7tRLmVVN+hMqt14y#Z>zu)Scl>5POMDh!+fJBlpA`wN zq)PMJSpoWh9HN24`&->H9vs(jeO9tR)M(X~c2(}S|KFg2$KFq!(7&#PJts6+Z^ttyG}vIrDJL|z*N#6z2e)+iIt$d|+{Z8ro0WZ=K zZB!B3ucZHu(q9i}YAk)m-&B6(@vT(PzZG#q**w8JW>1Crut{}DdApjFe))XZQl`TQw*a0_WDn?KL1`pehXmsEZ^zDM=A=jV^_;?FNU0PDRC z(BhiszruXS`sMGCKdSr&2$MeVH-Dvmz-J_cx+4dGe=2`A!%C*Ti(x|chNtt>MxIz!{n#o@)Jh#X2C3u zj_M;I#Qv^BBGz-bUkFGf^U1MP!A$0bnGqxDTsB#N3RyxVa$+o<8;++%!p!9hB3_&X ze6dL<&16FF*!7vV%1Qzcv)0j6B(r9I3arCyqL|4{fuN(UN(npzuTbskjX#y_I(oE= zo`!l7`pCoth|k4Z+fv!l96$Ad5Z#9&y-^YE+baY$k$0QDM52%r - -int main(int argc, char** argv) -{ - (void)argv; -#ifndef pthread_create - return ((int*)(&pthread_create))[argc]; -#else - (void)argc; - return 0; -#endif -} - -Determining if the function pthread_create exists in the pthreads failed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c -Linking C executable cmTryCompileExec26988121 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -o cmTryCompileExec26988121 -rdynamic -lpthreads -/usr/bin/ld: cannot find -lpthreads -collect2: error: ld returned 1 exit status -make[1]: *** [cmTryCompileExec26988121] Error 1 -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -make: *** [cmTryCompileExec26988121/fast] Error 2 - - diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log deleted file mode 100644 index a3757fc345..0000000000 --- a/build/CMakeFiles/CMakeOutput.log +++ /dev/null @@ -1,293 +0,0 @@ -The system is: Linux - 3.13.0-24-generic - x86_64 -Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. -Compiler: /usr/bin/cc -Build flags: -Id flags: - -The output was: -0 - - -Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out" - -The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out" - -Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. -Compiler: /usr/bin/c++ -Build flags: -Id flags: - -The output was: -0 - - -Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out" - -The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out" - -Determining if the C compiler works passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c -Linking C executable cmTryCompileExec4189733644 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -o cmTryCompileExec4189733644 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Detecting C compiler ABI info compiled with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c -Linking C executable cmTryCompileExec2944435992 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1 -/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic -Using built-in specs. -COLLECT_GCC=/usr/bin/cc -COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper -Target: x86_64-linux-gnu -Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu -Thread model: posix -gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) -COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ -LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Parsed C implicit link information from above output: - link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] - ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] - ignore line: [] - ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"] - ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build] - ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] - ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] - ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] - ignore line: [/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c] - ignore line: [Linking C executable cmTryCompileExec2944435992] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1] - ignore line: [/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic ] - ignore line: [Using built-in specs.] - ignore line: [COLLECT_GCC=/usr/bin/cc] - ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] - ignore line: [Target: x86_64-linux-gnu] - ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] - ignore line: [Thread model: posix] - ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] - ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] - ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore - arg [--sysroot=/] ==> ignore - arg [--build-id] ==> ignore - arg [--eh-frame-hdr] ==> ignore - arg [-m] ==> ignore - arg [elf_x86_64] ==> ignore - arg [--hash-style=gnu] ==> ignore - arg [--as-needed] ==> ignore - arg [-export-dynamic] ==> ignore - arg [-dynamic-linker] ==> ignore - arg [/lib64/ld-linux-x86-64.so.2] ==> ignore - arg [-zrelro] ==> ignore - arg [-o] ==> ignore - arg [cmTryCompileExec2944435992] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] - arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] - arg [-L/lib/../lib] ==> dir [/lib/../lib] - arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] - arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] - arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore - arg [-lgcc] ==> lib [gcc] - arg [--as-needed] ==> ignore - arg [-lgcc_s] ==> lib [gcc_s] - arg [--no-as-needed] ==> ignore - arg [-lc] ==> lib [c] - arg [-lgcc] ==> lib [gcc] - arg [--as-needed] ==> ignore - arg [-lgcc_s] ==> lib [gcc_s] - arg [--no-as-needed] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore - remove lib [gcc] - remove lib [gcc_s] - remove lib [gcc] - remove lib [gcc_s] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] - collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] - collapse library dir [/lib/../lib] ==> [/lib] - collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/../lib] ==> [/usr/lib] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] - implicit libs: [c] - implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] - implicit fwks: [] - - -Determining if the CXX compiler works passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -/usr/bin/c++ -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx -Linking CXX executable cmTryCompileExec191173331 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1 -/usr/bin/c++ CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -o cmTryCompileExec191173331 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Detecting CXX compiler ABI info compiled with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp -Linking CXX executable cmTryCompileExec843003076 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1 -/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic -Using built-in specs. -COLLECT_GCC=/usr/bin/c++ -COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper -Target: x86_64-linux-gnu -Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu -Thread model: posix -gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) -COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ -LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Parsed CXX implicit link information from above output: - link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] - ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] - ignore line: [] - ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"] - ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build] - ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] - ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] - ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] - ignore line: [/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp] - ignore line: [Linking CXX executable cmTryCompileExec843003076] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1] - ignore line: [/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic ] - ignore line: [Using built-in specs.] - ignore line: [COLLECT_GCC=/usr/bin/c++] - ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] - ignore line: [Target: x86_64-linux-gnu] - ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] - ignore line: [Thread model: posix] - ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] - ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] - ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore - arg [--sysroot=/] ==> ignore - arg [--build-id] ==> ignore - arg [--eh-frame-hdr] ==> ignore - arg [-m] ==> ignore - arg [elf_x86_64] ==> ignore - arg [--hash-style=gnu] ==> ignore - arg [--as-needed] ==> ignore - arg [-export-dynamic] ==> ignore - arg [-dynamic-linker] ==> ignore - arg [/lib64/ld-linux-x86-64.so.2] ==> ignore - arg [-zrelro] ==> ignore - arg [-o] ==> ignore - arg [cmTryCompileExec843003076] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] - arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] - arg [-L/lib/../lib] ==> dir [/lib/../lib] - arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] - arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] - arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore - arg [-lstdc++] ==> lib [stdc++] - arg [-lm] ==> lib [m] - arg [-lgcc_s] ==> lib [gcc_s] - arg [-lgcc] ==> lib [gcc] - arg [-lc] ==> lib [c] - arg [-lgcc_s] ==> lib [gcc_s] - arg [-lgcc] ==> lib [gcc] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore - remove lib [gcc_s] - remove lib [gcc] - remove lib [gcc_s] - remove lib [gcc] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] - collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] - collapse library dir [/lib/../lib] ==> [/lib] - collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/../lib] ==> [/usr/lib] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] - implicit libs: [stdc++;m;c] - implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] - implicit fwks: [] - - -Determining if files pthread.h exist passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c -Linking C executable cmTryCompileExec3897003384 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -o cmTryCompileExec3897003384 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Determining if the function pthread_create exists in the pthread passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c -Linking C executable cmTryCompileExec4077710905 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -o cmTryCompileExec4077710905 -rdynamic -lpthread -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt deleted file mode 100644 index b1bfe30c0e..0000000000 --- a/build/CMakeFiles/CMakeRuleHashes.txt +++ /dev/null @@ -1,5 +0,0 @@ -# Hashes of file build rules. -353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results -d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen -d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests -d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake deleted file mode 100644 index d232bacff9..0000000000 --- a/build/CMakeFiles/Makefile.cmake +++ /dev/null @@ -1,150 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# The generator used is: -SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") - -# The top level Makefile was generated from the following files: -SET(CMAKE_MAKEFILE_DEPENDS - "CMakeCache.txt" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeSystem.cmake" - "catkin/catkin_generated/version/package.cmake" - "catkin_generated/order_packages.cmake" - "/home/roman/src/Firmware/src/CMakeLists.txt" - "/opt/ros/indigo/share/catkin/cmake/../package.xml" - "/opt/ros/indigo/share/catkin/cmake/all.cmake" - "/opt/ros/indigo/share/catkin/cmake/assert.cmake" - "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake" - "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake" - "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em" - "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake" - "/opt/ros/indigo/share/catkin/cmake/empy.cmake" - "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in" - "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake" - "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py" - "/opt/ros/indigo/share/catkin/cmake/legacy.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake" - "/opt/ros/indigo/share/catkin/cmake/python.cmake" - "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake" - "/opt/ros/indigo/share/catkin/cmake/stamp.cmake" - "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake" - "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in" - "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in" - "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c" - "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp" - "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake" - "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake" - "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake" - "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake" - "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake" - "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake" - "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake" - "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake" - "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c" - "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake" - "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake" - "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake" - "/usr/share/cmake-2.8/Modules/FindGTest.cmake" - "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake" - "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake" - "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake" - "/usr/share/cmake-2.8/Modules/FindThreads.cmake" - "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake" - "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake" - "/usr/src/gtest/CMakeLists.txt" - "/usr/src/gtest/cmake/internal_utils.cmake" - ) - -# The corresponding makefile is: -SET(CMAKE_MAKEFILE_OUTPUTS - "Makefile" - "CMakeFiles/cmake.check_cache" - ) - -# Byproducts of CMake generate step: -SET(CMAKE_MAKEFILE_PRODUCTS - "CMakeFiles/2.8.12.2/CMakeSystem.cmake" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "catkin_generated/stamps/Project/package.xml.stamp" - "catkin_generated/installspace/_setup_util.py" - "catkin_generated/installspace/env.sh" - "catkin_generated/installspace/setup.bash" - "catkin_generated/installspace/setup.sh" - "catkin_generated/installspace/setup.zsh" - "catkin_generated/installspace/.rosinstall" - "catkin_generated/generate_cached_setup.py" - "catkin_generated/env_cached.sh" - "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp" - "catkin_generated/order_packages.py" - "catkin_generated/stamps/Project/order_packages.cmake.em.stamp" - "CMakeFiles/CMakeDirectoryInformation.cmake" - "gtest/CMakeFiles/CMakeDirectoryInformation.cmake" - ) - -# Dependency information for all targets: -SET(CMAKE_DEPEND_INFO_FILES - "CMakeFiles/clean_test_results.dir/DependInfo.cmake" - "CMakeFiles/doxygen.dir/DependInfo.cmake" - "CMakeFiles/run_tests.dir/DependInfo.cmake" - "CMakeFiles/tests.dir/DependInfo.cmake" - "gtest/CMakeFiles/gtest.dir/DependInfo.cmake" - "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake" - ) diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2 deleted file mode 100644 index 2a11fe8ef9..0000000000 --- a/build/CMakeFiles/Makefile2 +++ /dev/null @@ -1,266 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -# The main recursive all target -all: -.PHONY : all - -# The main recursive preinstall target -preinstall: -.PHONY : preinstall - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Target rules for target CMakeFiles/clean_test_results.dir - -# All Build rule for target. -CMakeFiles/clean_test_results.dir/all: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target clean_test_results" -.PHONY : CMakeFiles/clean_test_results.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/clean_test_results.dir/rule - -# Convenience name for target. -clean_test_results: CMakeFiles/clean_test_results.dir/rule -.PHONY : clean_test_results - -# clean rule for target. -CMakeFiles/clean_test_results.dir/clean: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean -.PHONY : CMakeFiles/clean_test_results.dir/clean - -# clean rule for target. -clean: CMakeFiles/clean_test_results.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/doxygen.dir - -# All Build rule for target. -CMakeFiles/doxygen.dir/all: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target doxygen" -.PHONY : CMakeFiles/doxygen.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/doxygen.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/doxygen.dir/rule - -# Convenience name for target. -doxygen: CMakeFiles/doxygen.dir/rule -.PHONY : doxygen - -# clean rule for target. -CMakeFiles/doxygen.dir/clean: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean -.PHONY : CMakeFiles/doxygen.dir/clean - -# clean rule for target. -clean: CMakeFiles/doxygen.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/run_tests.dir - -# All Build rule for target. -CMakeFiles/run_tests.dir/all: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target run_tests" -.PHONY : CMakeFiles/run_tests.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/run_tests.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/run_tests.dir/rule - -# Convenience name for target. -run_tests: CMakeFiles/run_tests.dir/rule -.PHONY : run_tests - -# clean rule for target. -CMakeFiles/run_tests.dir/clean: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean -.PHONY : CMakeFiles/run_tests.dir/clean - -# clean rule for target. -clean: CMakeFiles/run_tests.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/tests.dir - -# All Build rule for target. -CMakeFiles/tests.dir/all: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target tests" -.PHONY : CMakeFiles/tests.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/tests.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/tests.dir/rule - -# Convenience name for target. -tests: CMakeFiles/tests.dir/rule -.PHONY : tests - -# clean rule for target. -CMakeFiles/tests.dir/clean: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean -.PHONY : CMakeFiles/tests.dir/clean - -# clean rule for target. -clean: CMakeFiles/tests.dir/clean -.PHONY : clean - -#============================================================================= -# Directory level rules for directory gtest - -# Convenience name for "all" pass in the directory. -gtest/all: -.PHONY : gtest/all - -# Convenience name for "clean" pass in the directory. -gtest/clean: gtest/CMakeFiles/gtest.dir/clean -gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : gtest/clean - -# Convenience name for "preinstall" pass in the directory. -gtest/preinstall: -.PHONY : gtest/preinstall - -#============================================================================= -# Target rules for target gtest/CMakeFiles/gtest.dir - -# All Build rule for target. -gtest/CMakeFiles/gtest.dir/all: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 1 - @echo "Built target gtest" -.PHONY : gtest/CMakeFiles/gtest.dir/all - -# Build rule for subdir invocation for target. -gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1 - $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : gtest/CMakeFiles/gtest.dir/rule - -# Convenience name for target. -gtest: gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest - -# clean rule for target. -gtest/CMakeFiles/gtest.dir/clean: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean -.PHONY : gtest/CMakeFiles/gtest.dir/clean - -# clean rule for target. -clean: gtest/CMakeFiles/gtest.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target gtest/CMakeFiles/gtest_main.dir - -# All Build rule for target. -gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 2 - @echo "Built target gtest_main" -.PHONY : gtest/CMakeFiles/gtest_main.dir/all - -# Build rule for subdir invocation for target. -gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2 - $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : gtest/CMakeFiles/gtest_main.dir/rule - -# Convenience name for target. -gtest_main: gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest_main - -# clean rule for target. -gtest/CMakeFiles/gtest_main.dir/clean: - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : gtest/CMakeFiles/gtest_main.dir/clean - -# clean rule for target. -clean: gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : clean - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt deleted file mode 100644 index 552e0c540a..0000000000 --- a/build/CMakeFiles/TargetDirectories.txt +++ /dev/null @@ -1,6 +0,0 @@ -/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir -/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir -/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir -/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir -/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir -/home/roman/src/Firmware/build/CMakeFiles/tests.dir diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c3..0000000000 --- a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make deleted file mode 100644 index b3776d0df4..0000000000 --- a/build/CMakeFiles/clean_test_results.dir/build.make +++ /dev/null @@ -1,66 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for clean_test_results. - -# Include the progress variables for this target. -include CMakeFiles/clean_test_results.dir/progress.make - -CMakeFiles/clean_test_results: - /usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results - -clean_test_results: CMakeFiles/clean_test_results -clean_test_results: CMakeFiles/clean_test_results.dir/build.make -.PHONY : clean_test_results - -# Rule to build all files generated by this target. -CMakeFiles/clean_test_results.dir/build: clean_test_results -.PHONY : CMakeFiles/clean_test_results.dir/build - -CMakeFiles/clean_test_results.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake -.PHONY : CMakeFiles/clean_test_results.dir/clean - -CMakeFiles/clean_test_results.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/clean_test_results.dir/depend - diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake deleted file mode 100644 index 46c1cb3380..0000000000 --- a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/clean_test_results" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make deleted file mode 100644 index 8b13789179..0000000000 --- a/build/CMakeFiles/clean_test_results.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache deleted file mode 100644 index 3dccd73172..0000000000 --- a/build/CMakeFiles/cmake.check_cache +++ /dev/null @@ -1 +0,0 @@ -# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c3..0000000000 --- a/build/CMakeFiles/doxygen.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make deleted file mode 100644 index f5cc02bb39..0000000000 --- a/build/CMakeFiles/doxygen.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for doxygen. - -# Include the progress variables for this target. -include CMakeFiles/doxygen.dir/progress.make - -CMakeFiles/doxygen: - -doxygen: CMakeFiles/doxygen -doxygen: CMakeFiles/doxygen.dir/build.make -.PHONY : doxygen - -# Rule to build all files generated by this target. -CMakeFiles/doxygen.dir/build: doxygen -.PHONY : CMakeFiles/doxygen.dir/build - -CMakeFiles/doxygen.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake -.PHONY : CMakeFiles/doxygen.dir/clean - -CMakeFiles/doxygen.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/doxygen.dir/depend - diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake deleted file mode 100644 index 3cf72d90fa..0000000000 --- a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/doxygen" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make deleted file mode 100644 index 8b13789179..0000000000 --- a/build/CMakeFiles/doxygen.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks deleted file mode 100644 index 573541ac97..0000000000 --- a/build/CMakeFiles/progress.marks +++ /dev/null @@ -1 +0,0 @@ -0 diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c3..0000000000 --- a/build/CMakeFiles/run_tests.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make deleted file mode 100644 index 3907c75732..0000000000 --- a/build/CMakeFiles/run_tests.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for run_tests. - -# Include the progress variables for this target. -include CMakeFiles/run_tests.dir/progress.make - -CMakeFiles/run_tests: - -run_tests: CMakeFiles/run_tests -run_tests: CMakeFiles/run_tests.dir/build.make -.PHONY : run_tests - -# Rule to build all files generated by this target. -CMakeFiles/run_tests.dir/build: run_tests -.PHONY : CMakeFiles/run_tests.dir/build - -CMakeFiles/run_tests.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake -.PHONY : CMakeFiles/run_tests.dir/clean - -CMakeFiles/run_tests.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/run_tests.dir/depend - diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake deleted file mode 100644 index 45a3e057b2..0000000000 --- a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/run_tests" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make deleted file mode 100644 index 8b13789179..0000000000 --- a/build/CMakeFiles/run_tests.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c3..0000000000 --- a/build/CMakeFiles/tests.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make deleted file mode 100644 index 720d530001..0000000000 --- a/build/CMakeFiles/tests.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for tests. - -# Include the progress variables for this target. -include CMakeFiles/tests.dir/progress.make - -CMakeFiles/tests: - -tests: CMakeFiles/tests -tests: CMakeFiles/tests.dir/build.make -.PHONY : tests - -# Rule to build all files generated by this target. -CMakeFiles/tests.dir/build: tests -.PHONY : CMakeFiles/tests.dir/build - -CMakeFiles/tests.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake -.PHONY : CMakeFiles/tests.dir/clean - -CMakeFiles/tests.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/tests.dir/depend - diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake deleted file mode 100644 index a0424cfc7c..0000000000 --- a/build/CMakeFiles/tests.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/tests" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make deleted file mode 100644 index 8b13789179..0000000000 --- a/build/CMakeFiles/tests.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake deleted file mode 100644 index 7b55348244..0000000000 --- a/build/CTestTestfile.cmake +++ /dev/null @@ -1,7 +0,0 @@ -# CMake generated Testfile for -# Source directory: /home/roman/src/Firmware/src -# Build directory: /home/roman/src/Firmware/build -# -# This file includes the relevant testing commands required for -# testing this directory and lists subdirectories to be tested as well. -SUBDIRS(gtest) diff --git a/build/Makefile b/build/Makefile deleted file mode 100644 index c7d796cc42..0000000000 --- a/build/Makefile +++ /dev/null @@ -1,262 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." - /usr/bin/cmake -i . -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: install/local -.PHONY : install/local/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: install/strip -.PHONY : install/strip/fast - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# Special rule for the target test -test: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." - /usr/bin/ctest --force-new-ctest-process $(ARGS) -.PHONY : test - -# Special rule for the target test -test/fast: test -.PHONY : test/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks - $(MAKE) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -#============================================================================= -# Target rules for targets named clean_test_results - -# Build rule for target. -clean_test_results: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 clean_test_results -.PHONY : clean_test_results - -# fast build rule for target. -clean_test_results/fast: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build -.PHONY : clean_test_results/fast - -#============================================================================= -# Target rules for targets named doxygen - -# Build rule for target. -doxygen: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 doxygen -.PHONY : doxygen - -# fast build rule for target. -doxygen/fast: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build -.PHONY : doxygen/fast - -#============================================================================= -# Target rules for targets named run_tests - -# Build rule for target. -run_tests: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 run_tests -.PHONY : run_tests - -# fast build rule for target. -run_tests/fast: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build -.PHONY : run_tests/fast - -#============================================================================= -# Target rules for targets named tests - -# Build rule for target. -tests: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 tests -.PHONY : tests - -# fast build rule for target. -tests/fast: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build -.PHONY : tests/fast - -#============================================================================= -# Target rules for targets named gtest - -# Build rule for target. -gtest: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 gtest -.PHONY : gtest - -# fast build rule for target. -gtest/fast: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build -.PHONY : gtest/fast - -#============================================================================= -# Target rules for targets named gtest_main - -# Build rule for target. -gtest_main: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 gtest_main -.PHONY : gtest_main - -# fast build rule for target. -gtest_main/fast: - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build -.PHONY : gtest_main/fast - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... clean_test_results" - @echo "... doxygen" - @echo "... edit_cache" - @echo "... install" - @echo "... install/local" - @echo "... install/strip" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... run_tests" - @echo "... test" - @echo "... tests" - @echo "... gtest" - @echo "... gtest_main" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake deleted file mode 100644 index 1cbfc826b0..0000000000 --- a/build/catkin/catkin_generated/version/package.cmake +++ /dev/null @@ -1,9 +0,0 @@ -set(_CATKIN_CURRENT_PACKAGE "catkin") -set(catkin_VERSION "0.6.9") -set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") -set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg") -set(catkin_DEPRECATED "") -set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose") -set(catkin_MAINTAINER "Dirk Thomas ") -set(catkin_BUILDTOOL_DEPENDS "cmake") -set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") \ No newline at end of file diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh deleted file mode 100755 index d6be91db5c..0000000000 --- a/build/catkin_generated/env_cached.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup_cached.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup_cached.sh" -exec "$@" diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py deleted file mode 100644 index 7a56cdb7f9..0000000000 --- a/build/catkin_generated/generate_cached_setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from __future__ import print_function -import argparse -import os -import stat -import sys - -# find the import for catkin's python package - either from source space or from an installed underlay -if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')): - sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python')) -try: - from catkin.environment_cache import generate_environment_script -except ImportError: - # search for catkin package in all workspaces and prepend to path - for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'): - python_path = os.path.join(workspace, 'lib/python2.7/dist-packages') - if os.path.isdir(os.path.join(python_path, 'catkin')): - sys.path.insert(0, python_path) - break - from catkin.environment_cache import generate_environment_script - -code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh') - -output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh' -with open(output_filename, 'w') as f: - #print('Generate script for cached setup "%s"' % output_filename) - f.write('\n'.join(code)) - -mode = os.stat(output_filename).st_mode -os.chmod(output_filename, mode | stat.S_IXUSR) diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall deleted file mode 100644 index a959c71efc..0000000000 --- a/build/catkin_generated/installspace/.rosinstall +++ /dev/null @@ -1,2 +0,0 @@ -- setup-file: - local-name: /home/roman/src/Firmware/install/setup.sh diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py deleted file mode 100755 index 8db644140c..0000000000 --- a/build/catkin_generated/installspace/_setup_util.py +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -'''This file generates shell code for the setup.SHELL scripts to set environment variables''' - -from __future__ import print_function -import argparse -import copy -import errno -import os -import platform -import sys - -CATKIN_MARKER_FILE = '.catkin' - -system = platform.system() -IS_DARWIN = (system == 'Darwin') -IS_WINDOWS = (system == 'Windows') - -# subfolder of workspace prepended to CMAKE_PREFIX_PATH -ENV_VAR_SUBFOLDERS = { - 'CMAKE_PREFIX_PATH': '', - 'CPATH': 'include', - 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], - 'PATH': 'bin', - 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], - 'PYTHONPATH': 'lib/python2.7/dist-packages', -} - - -def rollback_env_variables(environ, env_var_subfolders): - ''' - Generate shell code to reset environment variables - by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. - This does not cover modifications performed by environment hooks. - ''' - lines = [] - unmodified_environ = copy.copy(environ) - for key in sorted(env_var_subfolders.keys()): - subfolders = env_var_subfolders[key] - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - value = _rollback_env_variable(unmodified_environ, key, subfolder) - if value is not None: - environ[key] = value - lines.append(assignment(key, value)) - if lines: - lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) - return lines - - -def _rollback_env_variable(environ, name, subfolder): - ''' - For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. - - :param subfolder: str '' or subfoldername that may start with '/' - :returns: the updated value of the environment variable. - ''' - value = environ[name] if name in environ else '' - env_paths = [path for path in value.split(os.pathsep) if path] - value_modified = False - if subfolder: - if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): - subfolder = subfolder[1:] - if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): - subfolder = subfolder[:-1] - for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): - path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path - path_to_remove = None - for env_path in env_paths: - env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path - if env_path_clean == path_to_find: - path_to_remove = env_path - break - if path_to_remove: - env_paths.remove(path_to_remove) - value_modified = True - new_value = os.pathsep.join(env_paths) - return new_value if value_modified else None - - -def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): - ''' - Based on CMAKE_PREFIX_PATH return all catkin workspaces. - - :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` - ''' - # get all cmake prefix paths - env_name = 'CMAKE_PREFIX_PATH' - value = environ[env_name] if env_name in environ else '' - paths = [path for path in value.split(os.pathsep) if path] - # remove non-workspace paths - workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] - return workspaces - - -def prepend_env_variables(environ, env_var_subfolders, workspaces): - ''' - Generate shell code to prepend environment variables - for the all workspaces. - ''' - lines = [] - lines.append(comment('prepend folders of workspaces to environment variables')) - - paths = [path for path in workspaces.split(os.pathsep) if path] - - prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') - lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) - - for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): - subfolder = env_var_subfolders[key] - prefix = _prefix_env_variable(environ, key, paths, subfolder) - lines.append(prepend(environ, key, prefix)) - return lines - - -def _prefix_env_variable(environ, name, paths, subfolders): - ''' - Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. - ''' - value = environ[name] if name in environ else '' - environ_paths = [path for path in value.split(os.pathsep) if path] - checked_paths = [] - for path in paths: - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - path_tmp = path - if subfolder: - path_tmp = os.path.join(path_tmp, subfolder) - # exclude any path already in env and any path we already added - if path_tmp not in environ_paths and path_tmp not in checked_paths: - checked_paths.append(path_tmp) - prefix_str = os.pathsep.join(checked_paths) - if prefix_str != '' and environ_paths: - prefix_str += os.pathsep - return prefix_str - - -def assignment(key, value): - if not IS_WINDOWS: - return 'export %s="%s"' % (key, value) - else: - return 'set %s=%s' % (key, value) - - -def comment(msg): - if not IS_WINDOWS: - return '# %s' % msg - else: - return 'REM %s' % msg - - -def prepend(environ, key, prefix): - if key not in environ or not environ[key]: - return assignment(key, prefix) - if not IS_WINDOWS: - return 'export %s="%s$%s"' % (key, prefix, key) - else: - return 'set %s=%s%%%s%%' % (key, prefix, key) - - -def find_env_hooks(environ, cmake_prefix_path): - ''' - Generate shell code with found environment hooks - for the all workspaces. - ''' - lines = [] - lines.append(comment('found environment hooks in workspaces')) - - generic_env_hooks = [] - generic_env_hooks_workspace = [] - specific_env_hooks = [] - specific_env_hooks_workspace = [] - generic_env_hooks_by_filename = {} - specific_env_hooks_by_filename = {} - generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' - specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None - # remove non-workspace paths - workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] - for workspace in reversed(workspaces): - env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') - if os.path.isdir(env_hook_dir): - for filename in sorted(os.listdir(env_hook_dir)): - if filename.endswith('.%s' % generic_env_hook_ext): - # remove previous env hook with same name if present - if filename in generic_env_hooks_by_filename: - i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) - generic_env_hooks.pop(i) - generic_env_hooks_workspace.pop(i) - # append env hook - generic_env_hooks.append(os.path.join(env_hook_dir, filename)) - generic_env_hooks_workspace.append(workspace) - generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] - elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): - # remove previous env hook with same name if present - if filename in specific_env_hooks_by_filename: - i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) - specific_env_hooks.pop(i) - specific_env_hooks_workspace.pop(i) - # append env hook - specific_env_hooks.append(os.path.join(env_hook_dir, filename)) - specific_env_hooks_workspace.append(workspace) - specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] - env_hooks = generic_env_hooks + specific_env_hooks - env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace - count = len(env_hooks) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) - for i in range(count): - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) - return lines - - -def _parse_arguments(args=None): - parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') - parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') - return parser.parse_known_args(args=args)[0] - - -if __name__ == '__main__': - try: - try: - args = _parse_arguments() - except Exception as e: - print(e, file=sys.stderr) - sys.exit(1) - - # environment at generation time - CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') - # prepend current workspace if not already part of CPP - base_path = os.path.dirname(__file__) - if base_path not in CMAKE_PREFIX_PATH: - CMAKE_PREFIX_PATH.insert(0, base_path) - CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) - - environ = dict(os.environ) - lines = [] - if not args.extend: - lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) - lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) - lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) - print('\n'.join(lines)) - - # need to explicitly flush the output - sys.stdout.flush() - except IOError as e: - # and catch potantial "broken pipe" if stdout is not writable - # which can happen when piping the output to a file but the disk is full - if e.errno == errno.EPIPE: - print(e, file=sys.stderr) - sys.exit(2) - raise - - sys.exit(0) diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh deleted file mode 100755 index 8aa9d244ae..0000000000 --- a/build/catkin_generated/installspace/env.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" -exec "$@" diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash deleted file mode 100644 index ff47af8f30..0000000000 --- a/build/catkin_generated/installspace/setup.bash +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/setup.bash.in - -CATKIN_SHELL=bash - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh deleted file mode 100644 index 95e1571bef..0000000000 --- a/build/catkin_generated/installspace/setup.sh +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/template/setup.sh.in - -# Sets various environment variables and sources additional environment hooks. -# It tries it's best to undo changes from a previously sourced setup file before. -# Supported command line options: -# --extend: skips the undoing of changes from a previously sourced setup file - -# since this file is sourced either use the provided _CATKIN_SETUP_DIR -# or fall back to the destination set at configure time -: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install} -_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" -unset _CATKIN_SETUP_DIR - -if [ ! -f "$_SETUP_UTIL" ]; then - echo "Missing Python script: $_SETUP_UTIL" - return 22 -fi - -# detect if running on Darwin platform -_UNAME=`uname -s` -_IS_DARWIN=0 -if [ "$_UNAME" = "Darwin" ]; then - _IS_DARWIN=1 -fi -unset _UNAME - -# make sure to export all environment variables -export CMAKE_PREFIX_PATH -export CPATH -if [ $_IS_DARWIN -eq 0 ]; then - export LD_LIBRARY_PATH -else - export DYLD_LIBRARY_PATH -fi -unset _IS_DARWIN -export PATH -export PKG_CONFIG_PATH -export PYTHONPATH - -# remember type of shell if not already set -if [ -z "$CATKIN_SHELL" ]; then - CATKIN_SHELL=sh -fi - -# invoke Python script to generate necessary exports of environment variables -_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` -if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then - echo "Could not create temporary file: $_SETUP_TMP" - return 1 -fi -CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP -_RC=$? -if [ $_RC -ne 0 ]; then - if [ $_RC -eq 2 ]; then - echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" - else - echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" - fi - unset _RC - unset _SETUP_UTIL - rm -f $_SETUP_TMP - unset _SETUP_TMP - return 1 -fi -unset _RC -unset _SETUP_UTIL -. $_SETUP_TMP -rm -f $_SETUP_TMP -unset _SETUP_TMP - -# source all environment hooks -_i=0 -while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do - eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i - unset _CATKIN_ENVIRONMENT_HOOKS_$_i - eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - # set workspace for environment hook - CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace - . "$_envfile" - unset CATKIN_ENV_HOOK_WORKSPACE - _i=$((_i + 1)) -done -unset _i - -unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh deleted file mode 100644 index b660717663..0000000000 --- a/build/catkin_generated/installspace/setup.zsh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env zsh -# generated from catkin/cmake/templates/setup.zsh.in - -CATKIN_SHELL=zsh -_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) -emulate sh # emulate POSIX -. "$_CATKIN_SETUP_DIR/setup.sh" -emulate zsh # back to zsh mode diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake deleted file mode 100644 index 6b0e2dff92..0000000000 --- a/build/catkin_generated/order_packages.cmake +++ /dev/null @@ -1,10 +0,0 @@ -# generated from catkin/cmake/em/order_packages.cmake.em - -set(CATKIN_ORDERED_PACKAGES "") -set(CATKIN_ORDERED_PACKAGE_PATHS "") -set(CATKIN_ORDERED_PACKAGES_IS_META "") -set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") - -set(CATKIN_MESSAGE_GENERATORS ) - -set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in") diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py deleted file mode 100644 index 5cb1765802..0000000000 --- a/build/catkin_generated/order_packages.py +++ /dev/null @@ -1,5 +0,0 @@ -# generated from catkin/cmake/template/order_packages.context.py.in -source_root_dir = "/home/roman/src/Firmware/src" -whitelisted_packages = "".split(';') if "" != "" else [] -blacklisted_packages = "".split(';') if "" != "" else [] -underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else [] diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh deleted file mode 100755 index e03bce4ba5..0000000000 --- a/build/catkin_generated/setup_cached.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/python/catkin/environment_cache.py - -# based on a snapshot of the environment before and after calling the setup script -# it emulates the modifications of the setup script without recurring computations - -# new environment variables - -# modified environment variables -export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" -export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH" -export CPATH="/home/roman/src/Firmware/devel/include:$CPATH" -export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib" -export PATH="/home/roman/src/Firmware/devel/bin:$PATH" -export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH" -export PWD="/home/roman/src/Firmware/build" -export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH" -export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES" -export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH" -export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" \ No newline at end of file diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp deleted file mode 100644 index 26882f02aa..0000000000 --- a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -from __future__ import print_function -import os -import sys - -import distutils.core -try: - import setuptools -except ImportError: - pass - -from argparse import ArgumentParser - - -def _get_locations(pkgs, package_dir): - """ - based on setuptools logic and the package_dir dict, builds a dict - of location roots for each pkg in pkgs. - See http://docs.python.org/distutils/setupscript.html - - :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents) - """ - # package_dir contains a dict {package_name: relativepath} - # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'} - # - # '' means where to look for any package unless a parent package - # is listed so package bar.pot is expected at lib2/bar/pot, - # whereas package sup.dee is expected at src/sup/dee - # - # if package_dir does not state anything about a package, - # setuptool expects the package folder to be in the root of the - # project - locations = {} - allprefix = package_dir.get('', '') - for pkg in pkgs: - parent_location = None - splits = pkg.split('.') - # we iterate over compound name from parent to child - # so once we found parent, children just append to their parent - for key_len in range(len(splits)): - key = '.'.join(splits[:key_len + 1]) - if key not in locations: - if key in package_dir: - locations[key] = package_dir[key] - elif parent_location is not None: - locations[key] = parent_location - else: - locations[key] = allprefix - parent_location = locations[key] - return locations - - -def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules): - """ - Generates lines to add to a cmake file which will set variables - - :param version: str, format 'int.int.int' - :param scripts: [list of str]: relative paths to scripts - :param package_dir: {modulename: path} - :pkgs: [list of str] python_packages declared in catkin package - :modules: [list of str] python modules - """ - prefix = '%s_SETUP_PY' % package_name - result = [] - result.append(r'set(%s_VERSION "%s")' % (prefix, version)) - result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts))) - - # Remove packages with '.' separators. - # - # setuptools allows specifying submodules in other folders than - # their parent - # - # The symlink approach of catkin does not work with such submodules. - # In the common case, this does not matter as the submodule is - # within the containing module. We verify this assumption, and if - # it passes, we remove submodule packages. - locations = _get_locations(pkgs, package_dir) - for pkgname, location in locations.items(): - if not '.' in pkgname: - continue - splits = pkgname.split('.') - # hack: ignore write-combining setup.py files for msg and srv files - if splits[1] in ['msg', 'srv']: - continue - # check every child has the same root folder as its parent - parent_name = '.'.join(splits[:1]) - if location != locations[parent_name]: - raise RuntimeError( - "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name])) - - # If checks pass, remove all submodules - pkgs = [p for p in pkgs if '.' not in p] - - resolved_pkgs = [] - for pkg in pkgs: - resolved_pkgs += [os.path.join(locations[pkg], pkg)] - - result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs))) - result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/"))) - - # skip modules which collide with package names - filtered_modules = [] - for modname in modules: - splits = modname.split('.') - # check all parents too - equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))] - if any(equals_package): - continue - filtered_modules.append(modname) - module_locations = _get_locations(filtered_modules, package_dir) - - result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules]))) - result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/"))) - - return result - - -def _create_mock_setup_function(package_name, outfile): - """ - Creates a function to call instead of distutils.core.setup or - setuptools.setup, which just captures some args and writes them - into a file that can be used from cmake - - :param package_name: name of the package - :param outfile: filename that cmake will use afterwards - :returns: a function to replace disutils.core.setup and setuptools.setup - """ - - def setup(*args, **kwargs): - ''' - Checks kwargs and writes a scriptfile - ''' - if 'version' not in kwargs: - sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name) - raise RuntimeError("version not found in setup.py") - version = kwargs['version'] - package_dir = kwargs.get('package_dir', {}) - - pkgs = kwargs.get('packages', []) - scripts = kwargs.get('scripts', []) - modules = kwargs.get('py_modules', []) - - unsupported_args = [ - 'entry_points', - 'exclude_package_data', - 'ext_modules ', - 'ext_package', - 'include_package_data', - 'namespace_packages', - 'setup_requires', - 'use_2to3', - 'zip_safe'] - used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs] - if used_unsupported_args: - sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name)) - - result = generate_cmake_file(package_name=package_name, - version=version, - scripts=scripts, - package_dir=package_dir, - pkgs=pkgs, - modules=modules) - with open(outfile, 'w') as out: - out.write('\n'.join(result)) - - return setup - - -def main(): - """ - Script main, parses arguments and invokes Dummy.setup indirectly. - """ - parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.') - parser.add_argument('package_name', help='Name of catkin package') - parser.add_argument('setupfile_path', help='Full path to setup.py') - parser.add_argument('outfile', help='Where to write result to') - - args = parser.parse_args() - - # print("%s" % sys.argv) - # PACKAGE_NAME = sys.argv[1] - # OUTFILE = sys.argv[3] - # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE), - # file=sys.stderr) - - # print("executing %s" % args.setupfile_path) - - # be sure you're in the directory containing - # setup.py so the sys.path manipulation works, - # so the import of __version__ works - os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path))) - - # patch setup() function of distutils and setuptools for the - # context of evaluating setup.py - try: - fake_setup = _create_mock_setup_function(package_name=args.package_name, - outfile=args.outfile) - - distutils_backup = distutils.core.setup - distutils.core.setup = fake_setup - try: - setuptools_backup = setuptools.setup - setuptools.setup = fake_setup - except NameError: - pass - - with open(args.setupfile_path, 'r') as fh: - exec(fh.read()) - finally: - distutils.core.setup = distutils_backup - try: - setuptools.setup = setuptools_backup - except NameError: - pass - -if __name__ == '__main__': - main() diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp deleted file mode 100644 index 087d4d802e..0000000000 --- a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp +++ /dev/null @@ -1,56 +0,0 @@ -# generated from catkin/cmake/em/order_packages.cmake.em -@{ -import os -try: - from catkin_pkg.cmake import get_metapackage_cmake_template_path -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.topological_order import topological_order -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.package import InvalidPackage -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -# vars defined in order_packages.context.py.in -try: - ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces) -except InvalidPackage as e: - print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"')) - ordered_packages = [] -fatal_error = False -}@ - -set(CATKIN_ORDERED_PACKAGES "") -set(CATKIN_ORDERED_PACKAGE_PATHS "") -set(CATKIN_ORDERED_PACKAGES_IS_META "") -set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") -@[for path, package in ordered_packages]@ -@[if path is None]@ -message(FATAL_ERROR "Circular dependency in subset of packages:\n@package") -@{ -fatal_error = True -}@ -@[elif package.name != 'catkin']@ -list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)") -list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))") -list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))") -list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')") -@{ -deprecated = [e for e in package.exports if e.tagname == 'deprecated'] -}@ -@[if deprecated]@ -message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')") -@[end if]@ -@[end if]@ -@[end for]@ - -@[if not fatal_error]@ -@{ -message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]] -}@ -set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators))) -@[end if]@ - -set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))") diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp deleted file mode 100644 index 7c2708ac3e..0000000000 --- a/build/catkin_generated/stamps/Project/package.xml.stamp +++ /dev/null @@ -1,37 +0,0 @@ - - - catkin - 0.6.9 - Low-level build system macros and infrastructure for ROS. - Dirk Thomas - BSD - - http://www.ros.org/wiki/catkin - https://github.com/ros/catkin/issues - https://github.com/ros/catkin - - Troy Straszheim - Morten Kjaergaard - Brian Gerkey - Dirk Thomas - - cmake - cmake - - python-argparse - python-catkin-pkg - - python-empy - - gtest - python-empy - python-nose - - python-mock - python-nose - - - - - - diff --git a/build/catkin_make.cache b/build/catkin_make.cache deleted file mode 100644 index 8b13789179..0000000000 --- a/build/catkin_make.cache +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake deleted file mode 100644 index 982fd705c6..0000000000 --- a/build/cmake_install.cmake +++ /dev/null @@ -1,140 +0,0 @@ -# Install script for directory: /home/roman/src/Firmware/src - -# Set the install prefix -IF(NOT DEFINED CMAKE_INSTALL_PREFIX) - SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") -ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) -STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - IF(BUILD_TYPE) - STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - ELSE(BUILD_TYPE) - SET(CMAKE_INSTALL_CONFIG_NAME "") - ENDIF(BUILD_TYPE) - MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - -# Set the component getting installed. -IF(NOT CMAKE_INSTALL_COMPONENT) - IF(COMPONENT) - MESSAGE(STATUS "Install component: \"${COMPONENT}\"") - SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - ELSE(COMPONENT) - SET(CMAKE_INSTALL_COMPONENT) - ENDIF(COMPONENT) -ENDIF(NOT CMAKE_INSTALL_COMPONENT) - -# Install shared libraries without execute permission? -IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - SET(CMAKE_INSTALL_SO_NO_EXE "1") -ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - - if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") - file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") - endif() - if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin") - file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "") - endif() -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/_setup_util.py") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/env.sh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.bash") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.sh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.zsh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/.rosinstall") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_LOCAL_ONLY) - # Include the install script for each subdirectory. - INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake") - -ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY) - -IF(CMAKE_INSTALL_COMPONENT) - SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") -ELSE(CMAKE_INSTALL_COMPONENT) - SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") -ENDIF(CMAKE_INSTALL_COMPONENT) - -FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "") -FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) - FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n") -ENDFOREACH(file) diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake deleted file mode 100644 index 06b7c63c13..0000000000 --- a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Relative path conversion top directories. -SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest") -SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") - -# Force unix paths in dependencies. -SET(CMAKE_FORCE_UNIX_PATHS 1) - - -# The C and CXX include file regular expressions for this directory. -SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") -SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") -SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) -SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake deleted file mode 100644 index 40234804c6..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake +++ /dev/null @@ -1,27 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - "CXX" - ) -# The set of files for implicit dependencies of each language: -SET(CMAKE_DEPENDS_CHECK_CXX - "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" - ) -SET(CMAKE_CXX_COMPILER_ID "GNU") - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "GTEST_CREATE_SHARED_LIBRARY=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - "/usr/src/gtest/include" - "/usr/src/gtest" - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make deleted file mode 100644 index 744feec6d6..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/build.make +++ /dev/null @@ -1,102 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Include any dependencies generated for this target. -include gtest/CMakeFiles/gtest.dir/depend.make - -# Include the progress variables for this target. -include gtest/CMakeFiles/gtest.dir/progress.make - -# Include the compile flags for this target's objects. -include gtest/CMakeFiles/gtest.dir/flags.make - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires: -.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build -.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o - -# Object files for target gtest -gtest_OBJECTS = \ -"CMakeFiles/gtest.dir/src/gtest-all.cc.o" - -# External object files for target gtest -gtest_EXTERNAL_OBJECTS = - -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so" - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE) - -# Rule to build all files generated by this target. -gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so -.PHONY : gtest/CMakeFiles/gtest.dir/build - -gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires -.PHONY : gtest/CMakeFiles/gtest.dir/requires - -gtest/CMakeFiles/gtest.dir/clean: - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake -.PHONY : gtest/CMakeFiles/gtest.dir/clean - -gtest/CMakeFiles/gtest.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : gtest/CMakeFiles/gtest.dir/depend - diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake deleted file mode 100644 index 015a1ee758..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake +++ /dev/null @@ -1,10 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/gtest.dir/src/gtest-all.cc.o" - "libgtest.pdb" - "libgtest.so" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang CXX) - INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make deleted file mode 100644 index 37ac348dbd..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/depend.make +++ /dev/null @@ -1,2 +0,0 @@ -# Empty dependencies file for gtest. -# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make deleted file mode 100644 index fa55300acd..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/flags.make +++ /dev/null @@ -1,8 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# compile CXX with /usr/bin/c++ -CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra - -CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS - diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt deleted file mode 100644 index 8745cedd2c..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/link.txt +++ /dev/null @@ -1 +0,0 @@ -/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make deleted file mode 100644 index 781c7de277..0000000000 --- a/build/gtest/CMakeFiles/gtest.dir/progress.make +++ /dev/null @@ -1,2 +0,0 @@ -CMAKE_PROGRESS_1 = 1 - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake deleted file mode 100644 index 9f3beee6e1..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake +++ /dev/null @@ -1,28 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - "CXX" - ) -# The set of files for implicit dependencies of each language: -SET(CMAKE_DEPENDS_CHECK_CXX - "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - ) -SET(CMAKE_CXX_COMPILER_ID "GNU") - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "GTEST_CREATE_SHARED_LIBRARY=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake" - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - "/usr/src/gtest/include" - "/usr/src/gtest" - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make deleted file mode 100644 index 67a7ec0180..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/build.make +++ /dev/null @@ -1,103 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Include any dependencies generated for this target. -include gtest/CMakeFiles/gtest_main.dir/depend.make - -# Include the progress variables for this target. -include gtest/CMakeFiles/gtest_main.dir/progress.make - -# Include the compile flags for this target's objects. -include gtest/CMakeFiles/gtest_main.dir/flags.make - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires: -.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build -.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o - -# Object files for target gtest_main -gtest_main_OBJECTS = \ -"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - -# External object files for target gtest_main -gtest_main_EXTERNAL_OBJECTS = - -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make -gtest/libgtest_main.so: gtest/libgtest.so -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so" - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE) - -# Rule to build all files generated by this target. -gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so -.PHONY : gtest/CMakeFiles/gtest_main.dir/build - -gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires -.PHONY : gtest/CMakeFiles/gtest_main.dir/requires - -gtest/CMakeFiles/gtest_main.dir/clean: - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake -.PHONY : gtest/CMakeFiles/gtest_main.dir/clean - -gtest/CMakeFiles/gtest_main.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : gtest/CMakeFiles/gtest_main.dir/depend - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake deleted file mode 100644 index c8fe838192..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake +++ /dev/null @@ -1,10 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - "libgtest_main.pdb" - "libgtest_main.so" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang CXX) - INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make deleted file mode 100644 index 1d67c1ab52..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/depend.make +++ /dev/null @@ -1,2 +0,0 @@ -# Empty dependencies file for gtest_main. -# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make deleted file mode 100644 index 93ab70f12f..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/flags.make +++ /dev/null @@ -1,8 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# compile CXX with /usr/bin/c++ -CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra - -CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt deleted file mode 100644 index d35b7f61ac..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/link.txt +++ /dev/null @@ -1 +0,0 @@ -/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make deleted file mode 100644 index 164e1d26ca..0000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/progress.make +++ /dev/null @@ -1,2 +0,0 @@ -CMAKE_PROGRESS_1 = 2 - diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks deleted file mode 100644 index 573541ac97..0000000000 --- a/build/gtest/CMakeFiles/progress.marks +++ /dev/null @@ -1 +0,0 @@ -0 diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake deleted file mode 100644 index bd4b57a8bf..0000000000 --- a/build/gtest/CTestTestfile.cmake +++ /dev/null @@ -1,6 +0,0 @@ -# CMake generated Testfile for -# Source directory: /usr/src/gtest -# Build directory: /home/roman/src/Firmware/build/gtest -# -# This file includes the relevant testing commands required for -# testing this directory and lists subdirectories to be tested as well. diff --git a/build/gtest/Makefile b/build/gtest/Makefile deleted file mode 100644 index bc8c23a004..0000000000 --- a/build/gtest/Makefile +++ /dev/null @@ -1,262 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." - /usr/bin/cmake -i . -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: install/local -.PHONY : install/local/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: install/strip -.PHONY : install/strip/fast - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# Special rule for the target test -test: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." - /usr/bin/ctest --force-new-ctest-process $(ARGS) -.PHONY : test - -# Special rule for the target test -test/fast: test -.PHONY : test/fast - -# The main all target -all: cmake_check_build_system - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -# Convenience name for target. -gtest/CMakeFiles/gtest.dir/rule: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest/CMakeFiles/gtest.dir/rule - -# Convenience name for target. -gtest: gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest - -# fast build rule for target. -gtest/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build -.PHONY : gtest/fast - -# Convenience name for target. -gtest/CMakeFiles/gtest_main.dir/rule: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest/CMakeFiles/gtest_main.dir/rule - -# Convenience name for target. -gtest_main: gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest_main - -# fast build rule for target. -gtest_main/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build -.PHONY : gtest_main/fast - -src/gtest-all.o: src/gtest-all.cc.o -.PHONY : src/gtest-all.o - -# target to build an object file -src/gtest-all.cc.o: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o -.PHONY : src/gtest-all.cc.o - -src/gtest-all.i: src/gtest-all.cc.i -.PHONY : src/gtest-all.i - -# target to preprocess a source file -src/gtest-all.cc.i: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i -.PHONY : src/gtest-all.cc.i - -src/gtest-all.s: src/gtest-all.cc.s -.PHONY : src/gtest-all.s - -# target to generate assembly for a file -src/gtest-all.cc.s: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s -.PHONY : src/gtest-all.cc.s - -src/gtest_main.o: src/gtest_main.cc.o -.PHONY : src/gtest_main.o - -# target to build an object file -src/gtest_main.cc.o: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -.PHONY : src/gtest_main.cc.o - -src/gtest_main.i: src/gtest_main.cc.i -.PHONY : src/gtest_main.i - -# target to preprocess a source file -src/gtest_main.cc.i: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i -.PHONY : src/gtest_main.cc.i - -src/gtest_main.s: src/gtest_main.cc.s -.PHONY : src/gtest_main.s - -# target to generate assembly for a file -src/gtest_main.cc.s: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s -.PHONY : src/gtest_main.cc.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... edit_cache" - @echo "... gtest" - @echo "... gtest_main" - @echo "... install" - @echo "... install/local" - @echo "... install/strip" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... test" - @echo "... src/gtest-all.o" - @echo "... src/gtest-all.i" - @echo "... src/gtest-all.s" - @echo "... src/gtest_main.o" - @echo "... src/gtest_main.i" - @echo "... src/gtest_main.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake deleted file mode 100644 index f51b656514..0000000000 --- a/build/gtest/cmake_install.cmake +++ /dev/null @@ -1,34 +0,0 @@ -# Install script for directory: /usr/src/gtest - -# Set the install prefix -IF(NOT DEFINED CMAKE_INSTALL_PREFIX) - SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") -ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) -STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - IF(BUILD_TYPE) - STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - ELSE(BUILD_TYPE) - SET(CMAKE_INSTALL_CONFIG_NAME "") - ENDIF(BUILD_TYPE) - MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - -# Set the component getting installed. -IF(NOT CMAKE_INSTALL_COMPONENT) - IF(COMPONENT) - MESSAGE(STATUS "Install component: \"${COMPONENT}\"") - SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - ELSE(COMPONENT) - SET(CMAKE_INSTALL_COMPONENT) - ENDIF(COMPONENT) -ENDIF(NOT CMAKE_INSTALL_COMPONENT) - -# Install shared libraries without execute permission? -IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - SET(CMAKE_INSTALL_SO_NO_EXE "1") -ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - From 7954540f45b662ade0ad4038cf936934d3d1eef7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:52:43 +0100 Subject: [PATCH 038/122] removed files which should not be in here --- ROMFS/px4fmu_common/init.d/13000_quadshot | 14 - .../init.d/Roman_mavlink_stream_conf | 12 - ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 - ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 63 -- ROMFS/px4fmu_common/mixers/FMU_quadshot.mix | 15 - src/modules/vtol_att_control/module.mk | 40 -- .../vtol_att_control_main.cpp | 642 ------------------ 7 files changed, 801 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot delete mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults delete mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix delete mode 100644 src/modules/vtol_att_control/module.mk delete mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot deleted file mode 100644 index 8ee306a387..0000000000 --- a/ROMFS/px4fmu_common/init.d/13000_quadshot +++ /dev/null @@ -1,14 +0,0 @@ -#!nsh -# -# Generic quadshot configuration file -# -# Roman Bapst -# - -sh /etc/init.d/rc.mc_defaults - -set MIXER FMU_quadshot - -set PWM_OUTPUTS 1234 -set PWM_MIN 1070 -set PWM_MAX 2000 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf deleted file mode 100644 index d26e4a3722..0000000000 --- a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf +++ /dev/null @@ -1,12 +0,0 @@ -#!nsh -# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard -# -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50 -usleep 100000 -mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50 -echo "Added additional streams on TELEM2" \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps deleted file mode 100644 index 23ade6d78b..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -# -# Standard apps for vtol: -# att & pos estimator, att & pos control. -# - -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start - -vtol_att_control start -mc_att_control start -mc_pos_control start -fw_att_control start -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults deleted file mode 100644 index f0ea9add07..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ /dev/null @@ -1,63 +0,0 @@ -#!nsh - -set VEHICLE_TYPE vtol - -if [ $DO_AUTOCONFIG == yes ] -then - #Default parameters for MC - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - - param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 - param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 - - param set NAV_ACCEPT_RAD 2.0 - - # - # Default parameters for FW - # - param set NAV_LAND_ALT 90 - param set NAV_RTL_ALT 100 - param set NAV_RTL_LAND_T -1 - param set NAV_ACCEPT_RAD 50 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 -fi - -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1075 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix deleted file mode 100644 index b077ff30ac..0000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -#Quadshot mixer for PX4FMU -#=========================== -R: 4v 10000 10000 10000 0 - -#mixer for the elevons -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 8000 8000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 -8000 -8000 0 -10000 10000 diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk deleted file mode 100644 index c349c23401..0000000000 --- a/src/modules/vtol_att_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2013, 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. -# -############################################################################ - -# -# VTOL attitude controller -# - -MODULE_COMMAND = vtol_att_control - -SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp deleted file mode 100644 index 38fa4eec18..0000000000 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ /dev/null @@ -1,642 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 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 VTOL_att_control_main.cpp - * Implementation of an attitude controller for VTOL airframes. This module receives data - * from both the fixed wing- and the multicopter attitude controllers and processes it. - * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- - * flight or transition). It also publishes the resulting controls on the actuator controls topics. - * - * @author Roman Bapst - * @author Lorenz Meier - * @author Thomas Gubler - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include -#include - -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - - struct { - float min_pwm_mc; //pwm value for idle in mc mode - } _params; - - struct { - param_t min_pwm_mc; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void set_idle_fw(); - void set_idle_mc(); -}; - -namespace VTOL_att_control -{ -VtolAttitudeControl *g_control; -} - -/** -* Constructor -*/ -VtolAttitudeControl::VtolAttitudeControl() : - _task_should_exit(false), - _control_task(-1), - - //init subscription handlers - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), - - //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) -{ - - flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ - - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); - memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); - memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); - memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); - memset(&_armed, 0, sizeof(_armed)); - - _params_handles.min_pwm_mc = param_find("PWM_MIN"); - - /* fetch initial parameter values */ - parameters_update(); -} - -/** -* Destructor -*/ -VtolAttitudeControl::~VtolAttitudeControl() -{ - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - VTOL_att_control::g_control = nullptr; -} - -/** -* Check for changes in vehicle control mode. -*/ -void VtolAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check if vehicle control mode has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -/** -* Check for changes in manual inputs. -*/ -void VtolAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} -/** -* Check for arming status updates. -*/ -void VtolAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - -/** -* Check for inputs from mc attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_mc_poll() -{ - bool updated; - orb_check(_actuator_inputs_mc, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); - } -} - -/** -* Check for inputs from fw attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_fw_poll() -{ - bool updated; - orb_check(_actuator_inputs_fw, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); - } -} - -/** -* Check for parameter updates. -*/ -void -VtolAttitudeControl::parameters_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -/** -* Update parameters. -*/ -int -VtolAttitudeControl::parameters_update() -{ - /* idle pwm */ - float v; - param_get(_params_handles.min_pwm_mc, &v); - _params.min_pwm_mc = v; - - return OK; -} - -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = 0; //roll elevon - _actuators_out_1.control[1] = 0; //pitch elevon -} - -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = _actuators_fw_in.control[0]; /*roll elevon*/ - _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ -} - -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < 4; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = 1100; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < 4; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - VTOL_att_control::g_control->task_main(); -} - -void VtolAttitudeControl::task_main() -{ - warnx("started"); - fflush(stdout); - - /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _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)); - - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); - - parameters_update(); /*initialize parameter cache/* - - /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ - - fds[0].fd = _actuator_inputs_mc; - fds[0].events = POLLIN; - fds[1].fd = _actuator_inputs_fw; - fds[1].events = POLLIN; - fds[2].fd = _params_sub; - fds[2].events = POLLIN; - - while (!_task_should_exit) { - /*Advertise/Publish vtol vehicle status*/ - if (_vtol_vehicle_status_pub > 0) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - if (fds[2].revents & POLLIN) { //parameters were updated, read them now - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - /* update parameters from storage */ - parameters_update(); - } - - vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - vehicle_manual_poll(); //Check for changes in manual inputs. - arming_status_poll(); //Check for arming status updates. - actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - parameters_update_poll(); - - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ - _vtol_vehicle_status.vtol_in_rw_mode = true; - - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; - } - - /* got data from mc_att_controller */ - if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - - fill_mc_att_control_output(); - - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } - } - - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ - _vtol_vehicle_status.vtol_in_rw_mode = false; - - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; - } - - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input - - fill_fw_att_control_output(); - - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } - } - } - - warnx("exit"); - _control_task = -1; - _exit(0); -} - -int -VtolAttitudeControl::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - - -int vtol_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: vtol_att_control {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); - } - - VTOL_att_control::g_control = new VtolAttitudeControl; - - if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); - } - - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - err(1, "start failed"); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); - } - - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; -} From 4915c15036604b56f0d1fa4a42a44c9e6cacbe64 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:58:33 +0100 Subject: [PATCH 039/122] removed files and code segments which should not be there and removed --- ROMFS/px4fmu_common/init.d/rc.autostart | 9 --- ROMFS/px4fmu_common/init.d/rcS | 1 - src/modules/uORB/topics/vtol_vehicle_status.h | 66 ------------------- 3 files changed, 76 deletions(-) delete mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 01aa8ed13b..78778d8066 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -16,7 +16,6 @@ # 10000 .. 10999 Wide arm / H frame # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -# 13000 .. 13999 Vtol # # Simulation setups @@ -239,11 +238,3 @@ if param compare SYS_AUTOSTART 12001 then sh /etc/init.d/12001_octo_cox fi - -# -# Quadshot -# - if param compare SYS_AUTOSTART 13000 - then - sh /etc/init.d/13000_quadshot - fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39172eea4c..ea04ece34d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -589,7 +589,6 @@ then then echo "[init] Starting addons script: $EXTRAS_FILE" sh $EXTRAS_FILE - sh /etc/init.d/Roman_mavlink_stream_conf else echo "[init] No addons script: $EXTRAS_FILE" fi diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h deleted file mode 100644 index 24ecca9faa..0000000000 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * 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 -#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 From 7e350555ea634013f083bccc2f5a8108fc5ef9f9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 12:36:05 +0100 Subject: [PATCH 040/122] checked out from PX4 master to avoid eclipse formatting which happened in the past --- .../fw_att_control/fw_att_control_main.cpp | 398 +++++++++--------- 1 file changed, 195 insertions(+), 203 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 80b58ec714..e770c11a27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,7 +83,8 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl { +class FixedwingAttitudeControl +{ public: /** * Constructor @@ -100,56 +101,54 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { - return _task_running; - } + bool task_running() { return _task_running; } private: - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ - 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 _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) */ - 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 manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ + 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 manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _debug; /**< if set to true, print debug output */ + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -183,14 +182,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ - } _parameters; /**< local copies of interesting parameters */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,71 +228,75 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); + void vehicle_manual_poll(); + /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control { +namespace att_control +{ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -301,28 +304,39 @@ namespace att_control { #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), _task_running(false), _control_task(-1), + _task_should_exit(false), + _task_running(false), + _control_task(-1), - /* subscriptions */ - _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( - -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( - -1), +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _global_pos_sub(-1), + _vehicle_status_sub(-1), - /* publications */ - _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( - -1), +/* publications */ + _rate_sp_pub(-1), + _attitude_sp_pub(-1), + _actuators_0_pub(-1), + _actuators_1_pub(-1), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite output")), - /* states */ - _setpoint_valid(false), _debug(false) { +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), +/* states */ + _setpoint_valid(false), + _debug(false) +{ /* safely initialize structs */ _att = {}; _accel = {}; @@ -335,6 +349,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; + _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -375,7 +390,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() { +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -403,7 +419,9 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() { att_control::g_control = nullptr; } -int FixedwingAttitudeControl::parameters_update() { +int +FixedwingAttitudeControl::parameters_update() +{ param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -411,26 +429,21 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, - &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, - &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, - &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, - &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, - &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -440,19 +453,16 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, - &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, - &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians( - _parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians( - _parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -482,7 +492,9 @@ int FixedwingAttitudeControl::parameters_update() { return OK; } -void FixedwingAttitudeControl::vehicle_control_mode_poll() { +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -490,12 +502,13 @@ void FixedwingAttitudeControl::vehicle_control_mode_poll() { if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, - &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } -void FixedwingAttitudeControl::vehicle_manual_poll() { +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ bool manual_updated; /* get pilots inputs */ @@ -507,7 +520,9 @@ void FixedwingAttitudeControl::vehicle_manual_poll() { } } -void FixedwingAttitudeControl::vehicle_airspeed_poll() { +void +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -518,7 +533,9 @@ void FixedwingAttitudeControl::vehicle_airspeed_poll() { } } -void FixedwingAttitudeControl::vehicle_accel_poll() { +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -528,7 +545,9 @@ void FixedwingAttitudeControl::vehicle_accel_poll() { } } -void FixedwingAttitudeControl::vehicle_setpoint_poll() { +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -539,18 +558,21 @@ void FixedwingAttitudeControl::vehicle_setpoint_poll() { } } -void FixedwingAttitudeControl::global_pos_poll() { +void +FixedwingAttitudeControl::global_pos_poll() +{ /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, - &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } } -void FixedwingAttitudeControl::vehicle_status_poll() { +void +FixedwingAttitudeControl::vehicle_status_poll() +{ /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -560,11 +582,15 @@ void FixedwingAttitudeControl::vehicle_status_poll() { } } -void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ att_control::g_control->task_main(); } -void FixedwingAttitudeControl::task_main() { +void +FixedwingAttitudeControl::task_main() +{ /* inform about start */ warnx("Initializing.."); @@ -641,6 +667,7 @@ void FixedwingAttitudeControl::task_main() { /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -694,8 +721,8 @@ void FixedwingAttitudeControl::task_main() { float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) - || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); @@ -713,9 +740,7 @@ void FixedwingAttitudeControl::task_main() { * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim - / ((airspeed < _parameters.airspeed_min) ? - _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; @@ -731,8 +756,7 @@ void FixedwingAttitudeControl::task_main() { !_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; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -758,12 +782,10 @@ void FixedwingAttitudeControl::task_main() { * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -782,13 +804,11 @@ void FixedwingAttitudeControl::task_main() { /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), - _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise( - ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -803,17 +823,11 @@ void FixedwingAttitudeControl::task_main() { float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; - if (_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n - + _att.R[1][0] * _global_pos.vel_e - + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n - + _att.R[1][1] * _global_pos.vel_e - + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n - + _att.R[1][2] * _global_pos.vel_e - + _att.R[2][2] * _global_pos.vel_d; - } else { + if(_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; + } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } @@ -822,81 +836,63 @@ void FixedwingAttitudeControl::task_main() { /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, - _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, - _roll_ctrl.get_desired_rate(), - _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = - (isfinite(roll_u)) ? - roll_u + _parameters.trim_roll : - _parameters.trim_roll; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("roll_u %.4f", (double) roll_u); + warnx("roll_u %.4f", (double)roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, - _att.pitch, _att.pitchspeed, _att.yawspeed, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = - (isfinite(pitch_u)) ? - pitch_u + _parameters.trim_pitch : - _parameters.trim_pitch; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx( - "pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double) pitch_u, - (double) _yaw_ctrl.get_desired_rate(), - (double) airspeed, - (double) airspeed_scaling, (double) roll_sp, - (double) pitch_sp, - (double) _roll_ctrl.get_desired_rate(), - (double) _pitch_ctrl.get_desired_rate(), - (double) _att_sp.roll_body); + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); } } - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, - _att.pitch, _att.pitchspeed, _att.yawspeed, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = - (isfinite(yaw_u)) ? - yaw_u + _parameters.trim_yaw : - _parameters.trim_yaw; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("yaw_u %.4f", (double) yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } } - /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (isfinite(throttle_sp) && @@ -905,15 +901,13 @@ void FixedwingAttitudeControl::task_main() { throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double) throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { - warnx( - "Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", - (double) roll_sp, (double) pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } @@ -930,13 +924,11 @@ void FixedwingAttitudeControl::task_main() { if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, - &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), - &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { @@ -958,19 +950,16 @@ void FixedwingAttitudeControl::task_main() { if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, - &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), - &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, - &_actuators_airframe); + 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], @@ -978,8 +967,7 @@ void FixedwingAttitudeControl::task_main() { } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), - &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); } } @@ -995,14 +983,18 @@ void FixedwingAttitudeControl::task_main() { _exit(0); } -int FixedwingAttitudeControl::start() { +int +FixedwingAttitudeControl::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, 2048, - (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1012,7 +1004,8 @@ int FixedwingAttitudeControl::start() { return OK; } -int fw_att_control_main(int argc, char *argv[]) { +int fw_att_control_main(int argc, char *argv[]) +{ if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1033,8 +1026,7 @@ int fw_att_control_main(int argc, char *argv[]) { } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr - || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); From 4e0882121e975f19be130c68cacbda3bd817f772 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:07:35 +0100 Subject: [PATCH 041/122] removed ifdef because this file is specific to the nuttx platform --- src/drivers/drv_accel.h | 64 +++++++++++++++++++---------------------- 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 49b104d380..1f98d966bd 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -42,13 +42,12 @@ #include #include -#ifdef CONFIG_ARCH_ARM + #include "drv_sensor.h" #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" -#endif /** * accel report structure. Reads from the device must be in multiples of this * structure. @@ -82,49 +81,46 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -#ifdef CONFIG_ARCH_ARM - ORB_DECLARE(sensor_accel0); - ORB_DECLARE(sensor_accel1); - ORB_DECLARE(sensor_accel2); +ORB_DECLARE(sensor_accel0); +ORB_DECLARE(sensor_accel1); +ORB_DECLARE(sensor_accel2); - /* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ +/* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ - #define _ACCELIOCBASE (0x2100) - #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) +#define _ACCELIOCBASE (0x2100) +#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) - /** set the accel internal sample rate to at least (arg) Hz */ - #define ACCELIOCSSAMPLERATE _ACCELIOC(0) +/** set the accel internal sample rate to at least (arg) Hz */ +#define ACCELIOCSSAMPLERATE _ACCELIOC(0) - /** return the accel internal sample rate in Hz */ - #define ACCELIOCGSAMPLERATE _ACCELIOC(1) +/** return the accel internal sample rate in Hz */ +#define ACCELIOCGSAMPLERATE _ACCELIOC(1) - /** set the accel internal lowpass filter to no lower than (arg) Hz */ - #define ACCELIOCSLOWPASS _ACCELIOC(2) +/** set the accel internal lowpass filter to no lower than (arg) Hz */ +#define ACCELIOCSLOWPASS _ACCELIOC(2) - /** return the accel internal lowpass filter in Hz */ - #define ACCELIOCGLOWPASS _ACCELIOC(3) +/** return the accel internal lowpass filter in Hz */ +#define ACCELIOCGLOWPASS _ACCELIOC(3) - /** set the accel scaling constants to the structure pointed to by (arg) */ - #define ACCELIOCSSCALE _ACCELIOC(5) +/** set the accel scaling constants to the structure pointed to by (arg) */ +#define ACCELIOCSSCALE _ACCELIOC(5) - /** get the accel scaling constants into the structure pointed to by (arg) */ - #define ACCELIOCGSCALE _ACCELIOC(6) +/** get the accel scaling constants into the structure pointed to by (arg) */ +#define ACCELIOCGSCALE _ACCELIOC(6) - /** set the accel measurement range to handle at least (arg) g */ - #define ACCELIOCSRANGE _ACCELIOC(7) +/** set the accel measurement range to handle at least (arg) g */ +#define ACCELIOCSRANGE _ACCELIOC(7) - /** get the current accel measurement range in g */ - #define ACCELIOCGRANGE _ACCELIOC(8) +/** get the current accel measurement range in g */ +#define ACCELIOCGRANGE _ACCELIOC(8) - /** get the result of a sensor self-test */ - #define ACCELIOCSELFTEST _ACCELIOC(9) - -#endif +/** get the result of a sensor self-test */ +#define ACCELIOCSELFTEST _ACCELIOC(9) #endif /* _DRV_ACCEL_H */ From 6b61e725916f90cabd9e8c07144dfcc5ac7b44f6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:08:23 +0100 Subject: [PATCH 042/122] added header file with platformspecific definitions --- src/platforms/px4_defines.h | 56 +++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 src/platforms/px4_defines.h diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 0000000000..8d8fd5f3ce --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 px4_defines.h + * + * Generally used magic defines + */ + +#pragma once + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +//#define PX4_MAIN_FUNCTION(_prefix) +#else +#include +//#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } + +#include "drv_sensor.h" +#include "drv_orb_dev.h" +#define ACCEL_DEVICE_PATH "/dev/accel" + +#endif From 3bfc4a5a52a7777088f21674dd8108026f5f057b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:10:36 +0100 Subject: [PATCH 043/122] removed platform specific includes --- src/lib/mathlib/math/Limits.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 713cb51b56..79e127fddd 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,10 +39,7 @@ #pragma once -#ifdef CONFIG_ARCH_ARM -#include -#endif - +#include #include namespace math { From c8ad06ff9966884242a1ce80eddb4a45efc30c50 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:26:24 +0100 Subject: [PATCH 044/122] removed platform specificness --- src/modules/systemlib/err.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index 2a201ee80b..ca13d62655 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -67,7 +67,6 @@ #include -#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT const char *getprogname(void); @@ -87,8 +86,4 @@ __EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, __END_DECLS -#else //we are using ROS (should make a variable!!!) -#include -#define warnx ROS_WARN -#endif #endif From ed409fd53757a43b49758d3ed5573e809a23b113 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 14:55:33 +0100 Subject: [PATCH 045/122] use px4_defines header to distinguish platform --- src/lib/geo_lookup/geo_mag_declination.h | 4 ---- src/lib/mathlib/math/Limits.hpp | 2 +- src/lib/mathlib/math/Vector.hpp | 2 +- src/modules/uORB/topics/actuator_armed.h | 8 +++----- src/modules/uORB/topics/actuator_controls.h | 6 +----- src/modules/uORB/topics/airspeed.h | 6 +----- src/modules/uORB/topics/manual_control_setpoint.h | 8 +++----- src/modules/uORB/topics/parameter_update.h | 8 ++------ src/modules/uORB/topics/vehicle_attitude.h | 6 +----- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 +----- src/modules/uORB/topics/vehicle_control_mode.h | 6 +----- src/modules/uORB/topics/vehicle_global_position.h | 7 ++----- src/modules/uORB/topics/vehicle_rates_setpoint.h | 6 +----- src/modules/uORB/topics/vehicle_status.h | 6 +----- src/platforms/px4_defines.h | 4 +++- 15 files changed, 22 insertions(+), 63 deletions(-) diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index d79b784121..0ac062d6d4 100644 --- a/src/lib/geo_lookup/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -40,12 +40,8 @@ #pragma once -#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT float get_mag_declination(float lat, float lon); __END_DECLS -#else -float get_mag_declination(float lat, float lon); -#endif diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 79e127fddd..fca4197b8d 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,7 @@ #pragma once -#include +#include #include namespace math { diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index b0b03980d3..9accd09070 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +//#include #endif namespace math diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 9ec9d10aba..1e10e0ad12 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -42,9 +42,8 @@ #define TOPIC_ACTUATOR_ARMED_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include + /** * @addtogroup topics * @{ @@ -65,7 +64,6 @@ struct actuator_armed_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_armed); -#endif + #endif diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 6c641dbcec..43f7a59ee9 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -47,9 +47,7 @@ #define TOPIC_ACTUATOR_CONTROLS_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ @@ -72,11 +70,9 @@ struct actuator_controls_s { */ /* actuator control sets; this list can be expanded as more controllers emerge */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -#endif #endif diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index 4c115a8113..676c37c778 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,9 +40,7 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #include /** @@ -65,8 +63,6 @@ struct airspeed_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(airspeed); -#endif #endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index af5df69792..15b55648d0 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -41,9 +41,8 @@ #define TOPIC_MANUAL_CONTROL_SETPOINT_H_ #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include + /** * Switch position */ @@ -107,7 +106,6 @@ struct manual_control_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(manual_control_setpoint); -#endif + #endif diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 7afb78d49d..fe9c9070fc 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -40,9 +40,7 @@ #define TOPIC_PARAMETER_UPDATE_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -58,8 +56,6 @@ struct parameter_update_s { * @} */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(parameter_update); -#endif -#endif +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 7780988c84..1df1433ac6 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -44,9 +44,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -89,8 +87,6 @@ struct vehicle_attitude_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8b5a761433..a503aa0c69 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -42,9 +42,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -84,8 +82,6 @@ struct vehicle_attitude_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude_setpoint); -#endif #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 78de55b7d9..2dd8550bc4 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,9 +48,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #include "vehicle_status.h" /** @@ -92,8 +90,6 @@ struct vehicle_control_mode_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_control_mode); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e8f010924d..f7a4324950 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,9 +45,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -83,7 +81,6 @@ struct vehicle_global_position_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_global_position); -#endif + #endif diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index cbfab89d60..e5cecf02b4 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -41,9 +41,7 @@ #define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -64,8 +62,6 @@ struct vehicle_rates_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_rates_setpoint); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6bd156ccd6..8e85b48353 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -53,9 +53,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @{ @@ -252,8 +250,6 @@ struct vehicle_status_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_status); -#endif #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 8d8fd5f3ce..327d0bea1b 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -36,7 +36,6 @@ * * Generally used magic defines */ - #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -45,6 +44,7 @@ */ #define __EXPORT //#define PX4_MAIN_FUNCTION(_prefix) +#define ORB_DECLARE(x) #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } @@ -53,4 +53,6 @@ #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" +#include + #endif From a0e2e4e8b30ef64728ff57e56a28b95527f4a1b0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:01:47 +0100 Subject: [PATCH 046/122] pixhawk specific files --- src/modules/fw_att_control/fw_att_control_base.cpp | 1 - src/modules/fw_att_control/fw_att_control_base.h | 2 -- src/platforms/px4_defines.h | 5 ----- 3 files changed, 8 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index d8ba159693..99780bc7eb 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -56,7 +56,6 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; - _accel = {}; _att_sp = {}; _manual = {}; _airspeed = {}; diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6b2efc46b2..1726c2e3e5 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -54,7 +54,6 @@ #include #include -#include #include class FixedwingAttitudeControlBase @@ -78,7 +77,6 @@ protected: int _control_task; /**< task handle for sensor task */ 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 manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 327d0bea1b..1ff46b97cb 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -48,11 +48,6 @@ #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } - -#include "drv_sensor.h" -#include "drv_orb_dev.h" -#define ACCEL_DEVICE_PATH "/dev/accel" - #include #endif From 02175f4a678575430a6455e2e5d7f44f6cf64665 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:32:20 +0100 Subject: [PATCH 047/122] removed unnecessary diff content --- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 - src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 - src/lib/ecl/ecl.h | 2 +- src/lib/geo/geo.h | 8 ++++---- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index dbcabd8478..0799dbe03b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -53,7 +53,6 @@ #include #include - class __EXPORT ECL_RollController //XXX: create controller superclass { public: diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index c9e80930f3..a360c14b89 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -52,7 +52,6 @@ #include #include - class __EXPORT ECL_YawController //XXX: create controller superclass { public: diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index 662e3a39f5..aa3c5000a4 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,6 @@ */ #include + #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time - diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index ff2d92389f..fd754a1ae6 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,12 +45,12 @@ #pragma once -#ifdef CONFIG_ARCH_ARM + #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" __BEGIN_DECLS -#endif + #include "geo_lookup/geo_mag_declination.h" #include @@ -278,6 +278,6 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); -#ifdef CONFIG_ARCH_ARM + __END_DECLS -#endif + From 8e205e0fef4f7a2d8fc832587ae51b685537991d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:33:02 +0100 Subject: [PATCH 048/122] use px4_config header for multiple platform support --- src/modules/uORB/topics/fence.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index 6f16c51cf8..a61f078ba1 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -42,7 +42,7 @@ #include #include -#include "../uORB.h" +#include /** * @addtogroup topics From 05fadc347e4137ecd409b1a6e79fe961f6070ae5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:33:16 +0100 Subject: [PATCH 049/122] updated --- src/platforms/px4_defines.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1ff46b97cb..535cf67fe9 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -45,6 +45,7 @@ #define __EXPORT //#define PX4_MAIN_FUNCTION(_prefix) #define ORB_DECLARE(x) + #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } From 19bc137cf319a0380f2cab9224d3cb67b39995e8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:34:31 +0100 Subject: [PATCH 050/122] removed unnecessary diff content --- src/lib/geo/geo.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index fd754a1ae6..2311e0a7c9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,7 +45,6 @@ #pragma once - #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" @@ -54,7 +53,6 @@ __BEGIN_DECLS #include "geo_lookup/geo_mag_declination.h" #include -#include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ @@ -278,6 +276,4 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); - __END_DECLS - From 284787e02c7471c1f8d651dced0497e964f33454 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:41:07 +0100 Subject: [PATCH 051/122] removed platform specific included, they are not needed --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 +------- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 ------ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ------ 3 files changed, 1 insertion(+), 19 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d1f79f0ead..926a8db2ac 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -47,12 +47,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_PitchController::ECL_PitchController() : _last_run(0), _tc(0.1f), @@ -81,7 +75,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 30176f92fb..94bd26f03d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -47,12 +47,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 1b4d8486c5..fe03b80651 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -46,12 +46,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_YawController::ECL_YawController() : _last_run(0), _k_p(0.0f), From 076c1e6238127b7ae14ea9119c79bc286394be8f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:43:47 +0100 Subject: [PATCH 052/122] removed unused include --- src/lib/mathlib/math/Quaternion.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index d8acc44434..b3cca30c6c 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -45,10 +45,6 @@ #include -#ifdef CONFIG_ARCH_ARM -#include "../CMSIS/Include/arm_math.h" -#endif - #include "Vector.hpp" #include "Matrix.hpp" From 647ec65bd0a164b4bdb38cfc5073e01695676263 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 16:01:01 +0100 Subject: [PATCH 053/122] added ros specific source file for performance_counter --- src/platforms/ros/perf_counter.cpp | 142 +++++++++++++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100755 src/platforms/ros/perf_counter.cpp diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp new file mode 100755 index 0000000000..aa8d85c60c --- /dev/null +++ b/src/platforms/ros/perf_counter.cpp @@ -0,0 +1,142 @@ +/* + * perf_counter.c + + * + * Created on: Sep 24, 2014 + * Author: roman + */ +#include +#include +#include + + + +perf_counter_t perf_alloc(enum perf_counter_type type, const char *name) +{ + return NULL; +} + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +void perf_free(perf_counter_t handle) +{ + +} + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_count(perf_counter_t handle) +{ + +} + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +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 + * has been called subsequently, no change is made to the counter. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_end(perf_counter_t handle) +{ + +} + +/** + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_cancel(perf_counter_t handle) +{ + +} + +/** + * Reset a performance counter. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +void perf_reset(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to stdout + * + * @param handle The counter to print. + */ +void perf_print_counter(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +void perf_print_counter_fd(int fd, perf_counter_t handle) +{ + +} + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +void perf_print_all(int fd) +{ + +} + +/** + * Reset all of the performance counters. + */ +void perf_reset_all(void) +{ + +} + +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +uint64_t perf_event_count(perf_counter_t handle) +{ + +} + + From 44127363b11a9280de818ff28047d170c487af1f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 12 Nov 2014 16:01:36 +0100 Subject: [PATCH 054/122] added setter and getter functions for use with euroc-gazebo simulator --- .../fw_att_control/fw_att_control_base.cpp | 57 +++++++++++++++++++ .../fw_att_control/fw_att_control_base.h | 6 ++ 2 files changed, 63 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 99780bc7eb..46fef3e67b 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -267,3 +267,60 @@ void FixedwingAttitudeControlBase::control_attitude() { } } + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { + // watch out, still need to see where we modify attitude for the tailsitter case + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _att.q[0] = quat(0); + _att.q[1] = quat(1); + _att.q[2] = quat(2); + _att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _att.R[0][0] = Rot(0,0); + _att.R[1][0] = Rot(1,0); + _att.R[2][0] = Rot(2,0); + _att.R[0][1] = Rot(0,1); + _att.R[1][1] = Rot(1,1); + _att.R[2][1] = Rot(2,1); + _att.R[0][2] = Rot(0,2); + _att.R[1][2] = Rot(1,2); + _att.R[2][2] = Rot(2,2); + + _att.R_valid = true; +} +void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + _att.rollspeed = angular_rate(0); + _att.pitchspeed = angular_rate(1); + _att.yawspeed = angular_rate(2); +} +void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _att_sp.roll_body = control_attitude_thrust_reference(0); + _att_sp.pitch_body = control_attitude_thrust_reference(1); + _att_sp.yaw_body = control_attitude_thrust_reference(2); + _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + _att_sp.R_body[0][0] = Rot_sp(0,0); + _att_sp.R_body[1][0] = Rot_sp(1,0); + _att_sp.R_body[2][0] = Rot_sp(2,0); + _att_sp.R_body[0][1] = Rot_sp(0,1); + _att_sp.R_body[1][1] = Rot_sp(1,1); + _att_sp.R_body[2][1] = Rot_sp(2,1); + _att_sp.R_body[0][2] = Rot_sp(0,2); + _att_sp.R_body[1][2] = Rot_sp(1,2); + _att_sp.R_body[2][2] = Rot_sp(2,2); +} +void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 1726c2e3e5..bde1b755f5 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -140,6 +140,12 @@ protected: void control_attitude(); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + }; #endif /* FW_ATT_CONTROL_BASE_H_ */ From 794a89e711a90a62510f6bdde8ee59511237f6ca Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 10:51:54 +0100 Subject: [PATCH 055/122] added more digits to pi/2 --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 832be66dbf..9fc3c78c8a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -51,7 +51,7 @@ #else #include #include -#define M_PI_2_F 1.570769 +#define M_PI_2_F 1.5707963267948966192 #endif namespace math { From 003b326c7731b0f3b9b139ae008d6e4d07cefab8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 11:16:22 +0100 Subject: [PATCH 056/122] applied fix_code_style.sh --- src/lib/mathlib/math/Matrix.hpp | 113 ++++++++++++++++++-------------- 1 file changed, 64 insertions(+), 49 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9fc3c78c8a..806f5933ad 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -51,10 +51,11 @@ #else #include #include -#define M_PI_2_F 1.5707963267948966192 +#define M_PI_2_F 1.5707963267948966192f #endif -namespace math { +namespace math +{ template class __EXPORT Matrix; @@ -83,9 +84,8 @@ public: * Initializes the elements to zero. */ MatrixBase() : - data {}, - arm_mat {M, N, &data[0][0]} - { + data {}, + arm_mat {M, N, &data[0][0]} { } virtual ~MatrixBase() {}; @@ -94,20 +94,17 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } MatrixBase(const float d[M][N]) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -158,9 +155,10 @@ public: */ bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) { + return false; + } return true; } @@ -170,9 +168,10 @@ public: */ bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) { + return true; + } return false; } @@ -192,8 +191,9 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + for (unsigned int j = 0; j < M; j++) { + res.data[i][j] = -data[i][j]; + } return res; } @@ -205,16 +205,18 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + res.data[i][j] = data[i][j] + m.data[i][j]; + } return res; } Matrix &operator +=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + data[i][j] += m.data[i][j]; + } return *static_cast*>(this); } @@ -226,16 +228,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + for (unsigned int j = 0; j < N; j++) { + res.data[i][j] = data[i][j] - m.data[i][j]; + } return res; } Matrix &operator -=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + data[i][j] -= m.data[i][j]; + } return *static_cast*>(this); } @@ -247,8 +251,9 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + for (unsigned int j = 0; j < N; j++) { + res.data[i][j] = data[i][j] * num; + } return res; @@ -256,8 +261,9 @@ public: Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int j = 0; j < N; j++) { + data[i][j] *= num; + } return *static_cast*>(this); } @@ -266,16 +272,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + for (unsigned int j = 0; j < N; j++) { + res[i][j] = data[i][j] / num; + } return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int j = 0; j < N; j++) { + data[i][j] /= num; + } return *static_cast*>(this); } @@ -290,9 +298,11 @@ public: arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map >(m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map > + (m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; Matrix res(Product.data()); return res; #endif @@ -307,7 +317,8 @@ public: arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); Me.transposeInPlace(); Matrix res(Me.data()); return res; @@ -323,8 +334,9 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea Matrix res(MyInverse.data()); return res; #endif @@ -344,16 +356,18 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + for (unsigned int i = 0; i < n; i++) { + data[i][i] = 1; + } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + for (unsigned int j = 0; j < N; j++) { + printf("%.3f\t", data[i][j]); + } printf(" ]\n"); } @@ -391,8 +405,9 @@ public: arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); Eigen::VectorXf Product = Me * Vec; Vector res(Product.data()); #endif @@ -427,8 +442,8 @@ public: */ Vector<3> operator *(const Vector<3> &v) const { Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); return res; } From d9314653826d36b0a9723d69a291220245b3f699 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 13:48:18 +0100 Subject: [PATCH 057/122] added ifdef guard --- src/lib/mathlib/math/Limits.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index c593936ce6..e16f33bd68 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -45,7 +45,10 @@ namespace math { -#define M_PI_F 3.14159265358979323846 + +#ifndef CONFIG_ARCH_ARM +#define M_PI_F 3.14159265358979323846f +#endif float __EXPORT min(float val1, float val2) { From efe0938e9990225636e5fed78e945ded5f24220f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 13:48:40 +0100 Subject: [PATCH 058/122] removed comment --- src/lib/mathlib/math/Vector.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 9accd09070..57b45e3ab2 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,6 @@ #include "../CMSIS/Include/arm_math.h" #else #include -//#include #endif namespace math From f619dba6f96d22152c1c97216a46a27981ff2472 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 15:33:21 +0100 Subject: [PATCH 059/122] Corrected time_gps_usec values description. Fixes #1474 --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 31e616f4f7..7888a50f4c 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -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 */ }; From 05a87a706a122b8a83becaaa94c70161fb69c82a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 16:17:17 +0100 Subject: [PATCH 060/122] move px4_defines file --- .../flow_position_estimator/flow_position_estimator_main.c | 2 +- src/include/px4.h | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- src/{include => platforms}/px4_defines.h | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename src/{include => platforms}/px4_defines.h (100%) diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 8cc9ed6862..5a23932d3a 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include "flow_position_estimator_params.h" diff --git a/src/include/px4.h b/src/include/px4.h index 45068a6f77..22d661b17c 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -59,7 +59,7 @@ #endif -#include +#include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index db6773b8a0..c0b1bb4040 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,7 +83,7 @@ #include #include #include -#include +#include #include "estimator_23states.h" diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 83fe255711..57c1e72f3c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -75,7 +75,7 @@ #include #include #include -#include +#include /** * Fixedwing attitude control app start / stop handling function diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f441c4a915..e07bcc225f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -90,7 +90,7 @@ #include #include "landingslope.h" #include "mtecs/mTecs.h" -#include +#include static int _control_task = -1; /**< task handle for sensor task */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6682a9c89c..cea847603e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2a601b6306..de6357d31b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" diff --git a/src/include/px4_defines.h b/src/platforms/px4_defines.h similarity index 100% rename from src/include/px4_defines.h rename to src/platforms/px4_defines.h From 905913986a03cdb31d056b278cadc2d6f6421e38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 07:43:08 +0100 Subject: [PATCH 061/122] update comments --- src/platforms/px4_nodehandle.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4153517565..cf47fec592 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -55,7 +55,6 @@ namespace px4 { -//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -71,7 +70,7 @@ public: //XXX empty lists }; - /* Constructor with callback to function */ + /* Subscribe with callback to function */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +78,8 @@ public: _subs.push_back(sub); return sub; } - /* Constructor with callback to class method */ + + /* Subscribe with callback to class method */ template Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); From 7a344b933738d9caedc0fe177cc4615c294eaf51 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:30:49 +0100 Subject: [PATCH 062/122] Display ESC data in the status output --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8147a8b898..7213c72c61 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include @@ -612,10 +614,30 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { printf("%d ", (int)(_outputs.output[i]*1000)); } printf("\n"); + + // ESC status + int esc_sub = orb_subscribe(ORB_ID(esc_status)); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), esc_sub, &esc); + + printf("ESC Status:\n"); + printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d\t", esc.esc[i].esc_address); + printf("%3.2f\t", (double)esc.esc[i].esc_voltage); + printf("%3.2f\t", (double)esc.esc[i].esc_current); + printf("%3.2f\t", (double)esc.esc[i].esc_temperature); + printf("%3.2f\t", (double)esc.esc[i].esc_setpoint); + printf("%d\t", esc.esc[i].esc_rpm); + printf("%d", esc.esc[i].esc_errorcount); + printf("\n"); + } } // Sensor bridges From f3fb32bc4790673e4a643f341b16d2434423dbea Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:43:17 +0100 Subject: [PATCH 063/122] Unsubscribe from the topic. --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 7213c72c61..60901e0c75 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -638,6 +638,8 @@ UavcanNode::print_info() printf("%d", esc.esc[i].esc_errorcount); printf("\n"); } + + orb_unsubscribe(esc_sub); } // Sensor bridges From 6a99b04fb74f6da3faea54c93d234a9b57d7bd0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 12:31:37 +0100 Subject: [PATCH 064/122] add parameter wrapper macros for ros --- src/examples/subscriber/subscriber.cpp | 8 ++++++++ src/platforms/px4_defines.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a29f9ab934..a8ecd4a7db 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,6 +74,14 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; + /* Define a parameter */ + PX4_PARAM_INIT("SUB_INTERV", 100); + + /* Read the parameter back for testing */ + int32_t sub_interval; + PX4_PARAM_GET("SUB_INTERV", &sub_interval); + PX4_INFO("Param SUB_INTERV = %d", sub_interval); + /** * The subscribe() call is how you tell ROS that you want to receive messages * on a given topic. This invokes a call to the ROS diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2dd57940d1..2d9051aaea 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,6 +51,8 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); +#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) #else /* * Building for NuttX From 1c79f0cef1f21cff7935ddd7caf048fd96991eea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 14:43:03 +0100 Subject: [PATCH 065/122] improve param wrapper for ros, prepare for px4 --- src/examples/subscriber/subscriber.cpp | 12 ++++++++---- src/include/px4.h | 1 + src/platforms/px4_defines.h | 17 +++++++++++++++-- src/platforms/px4_nodehandle.h | 1 + 4 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a8ecd4a7db..e52d661a99 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,13 +74,17 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; - /* Define a parameter */ - PX4_PARAM_INIT("SUB_INTERV", 100); + /* Define parameters */ + px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); + px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); /* Read the parameter back for testing */ int32_t sub_interval; - PX4_PARAM_GET("SUB_INTERV", &sub_interval); + float test_float; + PX4_PARAM_GET(p_sub_interv, &sub_interval); PX4_INFO("Param SUB_INTERV = %d", sub_interval); + PX4_PARAM_GET(p_test_float, &test_float); + PX4_INFO("Param SUB_TESTF = %f", (double)test_float); /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -97,7 +101,7 @@ PX4_MAIN_FUNCTION(subscriber) { * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); PX4_INFO("subscribed"); diff --git a/src/include/px4.h b/src/include/px4.h index 22d661b17c..2d5d259648 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -56,6 +56,7 @@ #include #include #include +#include #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2d9051aaea..a10df858a8 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,8 +51,18 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); -#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) +typedef const std::string px4_param_t; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) #else /* * Building for NuttX @@ -65,6 +75,9 @@ #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +typedef param_t px4_param_t +#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index cf47fec592..a2775d69ab 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -99,6 +99,7 @@ public: void spin() { ros::spin(); } void spinOnce() { ros::spinOnce(); } + private: static const uint32_t kQueueSizeDefault = 1000; std::list _subs; From 924350a5de1a609e470618ef78212bf7b0044c33 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 15:11:27 +0100 Subject: [PATCH 066/122] bring up param wrapper for px4, moved global include file --- src/examples/subscriber/module.mk | 3 +- src/examples/subscriber/subscriber.cpp | 2 +- src/examples/subscriber/subscriber_params.c | 55 +++++++++++++++++++ src/include/px4.h | 22 +------- src/platforms/px4_defines.h | 4 +- src/platforms/px4_includes.h | 61 +++++++++++++++++++++ 6 files changed, 123 insertions(+), 24 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.c create mode 100644 src/platforms/px4_includes.h diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 481c36ea70..90b4d8ffc5 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp +SRCS = subscriber.cpp \ + subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index e52d661a99..cc9b7a7942 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -84,7 +84,7 @@ PX4_MAIN_FUNCTION(subscriber) { PX4_PARAM_GET(p_sub_interv, &sub_interval); PX4_INFO("Param SUB_INTERV = %d", sub_interval); PX4_PARAM_GET(p_test_float, &test_float); - PX4_INFO("Param SUB_TESTF = %f", (double)test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); /** * The subscribe() call is how you tell ROS that you want to receive messages diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c new file mode 100644 index 0000000000..e28bfbc363 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler + */ + +#include + +/** + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example + */ +PARAM_DEFINE_INT32(SUB_INTERV, 100); + +/** + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); diff --git a/src/include/px4.h b/src/include/px4.h index 2d5d259648..ca702d63ce 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -39,27 +39,7 @@ #pragma once -#include - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#include "ros/ros.h" -#include "px4/rc_channels.h" - -#else -/* - * Building for NuttX - */ -#include -#include -#include -#include -#include - -#endif - +#include "../platforms/px4_includes.h" #include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a10df858a8..a715075108 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -63,10 +63,12 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + #else /* * Building for NuttX */ +#include #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) #define PX4_WARN warnx #define PX4_WARN warnx @@ -75,7 +77,7 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -typedef param_t px4_param_t +typedef param_t px4_param_t; #define PX4_PARAM_INIT(_name, _default) param_find(_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h new file mode 100644 index 0000000000..a3b59b766e --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 px4_includes.h + * + * Includes headers depending on the build target + */ + +#pragma once + +#include + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#include "ros/ros.h" +#include "px4/rc_channels.h" + +#else +/* + * Building for NuttX + */ +#include +#include +#include +#include +#include + +#endif From c2e2b3d52fa7bcfbbec110b5555d2c437657f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 17:04:15 +0100 Subject: [PATCH 067/122] make param wrapper macros compatible for px4 and ros, needs cleanup --- makefiles/setup.mk | 6 ++- src/examples/subscriber/subscriber.cpp | 5 ++- src/examples/subscriber/subscriber_params.c | 7 ++-- src/examples/subscriber/subscriber_params.h | 43 +++++++++++++++++++++ src/platforms/px4_defines.h | 21 +++++++--- 5 files changed, 70 insertions(+), 12 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.h diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a0876..435c240bfa 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -37,13 +37,14 @@ # Some useful paths. # # Note that in general we always keep directory paths with the separator -# at the end, and join paths without explicit separators. This reduces +# at the end, and join paths without explicit separators. This reduces # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ +export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ @@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ $(PX4_INCLUDE_DIR) \ - $(PX4_LIB_DIR) + $(PX4_LIB_DIR) \ + $(PX4_PLATFORMS_DIR) # # Tools diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index cc9b7a7942..68003b6dfc 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,6 +26,7 @@ */ #include +#include "subscriber_params.h" using namespace px4; @@ -75,8 +76,8 @@ PX4_MAIN_FUNCTION(subscriber) { px4::NodeHandle n; /* Define parameters */ - px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); - px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); + px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); /* Read the parameter back for testing */ int32_t sub_interval; diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index e28bfbc363..0d36f5021d 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -38,18 +38,19 @@ * @author Thomas Gubler */ -#include +#include +#include "subscriber_params.h" /** * Interval of one subscriber in the example in ms * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, 100); +PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); +PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 0000000000..3f3ff5bce8 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 subscriber_params.h + * Default parameters for the subscriber example + * + * @author Thomas Gubler + */ +#pragma once + +#define SUB_INTERV_DEFAULT_VALUE 100 +#define SUB_TESTF_DEFAULT_VALUE 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a715075108..6760f57ead 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,11 +39,15 @@ #pragma once +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE + + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT +#include "ros/ros.h" #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO @@ -51,18 +55,21 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -typedef const std::string px4_param_t; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { +typedef const char* px4_param_t; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); return (px4_param_t)name; }; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +#define PX4_PARAM_INT32_T int //XXX #else /* @@ -78,8 +85,9 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) typedef param_t px4_param_t; -#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_INIT(_name) param_find(#_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) +#define PX4_PARAM_INT32_T int32_t #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ @@ -92,3 +100,6 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; +// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; From 83d97fd1c27728145e4869f957afcc7cae57ff8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 09:53:37 +0100 Subject: [PATCH 068/122] better support for param default values on ros and px4 --- src/examples/subscriber/subscriber_params.c | 4 ++-- src/examples/subscriber/subscriber_params.h | 4 ++-- src/platforms/px4_defines.h | 4 +++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index 0d36f5021d..a43817b5b9 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -46,11 +46,11 @@ * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h index 3f3ff5bce8..f6f1d6ba0c 100644 --- a/src/examples/subscriber/subscriber_params.h +++ b/src/examples/subscriber/subscriber_params.h @@ -39,5 +39,5 @@ */ #pragma once -#define SUB_INTERV_DEFAULT_VALUE 100 -#define SUB_TESTF_DEFAULT_VALUE 3.14f +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6760f57ead..09338acabf 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,7 +39,9 @@ #pragma once -#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT +#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) From 8e8f84bde0d2e15734d931ea38a7b294a06d7314 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 10:39:24 +0100 Subject: [PATCH 069/122] uorb topic header generator: only create new files if the file content really changed --- Makefile | 5 +- Tools/px_generate_uorb_topic_headers.py | 121 +++++++++++++++++------- 2 files changed, 90 insertions(+), 36 deletions(-) diff --git a/Makefile b/Makefile index 7df8004a40..bdbc18be54 100644 --- a/Makefile +++ b/Makefile @@ -227,12 +227,15 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" $(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ - -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR)) + -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) +# clean up temporary files + $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) # # Testing targets diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index a738dcb7ef..2ddbd69847 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -39,6 +39,8 @@ message files """ from __future__ import print_function import os +import shutil +import filecmp import argparse import genmsg.template_tools @@ -55,44 +57,93 @@ package = 'px4' def convert_file(filename, outputdir, templatedir, includepath): - """ - Converts a single .msg file to a uorb header - """ - print("Generating uORB headers from {0}".format(filename)) - genmsg.template_tools.generate_from_file(filename, - package, - outputdir, - templatedir, - includepath, - msg_template_map, - srv_template_map) + """ + Converts a single .msg file to a uorb header + """ + print("Generating uORB headers from {0}".format(filename)) + genmsg.template_tools.generate_from_file(filename, + package, + outputdir, + templatedir, + includepath, + msg_template_map, + srv_template_map) def convert_dir(inputdir, outputdir, templatedir): - """ - Converts all .msg files in inputdir to uORB header files - """ - includepath = incl_default + [':'.join([package, inputdir])] - for f in os.listdir(inputdir): - fn = os.path.join(inputdir, f) - if os.path.isfile(fn): - convert_file(fn, outputdir, templatedir, includepath) + """ + Converts all .msg files in inputdir to uORB header files + """ + includepath = incl_default + [':'.join([package, inputdir])] + for f in os.listdir(inputdir): + fn = os.path.join(inputdir, f) + if os.path.isfile(fn): + convert_file( + fn, + outputdir, + templatedir, + includepath) + + +def copy_changed(inputdir, outputdir): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + # Check if f exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, f) + if not os.path.isfile(fno): + shutil.copy(fni, fno) + print("{0}: new header file".format(f)) + continue + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + print("{0}: updated".format(f)) + continue + + print("{0}: unchanged".format(f)) + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + """ + Converts all .msg files in inputdir to uORB header files + Unchanged existing files are not overwritten. + """ + # Create new headers in temporary output directory + convert_dir(inputdir, temporarydir, templatedir) + + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir) if __name__ == "__main__": - parser = argparse.ArgumentParser( - description='Convert msg files to uorb headers') - parser.add_argument('-d', dest='dir', help='directory with msg files') - parser.add_argument('-f', dest='file', - help="files to convert (use only without -d)", - nargs="+") - parser.add_argument('-e', dest='templatedir', - help='directory with template files',) - parser.add_argument('-o', dest='outputdir', - help='output directory for header files') - args = parser.parse_args() + parser = argparse.ArgumentParser( + description='Convert msg files to uorb headers') + parser.add_argument('-d', dest='dir', help='directory with msg files') + parser.add_argument('-f', dest='file', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-e', dest='templatedir', + help='directory with template files',) + parser.add_argument('-o', dest='outputdir', + help='output directory for header files') + parser.add_argument('-t', dest='temporarydir', + help='temporary directory') + args = parser.parse_args() - if args.file is not None: - for f in args.file: - convert_file(f, args.outputdir, args.templatedir, incl_default) - elif args.dir is not None: - convert_dir(args.dir, args.outputdir, args.templatedir) + if args.file is not None: + for f in args.file: + convert_file( + f, + args.outputdir, + args.templatedir, + incl_default) + elif args.dir is not None: + convert_dir_save( + args.dir, + args.outputdir, + args.templatedir, + args.temporarydir) From befe4c399e1f0f2864f91ddde4db585dabf3db99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 11:50:59 +0100 Subject: [PATCH 070/122] comments for px4 defines --- src/platforms/px4_defines.h | 53 +++++++++++++++++++++++++++++-------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 09338acabf..5858a69d86 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -38,8 +38,10 @@ */ #pragma once - +/* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT + +/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */ #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) @@ -50,14 +52,28 @@ */ #define __EXPORT #include "ros/ros.h" +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) + +/* Print/output wrappers */ #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO + +/* Topic Handle */ #define PX4_TOPIC(_name) #_name + +/* Topic type */ #define PX4_TOPIC_T(_name) _name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); + +/* Parameter handle datatype */ typedef const char* px4_param_t; + +/* Helper fucntions to set ROS params, only int and float supported */ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); return (px4_param_t)name; @@ -66,35 +82,53 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; -// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; + +/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ #define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) -// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) + +/* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) -#define PX4_PARAM_INT32_T int //XXX #else /* * Building for NuttX */ #include +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx + +/* Print/output wrappers */ #define PX4_WARN warnx #define PX4_INFO warnx + +/* Topic Handle */ #define PX4_TOPIC(_name) ORB_ID(_name) + +/* Topic type */ #define PX4_TOPIC_T(_name) _name##_s + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) + +/* Parameter handle datatype */ typedef param_t px4_param_t; + +/* Initialize a param, get param handle */ #define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) -#define PX4_PARAM_INT32_T int32_t #endif -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) + +/* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) /* wrapper for 2d matrices */ @@ -102,6 +136,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; From 0f094d35d5dd183e44148eb1acbd7b7d76fde669 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 12:22:21 +0100 Subject: [PATCH 071/122] nodehandle: add documentation comments --- src/platforms/px4_nodehandle.h | 54 ++++++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a2775d69ab..1762e16568 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -70,7 +70,11 @@ public: //XXX empty lists }; - /* Subscribe with callback to function */ + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +83,11 @@ public: return sub; } - /* Subscribe with callback to class method */ + /** + * Subscribe with callback to class method + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); @@ -88,6 +96,10 @@ public: return sub; } + /** + * Advertise topic + * @param topic Name of the topic + */ template Publisher * advertise(const char *topic) { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); @@ -96,14 +108,21 @@ public: return pub; } - void spin() { ros::spin(); } - + /** + * Calls all callback waiting to be called + */ void spinOnce() { ros::spinOnce(); } + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ + void spin() { ros::spin(); } + + private: - static const uint32_t kQueueSizeDefault = 1000; - std::list _subs; - std::list _pubs; + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -117,6 +136,13 @@ public: ~NodeHandle() {}; + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + template Subscriber * subscribe(const struct orb_metadata *meta, std::function callback, @@ -130,6 +156,10 @@ public: return (Subscriber*)sub_px4; } + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ template Publisher * advertise(const struct orb_metadata *meta) { //XXX @@ -137,6 +167,9 @@ public: return pub; } + /** + * Calls all callback waiting to be called + */ void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); @@ -153,6 +186,9 @@ public: } } + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { while (ok()) { const int timeout_ms = 100; @@ -176,8 +212,8 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; - List _pubs; + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; From e0bb713bb03c0aa79e69496c787c012a8e1b78d1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:06:38 +0100 Subject: [PATCH 072/122] more documentation comments --- src/platforms/px4_middleware.h | 13 +++++++++++++ src/platforms/px4_publisher.h | 28 ++++++++++++++++++++++++---- src/platforms/px4_subscriber.h | 32 +++++++++++++++++++++++++++++--- 3 files changed, 66 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index dece729070..c1465b4b1b 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Returns true if the app/task should continue to run + */ bool ok() { return ros::ok(); } #else extern bool task_should_exit; +/** + * Returns true if the app/task should continue to run + */ bool ok() { return !task_should_exit; } #endif @@ -60,8 +66,15 @@ class Rate { public: + /** + * Construct the Rate object and set rate + * @param rate_hz rate from which sleep time is calculated in Hz + */ explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + /** + * Sleep for 1/rate_hz s + */ void sleep() { usleep(sleep_interval); } private: diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 9ce211d250..b67421066a 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -52,33 +52,53 @@ namespace px4 class Publisher { public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template int publish(const M &msg) { _ros_pub.publish(msg); return 0; } private: - ros::Publisher _ros_pub; + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : public uORB::PublicationNode { public: + /** + * Construct Publisher by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param list publisher is added to this list + */ Publisher(const struct orb_metadata *meta, List * list) : uORB::PublicationNode(meta, list) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template int publish(const M &msg) { uORB::PublicationBase::update((void*)&msg); return 0; } - void update() { - //XXX list traversal callback, needed? - } ; + /** + * Empty callback for list traversal + */ + void update() {} ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6e0ef8aed6..fdd0367d59 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -54,15 +54,24 @@ namespace px4 class Subscriber { public: + /** + * Construct Subscriber by providing a ros::Subscriber + * @param ros_sub the ros subscriber which will be used to perform the publications + */ Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) {} + ~Subscriber() {}; private: - ros::Subscriber _ros_sub; + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ }; + #else -// typedef std::function CallbackFunction; + +/** + * Subscriber class which is used by nodehandle when building for NuttX + */ class Subscriber { public: @@ -71,12 +80,22 @@ public: private: }; +/** + * Subscriber class that is templated with the uorb subscription message type + */ template class SubscriberPX4 : public Subscriber, public uORB::Subscription { public: + /** + * Construct SubscriberPX4 by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ SubscriberPX4(const struct orb_metadata *meta, unsigned interval, std::function callback, @@ -86,8 +105,14 @@ public: _callback(callback) //XXX store callback {} + ~SubscriberPX4() {}; + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ void update() { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ @@ -102,7 +127,8 @@ public: }; private: - std::function _callback; + std::function _callback; /**< Callback handle, + called when new data is available */ }; #endif From f4a326c2bf7af6eac86983cd65e66ff76e623e22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:20:12 +0100 Subject: [PATCH 073/122] platform headers: fix code style --- src/platforms/px4_defines.h | 8 +++--- src/platforms/px4_nodehandle.h | 49 +++++++++++++++++++++------------- src/platforms/px4_publisher.h | 7 ++--- src/platforms/px4_subscriber.h | 11 ++++---- 4 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 5858a69d86..af7188f32c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -71,14 +71,16 @@ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Parameter handle datatype */ -typedef const char* px4_param_t; +typedef const char *px4_param_t; /* Helper fucntions to set ROS params, only int and float supported */ -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ ros::param::set(name, value); return (px4_param_t)name; }; -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ ros::param::set(name, value); return (px4_param_t)name; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 1762e16568..5b7247b203 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -66,7 +66,8 @@ public: _pubs() {} - ~NodeHandle() { + ~NodeHandle() + { //XXX empty lists }; @@ -76,9 +77,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber * subscribe(const char *topic, void(*fp)(M)) { + Subscriber *subscribe(const char *topic, void(*fp)(M)) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } @@ -89,9 +91,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { + Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } @@ -101,7 +104,8 @@ public: * @param topic Name of the topic */ template - Publisher * advertise(const char *topic) { + Publisher *advertise(const char *topic) + { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); @@ -121,8 +125,8 @@ public: private: static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ - std::list _subs; /**< Subcriptions of node */ - std::list _pubs; /**< Publications of node */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -144,16 +148,18 @@ public: */ template - Subscriber * subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) { + Subscriber *subscribe(const struct orb_metadata *meta, + std::function callback, + unsigned interval) + { SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { _sub_min_interval = sub_px4; } - return (Subscriber*)sub_px4; + + return (Subscriber *)sub_px4; } /** @@ -161,16 +167,18 @@ public: * @param meta Describes the topic which is advertised */ template - Publisher * advertise(const struct orb_metadata *meta) { + Publisher *advertise(const struct orb_metadata *meta) + { //XXX - Publisher * pub = new Publisher(meta, &_pubs); + Publisher *pub = new Publisher(meta, &_pubs); return pub; } /** * Calls all callback waiting to be called */ - void spinOnce() { + void spinOnce() + { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; @@ -189,9 +197,11 @@ public: /** * Keeps calling callbacks for incomming messages, returns when module is terminated */ - void spin() { + void spin() + { while (ok()) { const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ if (_sub_min_interval == nullptr) { usleep(timeout_ms * 1000); @@ -202,6 +212,7 @@ public: struct pollfd pfd; pfd.fd = _sub_min_interval->getHandle(); pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { /* timed out */ continue; @@ -212,9 +223,9 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ + uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index b67421066a..cb15eeb58f 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -80,7 +80,7 @@ public: * @param list publisher is added to this list */ Publisher(const struct orb_metadata *meta, - List * list) : + List *list) : uORB::PublicationNode(meta, list) {} @@ -90,8 +90,9 @@ public: * @param msg the message which is published to the topic */ template - int publish(const M &msg) { - uORB::PublicationBase::update((void*)&msg); + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); return 0; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index fdd0367d59..e995c6e49d 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -97,9 +97,9 @@ public: * @param list subscriber is added to this list */ SubscriberPX4(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List * list) : + unsigned interval, + std::function callback, + List *list) : Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) @@ -113,7 +113,8 @@ public: * Invoked by the list traversal in NodeHandle::spinOnce * If new data is available the callback is called */ - void update() { + void update() + { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -127,7 +128,7 @@ public: }; private: - std::function _callback; /**< Callback handle, + std::function _callback; /**< Callback handle, called when new data is available */ }; From 6f9cbd97510dce4d0bfe715fa77af3b99d282659 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 16:40:46 +0100 Subject: [PATCH 074/122] add genmsg and gencpp python modules, ros not required anymore for message generation --- .gitmodules | 6 ++++++ Makefile | 7 +++++-- Tools/gencpp | 1 + Tools/genmsg | 1 + 4 files changed, 13 insertions(+), 2 deletions(-) create mode 160000 Tools/gencpp create mode 160000 Tools/genmsg diff --git a/.gitmodules b/.gitmodules index 4b84afac2a..4996b274bd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,9 @@ [submodule "uavcan"] path = uavcan url = git://github.com/pavel-kirienko/uavcan.git +[submodule "Tools/genmsg"] + path = Tools/genmsg + url = https://github.com/ros/genmsg.git +[submodule "Tools/gencpp"] + path = Tools/gencpp + url = https://github.com/ros/gencpp.git diff --git a/Makefile b/Makefile index bdbc18be54..f2e467e5aa 100644 --- a/Makefile +++ b/Makefile @@ -228,12 +228,15 @@ MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary +GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" - $(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ - -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) # clean up temporary files $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) diff --git a/Tools/gencpp b/Tools/gencpp new file mode 160000 index 0000000000..26a86f04bc --- /dev/null +++ b/Tools/gencpp @@ -0,0 +1 @@ +Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a diff --git a/Tools/genmsg b/Tools/genmsg new file mode 160000 index 0000000000..72f0383f0e --- /dev/null +++ b/Tools/genmsg @@ -0,0 +1 @@ +Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454 From 4c950eb76b0d63be7936dfcd68eb6eed266b91ad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 08:57:24 +0100 Subject: [PATCH 075/122] mc att control: make the main app use the base class, move euroc functions into own class --- .../mc_att_control/mc_att_control_base.cpp | 120 +------ .../mc_att_control/mc_att_control_base.h | 17 +- .../mc_att_control/mc_att_control_main.cpp | 300 ++---------------- .../mc_att_control/mc_att_control_sim.cpp | 107 +++++++ .../mc_att_control/mc_att_control_sim.h | 95 ++++++ src/modules/mc_att_control/module.mk | 3 +- 6 files changed, 241 insertions(+), 401 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index baf2bfe65c..d21deb7157 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -47,13 +47,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _task_should_exit(false), + _control_task(-1), - _task_should_exit(false), _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), - _actuators_0_circuit_breaker_enabled(false), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _publish_att_sp(false) { memset(&_v_att, 0, sizeof(_v_att)); @@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); - - // setup standard gains - _params.att_p(0) = 5.0; - _params.rate_p(0) = 0.05; - _params.rate_i(0) = 0.0; - _params.rate_d(0) = 0.003; - /* pitch gains */ - _params.att_p(1) = 5.0; - _params.rate_p(1) = 0.05; - _params.rate_i(1) = 0.0; - _params.rate_d(1) = 0.003; - /* yaw gains */ - _params.att_p(2) = 2.8; - _params.rate_p(2) = 0.2; - _params.rate_i(2) = 0.1; - _params.rate_d(2) = 0.0; - _params.yaw_rate_max = 0.5; - _params.yaw_ff = 0.5; - _params.man_roll_max = 0.6; - _params.man_pitch_max = 0.6; - _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { } -void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { -} - void MulticopterAttitudeControlBase::control_attitude(float dt) { float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + _publish_att_sp = false; if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ @@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; + _publish_att_sp = true; } if (!_armed.armed) { @@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _reset_yaw_sp = false; _v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } if (!_v_control_mode.flag_control_velocity_enabled) { @@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } } else { @@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.R_valid = true; } -// /* publish the attitude setpoint if needed */ -// if (publish_att_sp) { -// _v_att_sp.timestamp = hrt_absolute_time(); -// -// if (_att_sp_pub > 0) { -// orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, -// &_v_att_sp); -// -// } else { -// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), -// &_v_att_sp); -// } -// } - /* rotation matrix for current state */ math::Matrix<3, 3> R; R.set(_v_att.R); @@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } - -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _v_att.q[0] = quat(0); - _v_att.q[1] = quat(1); - _v_att.q[2] = quat(2); - _v_att.q[3] = quat(3); - - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); - - _v_att.R_valid = true; -} - -void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { - // check if this is consistent !!! - _v_att.rollspeed = angular_rate(0); - _v_att.pitchspeed = angular_rate(1); - _v_att.yawspeed = angular_rate(2); -} - -void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { - _v_att_sp.roll_body = control_attitude_thrust_reference(0); - _v_att_sp.pitch_body = control_attitude_thrust_reference(1); - _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; - - // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); -} - -void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} - diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 515fb0c14a..009f762689 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -83,12 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); - - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -105,8 +100,6 @@ protected: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ @@ -130,8 +123,10 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - void vehicle_attitude_setpoint_poll(); //provisional + + bool _publish_att_sp; + + virtual void vehicle_attitude_setpoint_poll() = 0; }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 81a13edb8f..7bc638e5d2 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include "mc_att_control_base.h" /** * Multicopter attitude control app start / stop handling function @@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControl +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase { public: /** @@ -108,7 +110,6 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -124,28 +125,6 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - 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 */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -170,19 +149,7 @@ private: param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - } _params; + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Update our local parameter cache. @@ -219,16 +186,6 @@ private: */ void arming_status_poll(); - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - /** * Shim for calling task_main from task_create. */ @@ -253,11 +210,8 @@ MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ + MulticopterAttitudeControlBase(), + /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { @@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll() } } -/* - * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) - */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; - - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); - } - - if (!_v_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; - } - - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); - } - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - /* reset yaw setpint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _v_att_sp.yaw_body = _v_att.yaw; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_v_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - if (_v_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; - } - - /* publish the attitude setpoint if needed */ - if (publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed.armed) { - _rates_int.zero(); - } - - /* current body angular rates */ - math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; - - /* angular rates error */ - math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } -} - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -787,6 +516,19 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); + /* publish the attitude setpoint if needed */ + if (_publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, + &_v_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), + &_v_att_sp); + } + } + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 0000000000..61a4237fc5 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,107 @@ +/* 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 mc_att_control_base.h + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 0000000000..20752b054d --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,95 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_base.h + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69b..ecd251d45c 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control_base.cpp \ + mc_att_control_params.c From 2f646e7082b49ef92bf9be1defd6fb7fdeec8480 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:02:06 +0100 Subject: [PATCH 076/122] mc att control: ran fix code style script --- .../mc_att_control/mc_att_control_base.cpp | 36 +++++++----- .../mc_att_control/mc_att_control_base.h | 3 +- .../mc_att_control/mc_att_control_main.cpp | 23 +++++--- .../mc_att_control/mc_att_control_sim.cpp | 56 ++++++++++--------- .../mc_att_control/mc_att_control_sim.h | 6 +- 5 files changed, 71 insertions(+), 53 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d21deb7157..a3b0340d2e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _I.identity(); } -MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ } -void MulticopterAttitudeControlBase::control_attitude(float dt) { +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; @@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* manual input, set or modify attitude setpoint */ if (_v_control_mode.flag_control_velocity_enabled - || _v_control_mode.flag_control_climb_rate_enabled) { + || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ vehicle_attitude_setpoint_poll(); } @@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi( - _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.yaw_body + yaw_sp_move_rate * dt); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } + _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* update attitude setpoint if not in position control mode */ _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; _v_att_sp.pitch_body = -_manual_control_sp.x - * _params.man_pitch_max; + * _params.man_pitch_max; _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* copy rotation matrix back to setpoint struct */ memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], - sizeof(_v_att_sp.R_body)); + sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* rotation matrix for roll/pitch only rotation */ R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, - _params.yaw_rate_max); + _params.yaw_rate_max); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } -void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ /* reset integral if disarmed */ if (!_armed.armed) { _rates_int.zero(); @@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) - + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; /* update integral only if not saturated on low limit */ @@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) - + _params.rate_i(i) * rates_err(i) * dt; + + _params.rate_i(i) * rates_err(i) * dt; if (isfinite( - rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } @@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_actuator_controls() { +void MulticopterAttitudeControlBase::set_actuator_controls() +{ _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 009f762689..e7793e624c 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -64,7 +64,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControlBase { +class MulticopterAttitudeControlBase +{ public: /** * Constructor diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7bc638e5d2..d6a58f4186 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -477,8 +477,9 @@ MulticopterAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -522,10 +523,11 @@ MulticopterAttitudeControl::task_main() if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); + &_v_att_sp); + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); + &_v_att_sp); } } @@ -547,7 +549,8 @@ MulticopterAttitudeControl::task_main() /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = _manual_control_sp.z; /* reset yaw setpoint after ACRO */ @@ -630,18 +633,21 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) { errx(1, "already running"); + } mc_att_control::g_control = new MulticopterAttitudeControl; - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; @@ -653,8 +659,9 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "not running"); + } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 61a4237fc5..faf7294208 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,7 +46,8 @@ using namespace std; #endif -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion _v_att.q[2] = quat(2); _v_att.q[3] = quat(3); - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); _v_att.R_valid = true; } -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ // check if this is consistent !!! _v_att.rollspeed = angular_rate(0); _v_att.pitchspeed = angular_rate(1); _v_att.yawspeed = angular_rate(2); } -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); } -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ motor_inputs(0) = _actuators.control[0]; motor_inputs(1) = _actuators.control[1]; motor_inputs(2) = _actuators.control[2]; diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 20752b054d..0b48455af0 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -82,9 +82,9 @@ public: /* setters and getters for interface with euroc-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); protected: void vehicle_attitude_setpoint_poll() {}; From eafc4b5f9eefabca5fba404d75b1e1425b2f579b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 08:57:24 +0100 Subject: [PATCH 077/122] mc att control: make the main app use the base class, move euroc functions into own class --- .../mc_att_control/mc_att_control_base.cpp | 120 +------ .../mc_att_control/mc_att_control_base.h | 17 +- .../mc_att_control/mc_att_control_main.cpp | 300 ++---------------- .../mc_att_control/mc_att_control_sim.cpp | 107 +++++++ .../mc_att_control/mc_att_control_sim.h | 95 ++++++ src/modules/mc_att_control/module.mk | 3 +- 6 files changed, 241 insertions(+), 401 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index baf2bfe65c..d21deb7157 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -47,13 +47,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _task_should_exit(false), + _control_task(-1), - _task_should_exit(false), _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), - _actuators_0_circuit_breaker_enabled(false), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _publish_att_sp(false) { memset(&_v_att, 0, sizeof(_v_att)); @@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); - - // setup standard gains - _params.att_p(0) = 5.0; - _params.rate_p(0) = 0.05; - _params.rate_i(0) = 0.0; - _params.rate_d(0) = 0.003; - /* pitch gains */ - _params.att_p(1) = 5.0; - _params.rate_p(1) = 0.05; - _params.rate_i(1) = 0.0; - _params.rate_d(1) = 0.003; - /* yaw gains */ - _params.att_p(2) = 2.8; - _params.rate_p(2) = 0.2; - _params.rate_i(2) = 0.1; - _params.rate_d(2) = 0.0; - _params.yaw_rate_max = 0.5; - _params.yaw_ff = 0.5; - _params.man_roll_max = 0.6; - _params.man_pitch_max = 0.6; - _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { } -void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { -} - void MulticopterAttitudeControlBase::control_attitude(float dt) { float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + _publish_att_sp = false; if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ @@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; + _publish_att_sp = true; } if (!_armed.armed) { @@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _reset_yaw_sp = false; _v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } if (!_v_control_mode.flag_control_velocity_enabled) { @@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } } else { @@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.R_valid = true; } -// /* publish the attitude setpoint if needed */ -// if (publish_att_sp) { -// _v_att_sp.timestamp = hrt_absolute_time(); -// -// if (_att_sp_pub > 0) { -// orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, -// &_v_att_sp); -// -// } else { -// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), -// &_v_att_sp); -// } -// } - /* rotation matrix for current state */ math::Matrix<3, 3> R; R.set(_v_att.R); @@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } - -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _v_att.q[0] = quat(0); - _v_att.q[1] = quat(1); - _v_att.q[2] = quat(2); - _v_att.q[3] = quat(3); - - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); - - _v_att.R_valid = true; -} - -void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { - // check if this is consistent !!! - _v_att.rollspeed = angular_rate(0); - _v_att.pitchspeed = angular_rate(1); - _v_att.yawspeed = angular_rate(2); -} - -void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { - _v_att_sp.roll_body = control_attitude_thrust_reference(0); - _v_att_sp.pitch_body = control_attitude_thrust_reference(1); - _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; - - // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); -} - -void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} - diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 515fb0c14a..009f762689 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -83,12 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); - - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -105,8 +100,6 @@ protected: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ @@ -130,8 +123,10 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - void vehicle_attitude_setpoint_poll(); //provisional + + bool _publish_att_sp; + + virtual void vehicle_attitude_setpoint_poll() = 0; }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c2..4709f0487b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include "mc_att_control_base.h" /** * Multicopter attitude control app start / stop handling function @@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControl +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase { public: /** @@ -108,7 +110,6 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -124,28 +125,6 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - 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 */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -170,19 +149,7 @@ private: param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - } _params; + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Update our local parameter cache. @@ -219,16 +186,6 @@ private: */ void arming_status_poll(); - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - /** * Shim for calling task_main from task_create. */ @@ -253,11 +210,8 @@ MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ + MulticopterAttitudeControlBase(), + /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { @@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll() } } -/* - * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) - */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; - - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); - } - - if (!_v_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; - } - - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); - } - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - /* reset yaw setpint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _v_att_sp.yaw_body = _v_att.yaw; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_v_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - if (_v_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; - } - - /* publish the attitude setpoint if needed */ - if (publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed.armed) { - _rates_int.zero(); - } - - /* current body angular rates */ - math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; - - /* angular rates error */ - math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } -} - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -789,6 +518,19 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); + /* publish the attitude setpoint if needed */ + if (_publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, + &_v_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), + &_v_att_sp); + } + } + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 0000000000..61a4237fc5 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,107 @@ +/* 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 mc_att_control_base.h + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 0000000000..20752b054d --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,95 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_base.h + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69b..ecd251d45c 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control_base.cpp \ + mc_att_control_params.c From 1c19d75cf429639ff9ab9ff23ad9fbcd1c936e98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:02:06 +0100 Subject: [PATCH 078/122] mc att control: ran fix code style script --- .../mc_att_control/mc_att_control_base.cpp | 36 +++++++----- .../mc_att_control/mc_att_control_base.h | 3 +- .../mc_att_control/mc_att_control_main.cpp | 23 +++++--- .../mc_att_control/mc_att_control_sim.cpp | 56 ++++++++++--------- .../mc_att_control/mc_att_control_sim.h | 6 +- 5 files changed, 71 insertions(+), 53 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d21deb7157..a3b0340d2e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _I.identity(); } -MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ } -void MulticopterAttitudeControlBase::control_attitude(float dt) { +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; @@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* manual input, set or modify attitude setpoint */ if (_v_control_mode.flag_control_velocity_enabled - || _v_control_mode.flag_control_climb_rate_enabled) { + || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ vehicle_attitude_setpoint_poll(); } @@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi( - _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.yaw_body + yaw_sp_move_rate * dt); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } + _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* update attitude setpoint if not in position control mode */ _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; _v_att_sp.pitch_body = -_manual_control_sp.x - * _params.man_pitch_max; + * _params.man_pitch_max; _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* copy rotation matrix back to setpoint struct */ memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], - sizeof(_v_att_sp.R_body)); + sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* rotation matrix for roll/pitch only rotation */ R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, - _params.yaw_rate_max); + _params.yaw_rate_max); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } -void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ /* reset integral if disarmed */ if (!_armed.armed) { _rates_int.zero(); @@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) - + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; /* update integral only if not saturated on low limit */ @@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) - + _params.rate_i(i) * rates_err(i) * dt; + + _params.rate_i(i) * rates_err(i) * dt; if (isfinite( - rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } @@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_actuator_controls() { +void MulticopterAttitudeControlBase::set_actuator_controls() +{ _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 009f762689..e7793e624c 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -64,7 +64,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControlBase { +class MulticopterAttitudeControlBase +{ public: /** * Constructor diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 4709f0487b..7a03553f9c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -479,8 +479,9 @@ MulticopterAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -524,10 +525,11 @@ MulticopterAttitudeControl::task_main() if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); + &_v_att_sp); + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); + &_v_att_sp); } } @@ -549,7 +551,8 @@ MulticopterAttitudeControl::task_main() /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = _manual_control_sp.z; /* reset yaw setpoint after ACRO */ @@ -632,18 +635,21 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) { errx(1, "already running"); + } mc_att_control::g_control = new MulticopterAttitudeControl; - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; @@ -655,8 +661,9 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "not running"); + } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 61a4237fc5..faf7294208 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,7 +46,8 @@ using namespace std; #endif -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion _v_att.q[2] = quat(2); _v_att.q[3] = quat(3); - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); _v_att.R_valid = true; } -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ // check if this is consistent !!! _v_att.rollspeed = angular_rate(0); _v_att.pitchspeed = angular_rate(1); _v_att.yawspeed = angular_rate(2); } -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); } -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ motor_inputs(0) = _actuators.control[0]; motor_inputs(1) = _actuators.control[1]; motor_inputs(2) = _actuators.control[2]; diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 20752b054d..0b48455af0 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -82,9 +82,9 @@ public: /* setters and getters for interface with euroc-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); protected: void vehicle_attitude_setpoint_poll() {}; From 85aa6554ad90e392a9538006f8cc483d481f0bbd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:20 +0100 Subject: [PATCH 079/122] re-add hardcoded parameters for euroc --- .../mc_att_control/mc_att_control_sim.cpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index faf7294208..a8467f6b12 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,6 +46,35 @@ using namespace std; #endif +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { math::Quaternion quat; From 417bc91363c2607906c612089cb4b7197a60f531 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:57 +0100 Subject: [PATCH 080/122] mc att: more cleanup between base and main class --- src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++----- src/modules/mc_att_control/mc_att_control_base.h | 10 +++++----- src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++-- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index a3b0340d2e..305fbaf892 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -32,6 +32,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -47,11 +52,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _task_should_exit(false), - _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), - _publish_att_sp(false) { diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index e7793e624c..662d32ee44 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -35,6 +35,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -88,11 +93,6 @@ public: void set_actuator_controls(); protected: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index d6a58f4186..887a535088 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. @@ -110,8 +113,10 @@ public: int start(); 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, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -211,6 +216,9 @@ MulticopterAttitudeControl *g_control; MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControlBase(), + _task_should_exit(false), + _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), From dfd2098e04761ee65ab09a9a8e6656a334ccae41 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:20 +0100 Subject: [PATCH 081/122] re-add hardcoded parameters for euroc --- .../mc_att_control/mc_att_control_sim.cpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index faf7294208..a8467f6b12 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,6 +46,35 @@ using namespace std; #endif +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { math::Quaternion quat; From ebcad32e377824cb47186a400d22195cacd07e3b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:57 +0100 Subject: [PATCH 082/122] mc att: more cleanup between base and main class --- src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++----- src/modules/mc_att_control/mc_att_control_base.h | 10 +++++----- src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++-- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index a3b0340d2e..305fbaf892 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -32,6 +32,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -47,11 +52,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _task_should_exit(false), - _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), - _publish_att_sp(false) { diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index e7793e624c..662d32ee44 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -35,6 +35,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -88,11 +93,6 @@ public: void set_actuator_controls(); protected: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7a03553f9c..16a3b627fd 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. @@ -110,8 +113,10 @@ public: int start(); 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, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -211,6 +216,9 @@ MulticopterAttitudeControl *g_control; MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControlBase(), + _task_should_exit(false), + _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), From 0f1a7e7b5b699601886adec5215bf818b050a758 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:33:26 +0100 Subject: [PATCH 083/122] whitespace --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 305fbaf892..17270884cc 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 662d32ee44..115667eb9f 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -23,8 +23,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index a8467f6b12..788f92661b 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 0b48455af0..ecc065e68e 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -23,8 +23,8 @@ * 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 +* 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 From c64c184948bed07f354904c0549b93225c85ad58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:33:26 +0100 Subject: [PATCH 084/122] whitespace --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 305fbaf892..17270884cc 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 662d32ee44..115667eb9f 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -23,8 +23,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index a8467f6b12..788f92661b 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 0b48455af0..ecc065e68e 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -23,8 +23,8 @@ * 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 +* 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 From 9c4fc14f230d4ebe2d82cf39ac5f4bd7f04d0c26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 10:32:52 +0100 Subject: [PATCH 085/122] fix base and sim file descriptions --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 2 ++ src/modules/mc_att_control/mc_att_control_sim.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_sim.h | 4 +++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 17270884cc..5e65e65087 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_base.cpp + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 115667eb9f..1c2f3fb6fe 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,6 +34,8 @@ /** * @file mc_att_control_base.h + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 788f92661b..d516d7e528 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index ecc065e68e..a1bf44fc96 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -33,7 +33,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * From 996438aafedbc23e86ca36171dc81ec23d322caa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 10:32:52 +0100 Subject: [PATCH 086/122] fix base and sim file descriptions --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 2 ++ src/modules/mc_att_control/mc_att_control_sim.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_sim.h | 4 +++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 17270884cc..5e65e65087 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_base.cpp + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 115667eb9f..1c2f3fb6fe 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,6 +34,8 @@ /** * @file mc_att_control_base.h + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 788f92661b..d516d7e528 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index ecc065e68e..a1bf44fc96 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -33,7 +33,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * From 52c35a8e20c99de4e7b6e27856d340a3a355250c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Dec 2014 15:36:58 +0100 Subject: [PATCH 087/122] solve conflict for definiton of FILE --- NuttX | 2 +- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/examples/subscriber/subscriber.cpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/NuttX b/NuttX index ae4b05e2c5..dbe128ceaf 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c +Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8 diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a2..b98b1a0afe 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ # Generic warnings # diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 68003b6dfc..0d9ca1fa76 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -27,6 +27,7 @@ #include #include "subscriber_params.h" +#include using namespace px4; From fd01c7fc58db1fca43f06b277cb2a0094440ecb3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Dec 2014 18:43:43 +0100 Subject: [PATCH 088/122] set NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index dbe128ceaf..89a6776fc1 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8 +Subproject commit 89a6776fc1b31d242c637d3f19072609bb98ec6c From 65629d09d5e2424c6fcaf261c95f6d6995c4afd7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 10:35:59 +0100 Subject: [PATCH 089/122] add mc att to cmakelist --- CMakeLists.txt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 636cc07e34..e91f2cc371 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,16 @@ target_link_libraries(subscriber px4 ) +add_executable(mc_att_control + src/modules/mc_att_control/mc_att_control_main.cpp + src/moudles/mc_att_control/mc_att_control_base.cpp) +add_dependencies(mc_att_control px4_generate_messages_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) + + ############# ## Install ## ############# From 87df7c3243412d4fc7a0c40967b2abe078f7a0b2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 10:37:01 +0100 Subject: [PATCH 090/122] move vehicle_attitude_setpoint to msg format --- msg/px4_msgs/vehicle_attitude_setpoint.msg | 21 +++++++ .../fw_att_control/fw_att_control_base.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../mc_att_control/mc_att_control_base.cpp | 4 +- .../mc_att_control/mc_att_control_base.h | 7 +-- .../mc_att_control/mc_att_control_main.cpp | 6 +- .../mc_pos_control/mc_pos_control_main.cpp | 22 +++---- .../uORB/topics/vehicle_attitude_setpoint.h | 59 +++++++------------ src/platforms/px4_defines.h | 1 + src/platforms/px4_includes.h | 1 + src/platforms/px4_middleware.h | 4 +- 11 files changed, 66 insertions(+), 64 deletions(-) create mode 100644 msg/px4_msgs/vehicle_attitude_setpoint.msg diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg new file mode 100644 index 0000000000..1a8e6e3d54 --- /dev/null +++ b/msg/px4_msgs/vehicle_attitude_setpoint.msg @@ -0,0 +1,21 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 46fef3e67b..f543c02f9c 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.cpp - * + * * @author Roman Bapst * */ @@ -40,7 +40,6 @@ #include #include #include -#include using namespace std; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe7..cb55a25aa7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 5e65e65087..f6b3268b59 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -172,7 +172,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ @@ -180,7 +180,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 1c2f3fb6fe..c1ea52f636 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * MC Attitude Controller * * @author Tobias Naegeli @@ -45,23 +45,20 @@ * @author Roman Bapst * */ - +#include #include #include #include #include #include #include -#include -#include #include #include #include #include #include #include -#include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 887a535088..c0e8957e0f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -52,8 +52,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include +#include #include #include #include @@ -62,8 +61,6 @@ #include #include #include -#include -#include #include #include #include @@ -78,6 +75,7 @@ #include #include #include + #include "mc_att_control_base.h" /** diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index cea847603e..4ea5fdfb64 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin */ -#include -#include +#include +#include +#include #include #include #include @@ -54,8 +55,7 @@ #include #include #include -#include -#include + #include #include #include @@ -67,8 +67,8 @@ #include #include #include -#include -#include +// #include +// #include #include #include #include @@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _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) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_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: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +987,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a503aa0c69..29fb104dfb 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2013-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 @@ -32,49 +31,37 @@ * ****************************************************************************/ -/** - * @file vehicle_attitude_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */ -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ + +#pragma once #include -#include -#include +#include "../uORB.h" + + /** * @addtogroup topics * @{ */ -/** - * vehicle attitude setpoint. - */ + struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ - - //! For quaternion-based attitude control - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float thrust; /**< Thrust in Newton the power system should generate */ - - bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ - bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ - bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ - + uint64_t timestamp; + float roll_body; + float pitch_body; + float yaw_body; + float R_body[9]; + bool R_valid; + float q_d[4]; + bool q_d_valid; + float q_e[4]; + bool q_e_valid; + float thrust; + bool roll_reset_integral; + bool pitch_reset_integral; + bool yaw_reset_integral; }; /** @@ -83,5 +70,3 @@ struct vehicle_attitude_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index af7188f32c..ff302448a5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -115,6 +115,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) /* Parameter handle datatype */ +#include typedef param_t px4_param_t; /* Initialize a param, get param handle */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a3b59b766e..a9425077eb 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index c1465b4b1b..b0bc404173 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -53,13 +53,13 @@ __EXPORT uint64_t get_time_micros(); /** * Returns true if the app/task should continue to run */ -bool ok() { return ros::ok(); } +inline bool ok() { return ros::ok(); } #else extern bool task_should_exit; /** * Returns true if the app/task should continue to run */ -bool ok() { return !task_should_exit; } +inline bool ok() { return !task_should_exit; } #endif class Rate From b3600e5ee6fc4550533d65c1c25abceb6db4466e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 11:06:56 +0100 Subject: [PATCH 091/122] manual_control_setpoint as msg --- msg/px4_msgs/manual_control_setpoint.msg | 44 +++++++++ .../mc_att_control/mc_att_control_base.h | 1 - .../mc_att_control/mc_att_control_main.cpp | 1 - src/modules/sensors/sensors.cpp | 8 +- .../uORB/topics/manual_control_setpoint.h | 98 +++++++------------ src/platforms/px4_includes.h | 1 + 6 files changed, 82 insertions(+), 71 deletions(-) create mode 100644 msg/px4_msgs/manual_control_setpoint.msg diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/px4_msgs/manual_control_setpoint.msg new file mode 100644 index 0000000000..d3cfb078be --- /dev/null +++ b/msg/px4_msgs/manual_control_setpoint.msg @@ -0,0 +1,44 @@ + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) +uint64 timestamp + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down +float32 r # yaw stick/twist positon, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle +float32 flaps # flap position +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop + +uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index c1ea52f636..d42d672c00 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c0e8957e0f..a8a3ae6b0b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 672dc52c3d..fca72c3040 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -187,8 +187,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + uint8_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -1512,7 +1512,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value) } } -switch_pos_t +uint8_t Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { @@ -1533,7 +1533,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi } } -switch_pos_t +uint8_t Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 13138ebc9a..440841f903 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -1,21 +1,20 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2013-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. + * 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. + * 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. + * 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 @@ -32,73 +31,44 @@ * ****************************************************************************/ -/** - * @file manual_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */ -#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ -#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ + +#pragma once #include -#include +#include "../uORB.h" + +#define SWITCH_POS_NONE 0 +#define SWITCH_POS_ON 1 +#define SWITCH_POS_MIDDLE 2 +#define SWITCH_POS_OFF 3 -/** - * Switch position - */ -typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ - SWITCH_POS_ON, /**< switch activated (value = 1) */ - SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ - SWITCH_POS_OFF /**< switch not activated (value = -1) */ -} switch_pos_t; /** * @addtogroup topics * @{ */ + struct manual_control_setpoint_s { uint64_t timestamp; - - /** - * Any of the channels may not be available and be set to NaN - * to indicate that it does not contain valid data. - * The variable names follow the definition of the - * MANUAL_CONTROL mavlink message. - * The default range is from -1 to 1 (mavlink message -1000 to 1000) - * The range for the z variable is defined from 0 to 1. (The z field of - * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - */ - float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle, - in general a positive value means forward or negative pitch and - a negative value means backward or positive pitch */ - float y; /**< stick position in y direction -1..1 - in general corresponds to right/left motion or roll of vehicle, - in general a positive value means right or positive roll and - a negative value means left or negative roll */ - float z; /**< throttle stick position 0..1 - in general corresponds to up/down motion or thrust of vehicle, - in general the value corresponds to the demanded throttle by the user, - if the input is used for setting the setpoint of a vertical position - controller any value > 0.5 means up and any value < 0.5 means down */ - float r; /**< yaw stick/twist positon, -1..1 - in general corresponds to the righthand rotation around the vertical - (downwards) axis of the vehicle */ - float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ + float x; + float y; + float z; + float r; + float flaps; + float aux1; + float aux2; + float aux3; + float aux4; + float aux5; + uint8_t mode_switch; + uint8_t return_switch; + uint8_t posctl_switch; + uint8_t loiter_switch; + uint8_t acro_switch; + uint8_t offboard_switch; }; /** @@ -107,5 +77,3 @@ struct manual_control_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(manual_control_setpoint); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a9425077eb..2045bd2a85 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include From dc945a3f3f9f7295c73fc8280d6a0048e059c469 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:12:23 +0100 Subject: [PATCH 092/122] actuator controls as msg --- msg/px4_msgs/actuator_controls.msg | 4 ++ msg/templates/msg.h.template | 3 +- .../mc_att_control/mc_att_control_base.h | 2 +- .../mc_att_control/mc_att_control_main.cpp | 6 --- src/modules/uORB/topics/actuator_controls.h | 38 ++++++------------- src/platforms/px4_defines.h | 7 ++++ src/platforms/px4_includes.h | 4 ++ src/systemcmds/esc_calib/esc_calib.c | 1 + 8 files changed, 30 insertions(+), 35 deletions(-) create mode 100644 msg/px4_msgs/actuator_controls.msg diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/px4_msgs/actuator_controls.msg new file mode 100644 index 0000000000..743f20cdff --- /dev/null +++ b/msg/px4_msgs/actuator_controls.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 5fce506b6b..84dea37a3b 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', - 'bool': 'bool'} + 'bool': 'bool', + 'actuator_controls': 'actuator_controls'} # Function to print a standard ros type def print_field_def(field): diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d42d672c00..c7853a0ecb 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,11 +53,11 @@ #include #include -#include #include #include #include #include +#include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a8a3ae6b0b..6ab4f95cd1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -61,12 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 43f7a59ee9..3e43107adf 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -31,48 +31,32 @@ * ****************************************************************************/ -/** - * @file actuator_controls.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */ -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H + +#pragma once #include -#include +#include "../uORB.h" -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +#define NUM_ACTUATOR_CONTROLS 8 +#define NUM_ACTUATOR_CONTROL_GROUPS 4 -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) /** * @addtogroup topics * @{ */ + struct actuator_controls_s { uint64_t timestamp; - float control[NUM_ACTUATOR_CONTROLS]; + float control[8]; }; /** * @} */ -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); - -#endif +/* register this as object request broker structure */ +ORB_DECLARE(actuator_controls); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ff302448a5..daeff9cf30 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -139,3 +139,10 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +/* Diverese uORB header defiens */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 2045bd2a85..3e94586c14 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -57,6 +57,10 @@ #include #include #include +#include +#include +#include +#include #include #include diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 7d80af3079..20967b541d 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,6 +39,7 @@ */ #include +#include #include #include From fc4b722e729fda1ed45aa214360b5bb9b462a2b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:17:30 +0100 Subject: [PATCH 093/122] fix headers, remove unneded uorb headers --- .../mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/actuator_controls.h | 62 --------------- .../uORB/topics/manual_control_setpoint.h | 79 ------------------- .../uORB/topics/vehicle_attitude_setpoint.h | 72 ----------------- src/platforms/px4_includes.h | 6 +- 5 files changed, 2 insertions(+), 218 deletions(-) delete mode 100644 src/modules/uORB/topics/actuator_controls.h delete mode 100644 src/modules/uORB/topics/manual_control_setpoint.h delete mode 100644 src/modules/uORB/topics/vehicle_attitude_setpoint.h diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index c7853a0ecb..5e9c0c4db6 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h deleted file mode 100644 index 3e43107adf..0000000000 --- a/src/modules/uORB/topics/actuator_controls.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */ - - -#pragma once - -#include -#include "../uORB.h" - -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 - - -/** - * @addtogroup topics - * @{ - */ - - -struct actuator_controls_s { - uint64_t timestamp; - float control[8]; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_controls); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h deleted file mode 100644 index 440841f903..0000000000 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */ - - -#pragma once - -#include -#include "../uORB.h" - -#define SWITCH_POS_NONE 0 -#define SWITCH_POS_ON 1 -#define SWITCH_POS_MIDDLE 2 -#define SWITCH_POS_OFF 3 - - -/** - * @addtogroup topics - * @{ - */ - - -struct manual_control_setpoint_s { - uint64_t timestamp; - float x; - float y; - float z; - float r; - float flaps; - float aux1; - float aux2; - float aux3; - float aux4; - float aux5; - uint8_t mode_switch; - uint8_t return_switch; - uint8_t posctl_switch; - uint8_t loiter_switch; - uint8_t acro_switch; - uint8_t offboard_switch; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(manual_control_setpoint); diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h deleted file mode 100644 index 29fb104dfb..0000000000 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */ - - -#pragma once - -#include -#include "../uORB.h" - - - -/** - * @addtogroup topics - * @{ - */ - - -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; - float roll_body; - float pitch_body; - float yaw_body; - float R_body[9]; - bool R_valid; - float q_d[4]; - bool q_d_valid; - float q_e[4]; - bool q_e_valid; - float thrust; - bool roll_reset_integral; - bool pitch_reset_integral; - bool yaw_reset_integral; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude_setpoint); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 3e94586c14..8b0c374029 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -57,10 +57,8 @@ #include #include #include -#include -#include -#include -#include +#include +#include #include #include From 99d89577cd4c7e663583773eccf0883c1df773c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:23:27 +0100 Subject: [PATCH 094/122] vehicle rates sp as msg --- msg/px4_msgs/vehicle_rates_setpoint.msg | 6 ++ .../uORB/topics/vehicle_rates_setpoint.h | 67 ------------------- 2 files changed, 6 insertions(+), 67 deletions(-) create mode 100644 msg/px4_msgs/vehicle_rates_setpoint.msg delete mode 100644 src/modules/uORB/topics/vehicle_rates_setpoint.h diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/px4_msgs/vehicle_rates_setpoint.msg new file mode 100644 index 0000000000..834113c798 --- /dev/null +++ b/msg/px4_msgs/vehicle_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h deleted file mode 100644 index e5cecf02b4..0000000000 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_rates_setpoint.h - * Definition of the vehicle rates setpoint topic - */ - -#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ -#define TOPIC_VEHICLE_RATES_SETPOINT_H_ - -#include -#include - -/** - * @addtogroup topics - * @{ - */ -struct vehicle_rates_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start */ - - float roll; /**< body angular rates in NED frame */ - float pitch; /**< body angular rates in NED frame */ - float yaw; /**< body angular rates in NED frame */ - float thrust; /**< thrust normalized to 0..1 */ - -}; /**< vehicle_rates_setpoint */ - -/** -* @} -*/ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_rates_setpoint); - -#endif From a71e6ed3c64d5c1c94d6f5f445b4b746ccf37eff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:25:39 +0100 Subject: [PATCH 095/122] change headers to use vehicle attitude msg --- src/modules/mc_att_control/mc_att_control_base.h | 1 - src/platforms/px4_includes.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 5e9c0c4db6..6c4b5c707d 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 8b0c374029..950f5cc795 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -59,6 +59,7 @@ #include #include #include +#include #include #include From b93fcca4339edc5312158dc4a70bf8abf2c8bb4b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:34:57 +0100 Subject: [PATCH 096/122] vehicle cotnrol mode as msg --- msg/px4_msgs/vehicle_control_mode.msg | 22 +++++ .../fw_pos_control_l1_main.cpp | 1 + .../mc_att_control/mc_att_control_base.h | 1 - .../uORB/topics/vehicle_control_mode.h | 95 ------------------- src/platforms/px4_includes.h | 1 + 5 files changed, 24 insertions(+), 96 deletions(-) create mode 100644 msg/px4_msgs/vehicle_control_mode.msg delete mode 100644 src/modules/uORB/topics/vehicle_control_mode.h diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/px4_msgs/vehicle_control_mode.msg new file mode 100644 index 0000000000..153a642bb4 --- /dev/null +++ b/msg/px4_msgs/vehicle_control_mode.msg @@ -0,0 +1,22 @@ + +uint64 timestamp # in microseconds since system start + # is set whenever the writing thread stores new data + +bool flag_armed + +bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing + +# XXX needs yet to be set by state machine helper +bool flag_system_hil_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_force_enabled # true if force control is mixed in +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_position_enabled # true if position is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_termination_enabled # true if flighttermination is enabled diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e07bcc225f..f8399d10be 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 6c4b5c707d..036dd4c48f 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h deleted file mode 100644 index 2dd8550bc4..0000000000 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * 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 - * 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 vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. - * - * All control apps should depend their actions based on the flags set here. - * - * @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include -#include -#include -#include "vehicle_status.h" - -/** - * @addtogroup topics @{ - */ - - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ - -struct vehicle_control_mode_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - bool flag_armed; - - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - // XXX needs yet to be set by state machine helper - bool flag_system_hil_enabled; - - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ - bool flag_control_offboard_enabled; /**< true if offboard control should be used */ - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_force_enabled; /**< true if force control is mixed in */ - bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 950f5cc795..a8b94076e6 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -60,6 +60,7 @@ #include #include #include +#include #include #include From 356e6f1eebfba8bc8630299ef091570f0c337ba5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 13:42:40 +0100 Subject: [PATCH 097/122] WIP, move some ORB defines --- src/modules/uORB/uORB.h | 9 ++++++++- src/platforms/px4_defines.h | 7 ------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad26..578ebf9ddc 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -125,7 +125,7 @@ typedef intptr_t orb_advert_t; * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -261,4 +261,11 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; __END_DECLS +/* Diverse uORB header defines */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); + #endif /* _UORB_UORB_H */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index daeff9cf30..ff302448a5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -139,10 +139,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -/* Diverese uORB header defiens */ //XXX: move to better location -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); From 88f4931fd1ac301c18921a5c12884263ac6fd68c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 13:44:08 +0100 Subject: [PATCH 098/122] actuator armed as msg --- msg/px4_msgs/actuator_armed.msg | 6 ++++ msg/px4_msgs/unused/actuator_armed.msg | 6 ---- .../mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/actuator_armed.h | 32 ++++++++----------- src/platforms/px4_includes.h | 1 + 5 files changed, 20 insertions(+), 26 deletions(-) create mode 100644 msg/px4_msgs/actuator_armed.msg delete mode 100644 msg/px4_msgs/unused/actuator_armed.msg diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg new file mode 100644 index 0000000000..b83adb8f24 --- /dev/null +++ b/msg/px4_msgs/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg deleted file mode 100644 index f6bf583073..0000000000 --- a/msg/px4_msgs/unused/actuator_armed.msg +++ /dev/null @@ -1,6 +0,0 @@ - -uint64 timestamp # Microseconds since system boot -bool armed # Set to true if system is armed -bool ready_to_arm # Set to true if system is ready to be armed -bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) -bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 036dd4c48f..49c8dad7d3 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 1e10e0ad12..918a305fdd 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -31,32 +31,28 @@ * ****************************************************************************/ -/** - * @file actuator_armed.h - * - * Actuator armed topic - * - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */ -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H + +#pragma once #include -#include +#include "../uORB.h" + + /** * @addtogroup topics * @{ */ -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - uint64_t timestamp; /**< Microseconds since system boot */ - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ +struct actuator_armed_s { + uint64_t timestamp; + bool armed; + bool ready_to_arm; + bool lockdown; + bool force_failsafe; }; /** @@ -65,5 +61,3 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a8b94076e6..b7771072ff 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -61,6 +61,7 @@ #include #include #include +#include #include #include From 77fd7b388b3b80881e405bec5ed56fb959082612 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:04:20 +0100 Subject: [PATCH 099/122] parameter update as msg --- msg/px4_msgs/parameter_update.msg | 1 + .../mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/parameter_update.h | 61 ------------------- src/platforms/px4_includes.h | 1 + 4 files changed, 2 insertions(+), 62 deletions(-) create mode 100644 msg/px4_msgs/parameter_update.msg delete mode 100644 src/modules/uORB/topics/parameter_update.h diff --git a/msg/px4_msgs/parameter_update.msg b/msg/px4_msgs/parameter_update.msg new file mode 100644 index 0000000000..39dc336e05 --- /dev/null +++ b/msg/px4_msgs/parameter_update.msg @@ -0,0 +1 @@ +uint64 timestamp # time at which the latest parameter was updated diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 49c8dad7d3..f81a0d7721 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h deleted file mode 100644 index fe9c9070fc..0000000000 --- a/src/modules/uORB/topics/parameter_update.h +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file parameter_update.h - * Notification about a parameter update. - */ - -#ifndef TOPIC_PARAMETER_UPDATE_H -#define TOPIC_PARAMETER_UPDATE_H - -#include -#include - -/** - * @addtogroup topics - * @{ - */ - -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; -}; - -/** - * @} - */ - -ORB_DECLARE(parameter_update); - -#endif \ No newline at end of file diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index b7771072ff..b8c8d034ed 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -62,6 +62,7 @@ #include #include #include +#include #include #include From 377030bd8a25f501c47aba573bf3125ad9061bd0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:20:11 +0100 Subject: [PATCH 100/122] mc att ctl: remove code which is already in base class --- .../mc_att_control/mc_att_control_main.cpp | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6ab4f95cd1..9dec6b8e6b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -227,33 +227,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); From b500cfb1f11b2000ad58d8c8b5b0d19ab9ebb438 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:20:11 +0100 Subject: [PATCH 101/122] mc att ctl: remove code which is already in base class --- .../mc_att_control/mc_att_control_main.cpp | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 887a535088..a5226eb7c5 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -236,33 +236,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); From fe6663ad9aa0a5fdb18e2ce3e23dd57d47ae8569 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:09:31 +0100 Subject: [PATCH 102/122] add platforms/nuttx to default makefile --- src/modules/mc_att_control/mc_att_control_main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9dec6b8e6b..631ca57e05 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -103,6 +103,8 @@ public: */ int start(); + void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -422,11 +424,13 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { + px4::NodeHandle n; /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + // PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -582,6 +586,10 @@ MulticopterAttitudeControl::task_main() _exit(0); } +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); +} + int MulticopterAttitudeControl::start() { @@ -603,8 +611,10 @@ MulticopterAttitudeControl::start() return OK; } -int mc_att_control_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(mc_att_control) { + px4::init(argc, argv, "mc_att_control"); + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); } From afa835744cbef667279913bbf79a0c4697a5ec8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:48:44 +0100 Subject: [PATCH 103/122] remove unnecessary type in msg.h template --- msg/templates/msg.h.template | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 84dea37a3b..5fce506b6b 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -104,8 +104,7 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', - 'bool': 'bool', - 'actuator_controls': 'actuator_controls'} + 'bool': 'bool'} # Function to print a standard ros type def print_field_def(field): From 9d5f06c9a779752cfb1662d5c794fa85fda3d494 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:50:30 +0100 Subject: [PATCH 104/122] remove actuator armed uorb topic --- src/modules/uORB/topics/actuator_armed.h | 63 ------------------------ 1 file changed, 63 deletions(-) delete mode 100644 src/modules/uORB/topics/actuator_armed.h diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h deleted file mode 100644 index 918a305fdd..0000000000 --- a/src/modules/uORB/topics/actuator_armed.h +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */ - - -#pragma once - -#include -#include "../uORB.h" - - - -/** - * @addtogroup topics - * @{ - */ - - -struct actuator_armed_s { - uint64_t timestamp; - bool armed; - bool ready_to_arm; - bool lockdown; - bool force_failsafe; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); From 76457e63c51695c814dda75c1cc351cb509c9a19 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:38:54 +0100 Subject: [PATCH 105/122] add nuttx platform to default makefile --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index df6b6d0c5b..38c5ebc7b3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -113,6 +113,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # OBC challenge From 712e5797eba482592c372b06384e79d217f6bc05 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:39:33 +0100 Subject: [PATCH 106/122] Subscription: define more templates --- src/modules/uORB/Subscription.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 61609d0092..fa0594c2ef 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -95,5 +95,7 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB From 9ed57211cc684d5135a254d9230c6d82bb092f9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:40:36 +0100 Subject: [PATCH 107/122] hack to define isspace in px4_defines, add macro for subscription without callback --- src/platforms/px4_defines.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ff302448a5..283dc5a53c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -96,6 +96,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) * Building for NuttX */ #include +#ifdef __cplusplus +#include +#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) @@ -113,6 +116,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) /* Parameter handle datatype */ #include @@ -126,10 +130,10 @@ typedef param_t px4_param_t; #endif /* Shortcut for subscribing to topics - * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) /* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) @@ -139,3 +143,7 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') From 24fd5759b5443cf198f29eb6d5eae8c80cb04fe0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:41:11 +0100 Subject: [PATCH 108/122] add missing __EXPORT --- src/platforms/px4_middleware.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index b0bc404173..16e73ec04a 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -59,7 +59,7 @@ extern bool task_should_exit; /** * Returns true if the app/task should continue to run */ -inline bool ok() { return !task_should_exit; } +__EXPORT inline bool ok() { return !task_should_exit; } #endif class Rate From e6224304606e20bbf79280978ba70326eabea575 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:41:54 +0100 Subject: [PATCH 109/122] px4 subscriber/nuttx: don't call null callback --- src/platforms/px4_subscriber.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e995c6e49d..5d0120f90e 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -115,6 +115,10 @@ public: */ void update() { + if (_callback == nullptr) { + return; + } + if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; From 03ba38d0a4e39dc513cae6b53ff7cf845c13161c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:42:57 +0100 Subject: [PATCH 110/122] very much WIP, start to make mc att control p4 and ros compatible --- .../mc_att_control/mc_att_control_main.cpp | 456 +++++++++--------- 1 file changed, 231 insertions(+), 225 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 631ca57e05..7a3b614825 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,17 +71,53 @@ #include "mc_att_control_base.h" +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ + +using namespace px4; + /** * Multicopter attitude control app start / stop handling function * * @ingroup apps */ -extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(mc_att_control); + +int mc_attitude_thread_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); +} + + +namespace px4 +{ +bool task_should_exit = false; +} + +// PX4_MAIN_FUNCTION(mc_att_control) { + // px4::init(argc, argv, "listener"); + + // px4::NodeHandle n; + + // PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000); + + /** + * px4::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). px4::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + // n.spin(); + // PX4_INFO("finished, returning"); + + // return 0; +// } + class MulticopterAttitudeControl : public MulticopterAttitudeControlBase { @@ -96,15 +132,10 @@ public: */ ~MulticopterAttitudeControl(); - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + void spin() { n.spin(); } + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -123,6 +154,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + px4::NodeHandle n; + struct { param_t roll_p; param_t roll_rate_p; @@ -184,15 +217,6 @@ private: */ void arming_status_poll(); - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main attitude control task. - */ - void task_main(); }; namespace mc_att_control @@ -204,7 +228,6 @@ namespace mc_att_control #endif static const int ERROR = -1; -MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : @@ -224,6 +247,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + n(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -252,6 +276,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + + /* + * do subscriptions + */ + // _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + // _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0); + // _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0); + // _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + PX4_SUBSCRIBE(n, vehicle_control_mode, 0); + // _params_sub = orb_subscribe(ORB_ID(parameter_update)); + PX4_SUBSCRIBE(n, parameter_update, 0); + // _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + PX4_SUBSCRIBE(n, manual_control_setpoint, 0); + // _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + PX4_SUBSCRIBE(n, actuator_armed, 0); + } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -275,7 +318,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - mc_att_control::g_control = nullptr; + // mc_att_control::g_control = nullptr; } int @@ -415,200 +458,140 @@ MulticopterAttitudeControl::arming_status_poll() } } -void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - mc_att_control::g_control->task_main(); -} +// void +// MulticopterAttitudeControl::task_main() +// { -void -MulticopterAttitudeControl::task_main() -{ - px4::NodeHandle n; - /* - * do subscriptions - */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - // PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _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)); + // [> wakeup source: vehicle attitude <] + // struct pollfd fds[1]; - /* initialize parameters cache */ - parameters_update(); + // fds[0].fd = _v_att_sub; + // fds[0].events = POLLIN; - /* wakeup source: vehicle attitude */ - struct pollfd fds[1]; + // while (!_task_should_exit) { - fds[0].fd = _v_att_sub; - fds[0].events = POLLIN; - while (!_task_should_exit) { + // perf_end(_loop_perf); + // } - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + // warnx("exit"); - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - perf_begin(_loop_perf); - - /* run controller on attitude changes */ - if (fds[0].revents & POLLIN) { - static uint64_t last_run = 0; - float dt = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too small (< 2ms) and too large (> 20ms) dt's */ - if (dt < 0.002f) { - dt = 0.002f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - - /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); - - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - - /* publish the attitude setpoint if needed */ - if (_publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); - } - } - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _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); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { - /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, - _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; - - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _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); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; - } - } - - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); - - /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = hrt_absolute_time(); - - if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } - } - } - } - - perf_end(_loop_perf); - } - - warnx("exit"); - - _control_task = -1; - _exit(0); -} + // _control_task = -1; + // _exit(0); +// } void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); -} -int -MulticopterAttitudeControl::start() -{ - ASSERT(_control_task == -1); + perf_begin(_loop_perf); - /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - if (_control_task < 0) { - warn("task start failed"); - return -errno; + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; } - return OK; + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); + + /* publish the attitude setpoint if needed */ + if (_publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, + &_v_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), + &_v_att_sp); + } + } + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _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); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp.z; + + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _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); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; + } + } + + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + } } PX4_MAIN_FUNCTION(mc_att_control) @@ -621,44 +604,67 @@ PX4_MAIN_FUNCTION(mc_att_control) if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) { - errx(1, "already running"); + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); } - mc_att_control::g_control = new MulticopterAttitudeControl; + task_should_exit = false; - if (mc_att_control::g_control == nullptr) { - errx(1, "alloc failed"); - } - - if (OK != mc_att_control::g_control->start()) { - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - err(1, "start failed"); - } + daemon_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + mc_attitude_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) { - errx(1, "not running"); - } + // if (!strcmp(argv[1], "stop")) { + // if (mc_att_control::g_control == nullptr) { + // errx(1, "not running"); + // } - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - exit(0); - } + // delete mc_att_control::g_control; + // mc_att_control::g_control = nullptr; + // exit(0); + // } - if (!strcmp(argv[1], "status")) { - if (mc_att_control::g_control) { - errx(0, "running"); + // if (!strcmp(argv[1], "status")) { + // if (mc_att_control::g_control) { + // errx(0, "running"); - } else { - errx(1, "not running"); - } - } + // } else { + // errx(1, "not running"); + // } + // } warnx("unrecognized command"); return 1; } + +int mc_attitude_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + MulticopterAttitudeControl attctl; + + thread_running = true; + + attctl.spin(); + + // while (!task_should_exit) { + // attctl.update(); + // } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + + From e43a05de3af19f8390aaf5432e7308cc5dabe5ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:17 +0100 Subject: [PATCH 111/122] add systemlib to px4 includes --- src/platforms/px4_includes.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index b8c8d034ed..525f94aae9 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -65,5 +65,6 @@ #include #include #include +#include #endif From cb77c16601338d2d0b22dc71ef057ec4d91652c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:45 +0100 Subject: [PATCH 112/122] add sched.h to systemlib includes SCHED_RR and SCHED_FIFO are defined in sched.h --- src/modules/systemlib/systemlib.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 3728f20672..6e22a557ea 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -41,6 +41,7 @@ #define SYSTEMLIB_H_ #include #include +#include __BEGIN_DECLS From 8ae1c9763d649470ae7c55e35507509dbb2612ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 16:22:19 +0100 Subject: [PATCH 113/122] write publisher example as class --- src/examples/publisher/module.mk | 3 +- src/examples/publisher/publisher.cpp | 100 --------------- src/examples/publisher/publisher_example.cpp | 67 ++++++++++ src/examples/publisher/publisher_example.h | 51 ++++++++ src/examples/publisher/publisher_main.cpp | 112 ++++++++++++++++ src/examples/subscriber/module.mk | 3 +- src/examples/subscriber/subscriber.cpp | 120 ------------------ .../subscriber/subscriber_example.cpp | 78 ++++++++++++ src/examples/subscriber/subscriber_example.h | 58 +++++++++ src/examples/subscriber/subscriber_main.cpp | 112 ++++++++++++++++ .../mc_att_control/mc_att_control_main.cpp | 3 +- 11 files changed, 483 insertions(+), 224 deletions(-) delete mode 100644 src/examples/publisher/publisher.cpp create mode 100644 src/examples/publisher/publisher_example.cpp create mode 100644 src/examples/publisher/publisher_example.h create mode 100644 src/examples/publisher/publisher_main.cpp delete mode 100644 src/examples/subscriber/subscriber.cpp create mode 100644 src/examples/subscriber/subscriber_example.cpp create mode 100644 src/examples/subscriber/subscriber_example.h create mode 100644 src/examples/subscriber/subscriber_main.cpp diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 0fc4316ecb..4f941cd439 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = publisher -SRCS = publisher.cpp +SRCS = publisher_main.cpp \ + publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp deleted file mode 100644 index 8ef147493f..0000000000 --- a/src/examples/publisher/publisher.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. 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. - */ - -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple sending of messages over the PX4 middleware system. - */ - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(publisher) -{ - - px4::init(argc, argv, "px4_publisher"); - - px4::NodeHandle n; - - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels); - - - px4::Rate loop_rate(10); - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - - while (px4::ok()) { - /** - * This is a message object. You stuff it with data, and then publish it. - */ - PX4_TOPIC_T(rc_channels) msg; - - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - rc_channels_pub->publish(msg); - - n.spinOnce(); - - loop_rate.sleep(); - ++count; - } - - return 0; -} diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp new file mode 100644 index 0000000000..3c716291b7 --- /dev/null +++ b/src/examples/publisher/publisher_example.cpp @@ -0,0 +1,67 @@ + +/**************************************************************************** + * + * 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 publisher_example.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ + +#include "publisher_example.h" + +PublisherExample::PublisherExample() : + _n(), + _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) +{ + +} + +int PublisherExample::main() +{ + px4::Rate loop_rate(10); + + while (px4::ok()) { + PX4_TOPIC_T(rc_channels) msg; + msg.timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.timestamp_last_valid); + + _rc_channels_pub->publish(msg); + + _n.spinOnce(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h new file mode 100644 index 0000000000..78c1ffc89f --- /dev/null +++ b/src/examples/publisher/publisher_example.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 publisher.h + * Example publisher for ros and px4 + * + * @author Thomas Gubler + */ +#include +class PublisherExample { +public: + PublisherExample(); + + ~PublisherExample() {}; + + int main(); +protected: + px4::NodeHandle _n; + px4::Publisher * _rc_channels_pub; +}; diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp new file mode 100644 index 0000000000..e1034fec53 --- /dev/null +++ b/src/examples/publisher/publisher_main.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 publisher_main.cpp + * Example publisher for ros and px4 + * + * @author Thomas Gubler + */ +#include +#include +#include "publisher_example.h" + +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +int publisher_task_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(publisher) +{ + px4::init(argc, argv, "publisher"); + + if (argc < 1) { + errx(1, "usage: publisher {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + publisher_task_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} + +int publisher_task_main(int argc, char *argv[]) +{ + warnx("starting"); + PublisherExample p; + thread_running = true; + p.main(); + + warnx("exiting."); + thread_running = false; + return 0; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 90b4d8ffc5..3156ebb30a 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,7 +37,8 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp \ +SRCS = subscriber_main.cpp \ + subscriber_example.cpp \ subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp deleted file mode 100644 index 0d9ca1fa76..0000000000 --- a/src/examples/subscriber/subscriber.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. 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. - */ - -#include -#include "subscriber_params.h" -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. - */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); -} -void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid); -} - -class RCHandler { -public: - void callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); - } -}; - -RCHandler rchandler; - - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(subscriber) { - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of px4::init() before using any other - * part of the PX4/ ROS system. - */ - px4::init(argc, argv, "listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - px4::NodeHandle n; - - /* Define parameters */ - px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); - px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); - - /* Read the parameter back for testing */ - int32_t sub_interval; - float test_float; - PX4_PARAM_GET(p_sub_interv, &sub_interval); - PX4_INFO("Param SUB_INTERV = %d", sub_interval); - PX4_PARAM_GET(p_test_float, &test_float); - PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); - PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); - PX4_INFO("subscribed"); - - /** - * px4::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). px4::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - n.spin(); - PX4_INFO("finished, returning"); - - return 0; -} diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp new file mode 100644 index 0000000000..cddab10355 --- /dev/null +++ b/src/examples/subscriber/subscriber_example.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 subscriber.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ + +#include "subscriber_params.h" +#include "subscriber_example.h" + +using namespace px4; + +void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +} + +SubscriberExample::SubscriberExample() : + _n(), + _sub_interval(0), + _test_float(0.0f) +{ + /* Define parameters */ + _p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + _p_test_float = PX4_PARAM_INIT(SUB_TESTF); + + /* Read the parameter back as example */ + PX4_PARAM_GET(_p_sub_interv, &_sub_interval); + PX4_INFO("Param SUB_INTERV = %d", _sub_interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); + + /* Do some subscriptions */ + /* Function */ + PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval); + /* Class Method */ + PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + PX4_INFO("subscribed"); +} + +/** + * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + */ +void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h new file mode 100644 index 0000000000..4bdc15c2d7 --- /dev/null +++ b/src/examples/subscriber/subscriber_example.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 subscriber.h + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ +#include +class SubscriberExample { +public: + SubscriberExample(); + + ~SubscriberExample() {}; + + void spin() {_n.spin();} +protected: + px4::NodeHandle _n; + px4_param_t _p_sub_interv; + int32_t _sub_interval; + px4_param_t _p_test_float; + float _test_float; + + void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + + +}; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp new file mode 100644 index 0000000000..32de4fd6eb --- /dev/null +++ b/src/examples/subscriber/subscriber_main.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 subscriber_main.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ +#include +#include +#include "subscriber_example.h" + +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +int subscriber_task_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(subscriber) +{ + px4::init(argc, argv, "subscriber"); + + if (argc < 1) { + errx(1, "usage: subscriber {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + subscriber_task_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} + +int subscriber_task_main(int argc, char *argv[]) +{ + warnx("starting"); + SubscriberExample s; + thread_running = true; + s.spin(); + + warnx("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7a3b614825..1103191e7f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -100,8 +100,7 @@ namespace px4 bool task_should_exit = false; } -// PX4_MAIN_FUNCTION(mc_att_control) { - // px4::init(argc, argv, "listener"); +// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener"); // px4::NodeHandle n; From da4cfad3c2dc14f17ccf0e9a8ce41309b76964e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 16:25:10 +0100 Subject: [PATCH 114/122] subscriber example: improve init --- src/examples/subscriber/subscriber_example.cpp | 8 +++----- src/examples/subscriber/subscriber_example.h | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index cddab10355..e6d63cab33 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file subscriber.cpp + * @file subscriber_example.cpp * Example subscriber for ros and px4 * * @author Thomas Gubler @@ -49,13 +49,11 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { SubscriberExample::SubscriberExample() : _n(), + _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), _sub_interval(0), + _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), _test_float(0.0f) { - /* Define parameters */ - _p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); - _p_test_float = PX4_PARAM_INIT(SUB_TESTF); - /* Read the parameter back as example */ PX4_PARAM_GET(_p_sub_interv, &_sub_interval); PX4_INFO("Param SUB_INTERV = %d", _sub_interval); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 4bdc15c2d7..b52f89ba87 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file subscriber.h + * @file subscriber_example.h * Example subscriber for ros and px4 * * @author Thomas Gubler From f0ad2c9ef55dc2508e777384f8d0496042ade9e8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 17:02:49 +0100 Subject: [PATCH 115/122] px4 subscriber: uorb: check if callback null at correct location --- src/platforms/px4_subscriber.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 5d0120f90e..45cb650b24 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -115,10 +115,6 @@ public: */ void update() { - if (_callback == nullptr) { - return; - } - if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -127,6 +123,12 @@ public: /* get latest data */ uORB::Subscription::update(); + + /* Check if there is a callback */ + if (_callback == nullptr) { + return; + } + /* Call callback which performs actions based on this data */ _callback(uORB::Subscription::getData()); From 1b6b6cd35c1f14766d49144bbc18d19d02656362 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 17:03:24 +0100 Subject: [PATCH 116/122] add no callback example to subscriber example --- src/examples/subscriber/subscriber_example.cpp | 15 ++++++++++----- src/examples/subscriber/subscriber_example.h | 3 ++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index e6d63cab33..460a3267a1 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -50,21 +50,24 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { SubscriberExample::SubscriberExample() : _n(), _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), - _sub_interval(0), + _interval(0), _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), _test_float(0.0f) { /* Read the parameter back as example */ - PX4_PARAM_GET(_p_sub_interv, &_sub_interval); - PX4_INFO("Param SUB_INTERV = %d", _sub_interval); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); PX4_PARAM_GET(_p_test_float, &_test_float); PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval); + PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); /* Class Method */ PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + /* No callback */ + _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + PX4_INFO("subscribed"); } @@ -72,5 +75,7 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid); + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index b52f89ba87..848d43f76a 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -48,9 +48,10 @@ public: protected: px4::NodeHandle _n; px4_param_t _p_sub_interv; - int32_t _sub_interval; + int32_t _interval; px4_param_t _p_test_float; float _test_float; + px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); From d04bbf11ec5d273902d3920981ccac49489d7098 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 18:34:41 +0100 Subject: [PATCH 117/122] subscriber example: add comment --- src/examples/subscriber/subscriber_example.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 460a3267a1..4e0350ff0f 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -73,6 +73,7 @@ SubscriberExample::SubscriberExample() : /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", From a1685ed6d093ec01214e6839ea66f9dd565e55ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 09:27:11 +0100 Subject: [PATCH 118/122] change definition of px4 main function --- src/examples/publisher/publisher_main.cpp | 6 +++--- src/examples/subscriber/subscriber_main.cpp | 10 +++++----- src/platforms/px4_defines.h | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index e1034fec53..c4e79aaa6b 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -49,9 +49,9 @@ bool task_should_exit = false; } using namespace px4; -int publisher_task_main(int argc, char *argv[]); +PX4_MAIN_FUNCTION(publisher); -PX4_MAIN_FUNCTION(publisher) +extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { px4::init(argc, argv, "publisher"); @@ -99,7 +99,7 @@ PX4_MAIN_FUNCTION(publisher) return 1; } -int publisher_task_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(publisher) { warnx("starting"); PublisherExample p; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 32de4fd6eb..0436dc9f2b 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -49,12 +49,10 @@ bool task_should_exit = false; } using namespace px4; -int subscriber_task_main(int argc, char *argv[]); +PX4_MAIN_FUNCTION(subscriber); -PX4_MAIN_FUNCTION(subscriber) +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) { - px4::init(argc, argv, "subscriber"); - if (argc < 1) { errx(1, "usage: subscriber {start|stop|status}"); } @@ -99,8 +97,10 @@ PX4_MAIN_FUNCTION(subscriber) return 1; } -int subscriber_task_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(subscriber) { + px4::init(argc, argv, "subscriber"); + warnx("starting"); SubscriberExample s; thread_running = true; diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 283dc5a53c..440f1b6fcd 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -100,7 +100,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #include #endif /* Main entry point */ -#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) /* Print/output wrappers */ #define PX4_WARN warnx From 173b1b2a8bcf2344be92b1f6e5acbc089a43fcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 10:51:19 +0100 Subject: [PATCH 119/122] WIP, make class based and extended subscriber/publisher example compile for ros --- CMakeLists.txt | 24 +++++++++++-------- src/examples/publisher/publisher_example.cpp | 2 ++ src/examples/publisher/publisher_example.h | 1 + src/examples/publisher/publisher_main.cpp | 6 +++-- .../subscriber/subscriber_example.cpp | 7 +++--- src/examples/subscriber/subscriber_example.h | 3 +++ src/examples/subscriber/subscriber_main.cpp | 6 +++-- src/platforms/px4_defines.h | 7 ++++-- src/platforms/px4_nodehandle.h | 13 ++++++++++ 9 files changed, 50 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e91f2cc371..f206a5aa2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,7 +111,9 @@ target_link_libraries(px4 ) ## Declare a test publisher -add_executable(publisher src/examples/publisher/publisher.cpp) +add_executable(publisher + src/examples/publisher/publisher_main.cpp + src/examples/publisher/publisher_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -124,7 +126,9 @@ target_link_libraries(publisher ) ## Declare a test subscriber -add_executable(subscriber src/examples/subscriber/subscriber.cpp) +add_executable(subscriber + src/examples/subscriber/subscriber_main.cpp + src/examples/subscriber/subscriber_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -136,14 +140,14 @@ target_link_libraries(subscriber px4 ) -add_executable(mc_att_control - src/modules/mc_att_control/mc_att_control_main.cpp - src/moudles/mc_att_control/mc_att_control_base.cpp) -add_dependencies(mc_att_control px4_generate_messages_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) +# add_executable(mc_att_control + # src/modules/mc_att_control/mc_att_control_main.cpp + # src/moudles/mc_att_control/mc_att_control_base.cpp) +# add_dependencies(mc_att_control px4_generate_messages_cpp) +# target_link_libraries(mc_att_control + # ${catkin_LIBRARIES} + # px4 +# ) ############# diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 3c716291b7..2e5779ebec 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -41,6 +41,8 @@ #include "publisher_example.h" +using namespace px4; + PublisherExample::PublisherExample() : _n(), _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 78c1ffc89f..304ecef47a 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -38,6 +38,7 @@ * @author Thomas Gubler */ #include + class PublisherExample { public: PublisherExample(); diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index c4e79aaa6b..8b692d9633 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -51,6 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(publisher); +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { px4::init(argc, argv, "publisher"); @@ -98,15 +99,16 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } +#endif PX4_MAIN_FUNCTION(publisher) { - warnx("starting"); + PX4_INFO("starting"); PublisherExample p; thread_running = true; p.main(); - warnx("exiting."); + PX4_INFO("exiting."); thread_running = false; return 0; } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 4e0350ff0f..39d0907520 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -76,7 +76,8 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); + //XXX + // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + // msg.timestamp_last_valid, + // ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 848d43f76a..73c7390352 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -38,6 +38,9 @@ * @author Thomas Gubler */ #include + +using namespace px4; + class SubscriberExample { public: SubscriberExample(); diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 0436dc9f2b..716233739b 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -51,6 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(subscriber); +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) { if (argc < 1) { @@ -96,17 +97,18 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } +#endif PX4_MAIN_FUNCTION(subscriber) { px4::init(argc, argv, "subscriber"); - warnx("starting"); + PX4_INFO("starting"); SubscriberExample s; thread_running = true; s.spin(); - warnx("exiting."); + PX4_INFO("exiting."); thread_running = false; return 0; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 440f1b6fcd..819954d0ef 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -66,9 +66,11 @@ #define PX4_TOPIC_T(_name) _name /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); /* Parameter handle datatype */ typedef const char *px4_param_t; @@ -113,9 +115,10 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_TOPIC_T(_name) _name##_s /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) /* Parameter handle datatype */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 5b7247b203..25b8e037d6 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -99,6 +99,19 @@ public: return sub; } + /** + * Subscribe with no callback, just the latest value is stored on updates + * @param topic Name of the topic + */ + template + Subscriber *subscribe(const char *topic) + { + //XXX missing implementation + // Subscriber *sub = new Subscriber(ros_sub); + // _subs.push_back(sub); + return (Subscriber *)NULL; + } + /** * Advertise topic * @param topic Name of the topic From fd6a5fd38b194300788c597fd1636b2a97e24642 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 10:58:47 +0100 Subject: [PATCH 120/122] move px4::init call --- src/examples/publisher/publisher_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 8b692d9633..5b50ecc6ca 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -54,8 +54,6 @@ PX4_MAIN_FUNCTION(publisher); #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { - px4::init(argc, argv, "publisher"); - if (argc < 1) { errx(1, "usage: publisher {start|stop|status}"); } @@ -103,6 +101,8 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[]) PX4_MAIN_FUNCTION(publisher) { + px4::init(argc, argv, "publisher"); + PX4_INFO("starting"); PublisherExample p; thread_running = true; From c68c277c94cacd2a64b634dd9c45ace2a04c2911 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 15:04:37 +0100 Subject: [PATCH 121/122] subscription class for ros now stores last message to avoid manual copy and support subscription with no callbacks --- CMakeLists.txt | 1 + .../subscriber/subscriber_example.cpp | 7 +- src/platforms/px4_defines.h | 18 ++- src/platforms/px4_nodehandle.h | 25 ++-- src/platforms/px4_subscriber.h | 113 +++++++++++++----- 5 files changed, 116 insertions(+), 48 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f206a5aa2e..489467db7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 39d0907520..95e208ca6d 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -76,8 +76,7 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - //XXX - // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - // msg.timestamp_last_valid, - // ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + ((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid); } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 819954d0ef..fa3ef216a5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -70,7 +70,7 @@ /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); /* Parameter handle datatype */ typedef const char *px4_param_t; @@ -93,6 +93,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberROS + #else /* * Building for NuttX @@ -130,6 +133,15 @@ typedef param_t px4_param_t; /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) + +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberUORB + +/* XXX this is a hack to resolve conflicts with NuttX headers */ +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') + #endif /* Shortcut for subscribing to topics @@ -146,7 +158,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -#define isspace(c) \ - ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ - (c) == '\r' || (c) == '\f' || c== '\v') diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 25b8e037d6..c24a183030 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,10 +77,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(M)) + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -91,10 +92,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -106,10 +108,11 @@ public: template Subscriber *subscribe(const char *topic) { - //XXX missing implementation - // Subscriber *sub = new Subscriber(ros_sub); - // _subs.push_back(sub); - return (Subscriber *)NULL; + Subscriber *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return sub; } /** @@ -165,7 +168,7 @@ public: std::function callback, unsigned interval) { - SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); + SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 45cb650b24..234b72f5b9 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,6 +36,7 @@ * * PX4 Middleware Wrapper Subscriber */ +#include #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -44,59 +45,108 @@ /* includes when building for NuttX */ #include #include -#include +#include "px4_nodehandle.h" #endif namespace px4 { - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class Subscriber -{ -public: - /** - * Construct Subscriber by providing a ros::Subscriber - * @param ros_sub the ros subscriber which will be used to perform the publications - */ - Subscriber(ros::Subscriber ros_sub) : - _ros_sub(ros_sub) - {} - - ~Subscriber() {}; -private: - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ -}; - -#else - /** - * Subscriber class which is used by nodehandle when building for NuttX + * Subscriber class which is used by nodehandle */ class Subscriber { public: Subscriber() {}; ~Subscriber() {}; -private: }; +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Subscriber class that is templated with the ros n message type + */ +template +class SubscriberROS : + public Subscriber +{ +friend class NodeHandle; + +public: + /** + * Construct Subscriber by providing a callback function + */ + SubscriberROS(std::function cbf) : + Subscriber(), + _ros_sub(), + _cbf(cbf), + _msg_current() + {} + + /** + * Construct Subscriber without a callback function + */ + SubscriberROS() : + Subscriber(), + _ros_sub(), + _cbf(NULL), + _msg_current() + {} + + + ~SubscriberROS() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return _msg_current; } + +protected: + /** + * Called on topic update, saves the current message and then calls the provided callback function + */ + void callback(const M &msg) { + /* Store data */ + _msg_current = msg; + + /* Call callback */ + if (_cbf != NULL) { + _cbf(msg); + } + + } + + /** + * Saves the ros subscriber to keep ros subscription alive + */ + void set_ros_sub(ros::Subscriber ros_sub) { + _ros_sub = ros_sub; + } + + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ + std::function _cbf; /**< Callback that the user provided on the subscription */ + M _msg_current; /**< Current Message value */ + +}; + +#else + /** * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberPX4 : +class SubscriberUORB : public Subscriber, public uORB::Subscription { public: /** - * Construct SubscriberPX4 by providing orb meta data + * Construct SubscriberUORB by providing orb meta data * @param meta orb metadata for the topic which is used * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberPX4(const struct orb_metadata *meta, + SubscriberUORB(const struct orb_metadata *meta, unsigned interval, std::function callback, List *list) : @@ -106,7 +156,7 @@ public: //XXX store callback {} - ~SubscriberPX4() {}; + ~SubscriberUORB() {}; /** * Update Subscription @@ -133,7 +183,14 @@ public: _callback(uORB::Subscription::getData()); }; -private: + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return uORB::Subscription::getData(); } + +protected: std::function _callback; /**< Callback handle, called when new data is available */ From 998646f03b229ae86bd6e57ff0bd15dd1763c342 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 15:33:32 +0100 Subject: [PATCH 122/122] add base class and template subscriber class as well to improve interface to get last msg value --- .../subscriber/subscriber_example.cpp | 2 +- src/examples/subscriber/subscriber_example.h | 2 +- src/platforms/px4_defines.h | 9 ++--- src/platforms/px4_nodehandle.h | 28 +++++++------- src/platforms/px4_publisher.h | 25 ++++++++++-- src/platforms/px4_subscriber.h | 38 +++++++++++++++---- 6 files changed, 70 insertions(+), 34 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 95e208ca6d..83ead4ba5a 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - ((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid); + _sub_rc_chan->get_msg().timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 73c7390352..c4b853d4de 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -54,7 +54,7 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::Subscriber * _sub_rc_chan; + px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index fa3ef216a5..9bb190380e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -93,9 +93,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) -/* Get a subscriber class type based on the topic name */ -#define PX4_SUBSCRIBER_T(_name) SubscriberROS - #else /* * Building for NuttX @@ -134,9 +131,6 @@ typedef param_t px4_param_t; /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) -/* Get a subscriber class type based on the topic name */ -#define PX4_SUBSCRIBER_T(_name) SubscriberUORB - /* XXX this is a hack to resolve conflicts with NuttX headers */ #define isspace(c) \ ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ @@ -150,6 +144,9 @@ typedef param_t px4_param_t; #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER(_name) Subscriber + /* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index c24a183030..9bd792c697 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,13 +77,13 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(const M&)) + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) { - Subscriber *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -92,13 +92,13 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) { - Subscriber *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -106,13 +106,13 @@ public: * @param topic Name of the topic */ template - Subscriber *subscribe(const char *topic) + Subscriber *subscribe(const char *topic) { - Subscriber *sub = new SubscriberROS(); + SubscriberBase *sub = new SubscriberROS(); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -141,10 +141,10 @@ public: private: static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ - std::list _subs; /**< Subcriptions of node */ - std::list _pubs; /**< Publications of node */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; -#else +#else //Building for NuttX class __EXPORT NodeHandle { public: @@ -164,7 +164,7 @@ public: */ template - Subscriber *subscribe(const struct orb_metadata *meta, + Subscriber *subscribe(const struct orb_metadata *meta, std::function callback, unsigned interval) { @@ -175,7 +175,7 @@ public: _sub_min_interval = sub_px4; } - return (Subscriber *)sub_px4; + return (Subscriber *)sub_px4; } /** diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index cb15eeb58f..6b6d8e39e9 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -48,15 +48,30 @@ namespace px4 { + +/** + * Untemplated publisher base class + * */ +class PublisherBase +{ +public: + PublisherBase() {}; + ~PublisherBase() {}; + +}; + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class Publisher +class Publisher : + public PublisherBase { public: /** * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) + Publisher(ros::Publisher ros_pub) : + PublisherBase(), + _ros_pub(ros_pub) {} ~Publisher() {}; @@ -71,7 +86,8 @@ private: }; #else class Publisher : - public uORB::PublicationNode + public uORB::PublicationNode, + public PublisherBase { public: /** @@ -81,7 +97,8 @@ public: */ Publisher(const struct orb_metadata *meta, List *list) : - uORB::PublicationNode(meta, list) + uORB::PublicationNode(meta, list), + PublisherBase() {} ~Publisher() {}; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 234b72f5b9..59c0ef70bd 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -50,14 +50,36 @@ namespace px4 { + +/** + * Untemplated subscriber base class + * */ +class SubscriberBase +{ +public: + SubscriberBase() {}; + ~SubscriberBase() {}; + +}; + /** * Subscriber class which is used by nodehandle */ -class Subscriber +template +class Subscriber : + public SubscriberBase { public: - Subscriber() {}; + Subscriber() : + SubscriberBase() + {}; ~Subscriber() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + virtual const M& get_msg() = 0; }; #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -66,7 +88,7 @@ public: */ template class SubscriberROS : - public Subscriber + public Subscriber { friend class NodeHandle; @@ -75,7 +97,7 @@ public: * Construct Subscriber by providing a callback function */ SubscriberROS(std::function cbf) : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(cbf), _msg_current() @@ -85,7 +107,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(NULL), _msg_current() @@ -128,14 +150,14 @@ protected: }; -#else +#else // Building for NuttX /** * Subscriber class that is templated with the uorb subscription message type */ template class SubscriberUORB : - public Subscriber, + public Subscriber, public uORB::Subscription { public: @@ -150,7 +172,7 @@ public: unsigned interval, std::function callback, List *list) : - Subscriber(), + Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) //XXX store callback