From a30a5d2665d3d0f68262d7de68aa5d4086a42321 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 14:27:49 +0200 Subject: 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 --- src/modules/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 +- .../attitude_estimator_ekf/codegen/AttitudeEKF.c | 1573 ++++++++++++++++++++ .../attitude_estimator_ekf/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 - src/modules/attitude_estimator_ekf/codegen/cross.c | 37 - src/modules/attitude_estimator_ekf/codegen/cross.h | 34 - src/modules/attitude_estimator_ekf/codegen/eye.c | 51 - src/modules/attitude_estimator_ekf/codegen/eye.h | 35 - .../attitude_estimator_ekf/codegen/mrdivide.c | 357 ----- .../attitude_estimator_ekf/codegen/mrdivide.h | 36 - src/modules/attitude_estimator_ekf/codegen/norm.c | 54 - src/modules/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 - .../attitude_estimator_ekf/codegen/rt_defines.h | 24 - .../attitude_estimator_ekf/codegen/rt_nonfinite.c | 87 -- .../attitude_estimator_ekf/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 (limited to 'src/modules') diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 000000000..1218cb65d --- /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 000000000..a8315bf57 --- /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 35dc39ec6..e1bbf5bc7 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 bc0e3b93a..5b57bfb4d 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 5985541ca..fbb6a18ff 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 000000000..d568e5737 --- /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 000000000..fc0275210 --- /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 000000000..63eb7e501 --- /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 9e97ad11a..000000000 --- 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 afa63c1a9..000000000 --- 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 7d620d7fa..000000000 --- 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 8b599be66..000000000 --- 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 7f9727419..000000000 --- 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 da84a5024..000000000 --- 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 30fd1e75e..000000000 --- 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 84ada9002..000000000 --- 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 e727f0684..000000000 --- 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 b89ab58ef..000000000 --- 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 94fbe7671..000000000 --- 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 a810f22e4..000000000 --- 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 2d3b0d51f..000000000 --- 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 0c418cc7b..000000000 --- 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 60cf77b57..000000000 --- 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 d035dae5e..000000000 --- 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 4bbebebe2..000000000 --- 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 34164d104..000000000 --- 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 145373cd0..000000000 --- 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 d84ca9573..000000000 --- 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 65fdaa96f..000000000 --- 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 356498363..000000000 --- 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 303d1d9d2..000000000 --- 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 bd56b30d9..000000000 --- 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 9a5c96267..9245a24c8 --- 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 99d0c5bf2..749b0a91c 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 -- cgit v1.2.3 From 63fa17ef0dd4b0a184f6e3e298113bb143b2cb44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:23:26 +0200 Subject: 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(-) (limited to 'src/modules') 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 e1bbf5bc7..667b74d1d 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 5b57bfb4d..5c33bc2ac 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 fbb6a18ff..5d3b6b244 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; }; -- cgit v1.2.3 From 963394f0e43a57dd765541b9c226b5cbf70c9073 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:42:01 +0200 Subject: att ekf: add descriptions for params --- .../attitude_estimator_ekf_params.c | 54 ++++++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) (limited to 'src/modules') 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 5c33bc2ac..5637ec102 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 -- cgit v1.2.3 From 58aabe648acf296267bd1e29e8a02b33ea9e7cf7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 11:25:09 +0200 Subject: update attitude estimator ekf to latest version mainly saves stack size --- src/modules/attitude_estimator_ekf/AttitudeEKF.m | 62 ++- .../attitudeKalmanfilter.prj | 7 +- .../attitude_estimator_ekf_main.cpp | 2 +- .../attitude_estimator_ekf/codegen/AttitudeEKF.c | 603 +++++++++------------ .../attitude_estimator_ekf/codegen/AttitudeEKF.h | 2 +- .../codegen/AttitudeEKF_types.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 7 files changed, 287 insertions(+), 393 deletions(-) (limited to 'src/modules') diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m index 1218cb65d..fea1a773e 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 a8315bf57..9ea520346 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 667b74d1d..766171a0f 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 d568e5737..68db382cf 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 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 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 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, + 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; + a[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]; + a[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; + S_k[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]; + 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; + d_a[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]; + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; } } for (i = 0; i < 6; i++) { - d_a[r2 + 6 * i] = 0.0F; + b_S_k[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_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; + d_a[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]; + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; } } for (i = 0; i < 6; i++) { - d_a[r2 + 6 * i] = 0.0F; + b_S_k[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_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 fc0275210..7094da1da 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 63eb7e501..3f7522ffa 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 9245a24c8..b5a02a7a6 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 * */ -- cgit v1.2.3 From 2c023c5d015fc63f7499a3b60751c771ec8a72b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 16 Oct 2014 22:52:08 +0200 Subject: mavlink: use altitude AMSL in VFR message --- src/modules/mavlink/mavlink_messages.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c182cfdb9..a730b6c98 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,9 +778,6 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; - MavlinkOrbSubscription *_sensor_combined_sub; - uint64_t _sensor_combined_time; - /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -796,9 +793,7 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0), - _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), - _sensor_combined_time(0) + _airspeed_time(0) {} void send(const hrt_abstime t) @@ -808,14 +803,12 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; - struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); - updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -824,7 +817,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); -- cgit v1.2.3 From 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: commander: Added time of RC-loss/gain to the mavlink_log_critical message to better inform the user --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df..23fdcb414 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; status_changed = true; } -- cgit v1.2.3 From 4afa8645832c71ea1c35dc73d9424df80fac531a Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 16:25:38 +0200 Subject: commander: Added duration of rc-loss to mavlink_log_critical message --- src/modules/commander/commander.cpp | 3 ++- src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 23fdcb414..d8ebee4b6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1594,6 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e3..8d797e21a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ -- cgit v1.2.3 From 276dc7fbbcc9d2b8baf65376b7ceb5e362ff978b Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 12:35:20 +0100 Subject: added px4flow integral frame, adjusted px4flow i2c driver, adjusted postition_estimator_inav --- src/drivers/px4flow/px4flow.cpp | 599 ++++++++++----------- .../position_estimator_inav_main.c | 11 +- src/modules/uORB/topics/optical_flow.h | 23 +- 3 files changed, 313 insertions(+), 320 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 04aba9eae..f74db1b52 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,15 +73,15 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x00 /* Measure Register */ - -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ +//#define PX4FLOW_REG 0x00 /* Measure Register 0*/ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -92,94 +92,103 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -//struct i2c_frame -//{ -// uint16_t frame_count; -// int16_t pixel_flow_x_sum; -// int16_t pixel_flow_y_sum; -// int16_t flow_comp_m_x; -// int16_t flow_comp_m_y; -// int16_t qual; -// int16_t gyro_x_rate; -// int16_t gyro_y_rate; -// int16_t gyro_z_rate; -// uint8_t gyro_range; -// uint8_t sonar_timestamp; -// int16_t ground_distance; -//}; -// -//struct i2c_frame f; - -class PX4FLOW : public device::I2C -{ +struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +}; +struct i2c_frame f; + +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t ground_distance; + uint8_t qual; +}__attribute__((packed)); +struct i2c_integral_frame f_integral; + + +class PX4FLOW: public device::I2C { public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - virtual int init(); + virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Diagnostics - print some basic information about the driver. + */ + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; + work_s _work; + RingBuffer *_reports;bool _sensor_ok; + int _measure_ticks;bool _collect_phase; - orb_advert_t _px4flow_topic; + orb_advert_t _px4flow_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); /** - * Stop the automatic measurement state machine. - */ - void stop(); + * Stop the automatic measurement state machine. + */ + void stop(); /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int measure(); - int collect(); + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); }; @@ -189,16 +198,12 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _px4flow_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), - _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) -{ + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( + false), _px4flow_topic(-1), _sample_perf( + perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( + perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( + perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -206,8 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -PX4FLOW::~PX4FLOW() -{ +PX4FLOW::~PX4FLOW() { /* make sure we are truly inactive */ stop(); @@ -216,9 +220,7 @@ PX4FLOW::~PX4FLOW() delete _reports; } -int -PX4FLOW::init() -{ +int PX4FLOW::init() { int ret = ERROR; /* do I2C init (and probe) first */ @@ -226,103 +228,79 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); + _reports = new RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) goto out; - /* get a publish handle on the px4flow topic */ - struct optical_flow_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - - if (_px4flow_topic < 0) - debug("failed to create px4flow object. Did you start uOrb?"); - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: - return ret; + out: return ret; } -int -PX4FLOW::probe() -{ - uint8_t val[22]; - - // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address - // 0. The ll40ls gives an error for that, whereas the flow - // happily returns some data - if (transfer(nullptr, 0, &val[0], 22) != OK) { - return -EIO; - } - - // that worked, so start a measurement cycle +int PX4FLOW::probe() { return measure(); } -int -PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) -{ +int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } - } + return OK; } + } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -358,11 +336,10 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t -PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) -{ +ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = reinterpret_cast(buffer); + struct optical_flow_s *rbuf = + reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -398,8 +375,8 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); +// /* wait for it to complete */ +// usleep(PX4FLOW_CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { @@ -407,6 +384,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } + /* wait for it to complete */ + usleep(PX4FLOW_CONVERSION_INTERVAL); + /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); @@ -417,9 +397,7 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int -PX4FLOW::measure() -{ +int PX4FLOW::measure() { int ret; /* @@ -428,11 +406,10 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } ret = OK; @@ -440,56 +417,90 @@ PX4FLOW::measure() return ret; } -int -PX4FLOW::collect() -{ - int ret = -EIO; +int PX4FLOW::collect() { + int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[46] = { 0 }; + perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 22); + if(PX4FLOW_REG==0x00){ + ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2) + } + if(PX4FLOW_REG==0x16){ + ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2) + } - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } -// f.frame_count = val[1] << 8 | val[0]; -// f.pixel_flow_x_sum= val[3] << 8 | val[2]; -// f.pixel_flow_y_sum= val[5] << 8 | val[4]; -// f.flow_comp_m_x= val[7] << 8 | val[6]; -// f.flow_comp_m_y= val[9] << 8 | val[8]; -// f.qual= val[11] << 8 | val[10]; -// f.gyro_x_rate= val[13] << 8 | val[12]; -// f.gyro_y_rate= val[15] << 8 | val[14]; -// f.gyro_z_rate= val[17] << 8 | val[16]; -// f.gyro_range= val[18]; -// f.sonar_timestamp= val[19]; -// f.ground_distance= val[21] << 8 | val[20]; - - int16_t flowcx = val[7] << 8 | val[6]; - int16_t flowcy = val[9] << 8 | val[8]; - int16_t gdist = val[21] << 8 | val[20]; + if (PX4FLOW_REG == 0) { + f.frame_count = val[1] << 8 | val[0]; + f.pixel_flow_x_sum = val[3] << 8 | val[2]; + f.pixel_flow_y_sum = val[5] << 8 | val[4]; + f.flow_comp_m_x = val[7] << 8 | val[6]; + f.flow_comp_m_y = val[9] << 8 | val[8]; + f.qual = val[11] << 8 | val[10]; + f.gyro_x_rate = val[13] << 8 | val[12]; + f.gyro_y_rate = val[15] << 8 | val[14]; + f.gyro_z_rate = val[17] << 8 | val[16]; + f.gyro_range = val[18]; + f.sonar_timestamp = val[19]; + f.ground_distance = val[21] << 8 | val[20]; + + f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; + f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; + f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; + f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; + f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; + f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; + f_integral.integration_timespan = val[37] << 24 | val[36] << 16 + | val[35] << 8 | val[34]; + f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 + | val[39] << 8 | val[38]; + f_integral.ground_distance = val[43] << 8 | val[42]; + f_integral.qual = val[44]; + } + if(PX4FLOW_REG==0x16){ + f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; + f_integral.pixel_flow_x_integral =val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral =val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral =val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral =val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral =val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12]; + f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16]; + f_integral.ground_distance =val[21] <<8 |val[20]; + f_integral.qual =val[22]; + } + struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; - report.sensor_id = 0; report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual;//0:bad ; 255 max quality + report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians + report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians + report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians + report.integration_timespan= f_integral.integration_timespan;//microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.sensor_id = 0; - - /* publish it */ - orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + if (_px4flow_topic < 0) { + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + } else { + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + } /* post a report to the ring */ if (_reports->force(&report)) { @@ -505,22 +516,17 @@ PX4FLOW::collect() return ret; } -void -PX4FLOW::start() -{ +void PX4FLOW::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + struct subsystem_info_s info = { true, true, true, + SUBSYSTEM_TYPE_OPTICALFLOW }; static orb_advert_t pub = -1; if (pub > 0) { @@ -530,71 +536,54 @@ PX4FLOW::start() } } -void -PX4FLOW::stop() -{ +void PX4FLOW::stop() { work_cancel(HPWORK, &_work); } -void -PX4FLOW::cycle_trampoline(void *arg) -{ - PX4FLOW *dev = (PX4FLOW *)arg; +void PX4FLOW::cycle_trampoline(void *arg) { + PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void -PX4FLOW::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } +void PX4FLOW::cycle() { +// /* collection phase? */ - /* next phase is measurement */ - _collect_phase = false; +// static uint64_t deltatime = hrt_absolute_time(); - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + if (OK != measure()) + log("measure error"); - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); + //usleep(PX4FLOW_CONVERSION_INTERVAL/40); - return; - } + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; } - /* measurement phase */ - if (OK != measure()) - log("measure error"); - /* next phase is collection */ - _collect_phase = true; +// deltatime = hrt_absolute_time()-deltatime; +// +// +// if(deltatime>PX4FLOW_CONVERSION_INTERVAL){ +// deltatime=PX4FLOW_CONVERSION_INTERVAL; +// } + + +// work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, +// _measure_ticks-USEC2TICK(deltatime)); + + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, + _measure_ticks); + +// deltatime = hrt_absolute_time(); - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); } -void -PX4FLOW::print_info() -{ +void PX4FLOW::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); @@ -605,8 +594,7 @@ PX4FLOW::print_info() /** * Local functions in support of the shell command. */ -namespace px4flow -{ +namespace px4flow { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -614,20 +602,18 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(); +void stop(); +void test(); +void reset(); +void info(); /** * Start the driver. */ -void -start() -{ +void start() { int fd; if (g_dev != nullptr) @@ -653,10 +639,9 @@ start() exit(0); -fail: + fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -667,15 +652,11 @@ fail: /** * Stop the driver */ -void stop() -{ - if (g_dev != nullptr) - { +void stop() { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + } else { errx(1, "driver not running"); } exit(0); @@ -686,9 +667,7 @@ void stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -void -test() -{ +void test() { struct optical_flow_s report; ssize_t sz; int ret; @@ -696,26 +675,27 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + err(1, + "%s open failed (try 'px4flow start' if the driver is not running", + PX4FLOW_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + // err(1, "immediate read failed"); - warnx("single read"); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("single read"); + warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); + warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); + /* start the sensor polling at 10Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2)) + errx(1, "failed to set 10Hz poll rate"); /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { + for (unsigned i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -733,9 +713,22 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); - warnx("time: %lld", report.timestamp); + + warnx("framecount_total: %u", f.frame_count); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); + warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral); + warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); + warnx("integration_timespan [us]: %u", f_integral.integration_timespan); + warnx("ground_distance: %0.2f m", + (double) f_integral.ground_distance / 1000); + warnx("time since last sonar update [us]: %i", + f_integral.time_since_last_sonar_update); + warnx("quality integration average : %i", f_integral.qual); + warnx("quality : %i", f.qual); } @@ -746,9 +739,7 @@ test() /** * Reset the driver. */ -void -reset() -{ +void reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) @@ -766,9 +757,7 @@ reset() /** * Print a little info about the driver. */ -void -info() -{ +void info() { if (g_dev == nullptr) errx(1, "driver not running"); @@ -780,20 +769,18 @@ info() } // namespace -int -px4flow_main(int argc, char *argv[]) -{ +int px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) px4flow::start(); - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + px4flow::stop(); /* * Test the driver/device. 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 81bbaa658..bc24e054e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -298,7 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; - hrt_abstime flow_prev = 0; // time of last flow measurement + //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) @@ -491,8 +491,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ - float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; - flow_prev = flow.flow_timestamp; +// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; +// flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && @@ -550,8 +550,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //todo check direction of x und y axis + flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 0196ae86b..f3731d213 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -55,16 +55,21 @@ */ struct optical_flow_s { - uint64_t timestamp; /**< in microseconds since system start */ - - uint64_t flow_timestamp; /**< timestamp from flow sensor */ - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint64_t timestamp; /**< in microseconds since system start */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ + float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ + float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ + float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ + float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint32_t integration_timespan; /** Date: Thu, 30 Oct 2014 16:39:02 +0100 Subject: replaced optical_flow mavlink message with optical_flow_rad, added gyro_temperature, adapted sd2log for px4flow integral frame --- src/drivers/px4flow/px4flow.cpp | 14 ++++++---- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 40 ++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++-------------- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/sdlog2/sdlog2.c | 13 +++++---- src/modules/sdlog2/sdlog2_messages.h | 18 ++++++++----- src/modules/uORB/topics/optical_flow.h | 1 + 8 files changed, 80 insertions(+), 56 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index c3660a967..daaf02a77 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -116,6 +116,7 @@ typedef struct i2c_integral_frame { uint32_t integration_timespan; uint32_t time_since_last_sonar_update; uint16_t ground_distance; + int16_t gyro_temperature; uint8_t qual; } __attribute__((packed)); struct i2c_integral_frame f_integral; @@ -456,16 +457,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[46] = { 0 }; + uint8_t val[47] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 45); // read 45 bytes (22+23 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 23); // read 23 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) } if (ret < 0) { @@ -500,7 +501,8 @@ PX4FLOW::collect() f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 | val[39] << 8 | val[38]; f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.qual = val[44]; + f_integral.gyro_temperature = val[45] << 8 | val[44]; + f_integral.qual = val[46]; } if (PX4FLOW_REG == 0x16) { @@ -513,7 +515,8 @@ PX4FLOW::collect() f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.qual = val[22]; + f_integral.gyro_temperature = val[23] << 8 | val[22]; + f_integral.qual = val[24]; } @@ -530,6 +533,7 @@ PX4FLOW::collect() report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f..3c1d360c2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1403,7 +1403,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 5.0f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; case MAVLINK_MODE_ONBOARD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..d955b5f76 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1786,33 +1786,32 @@ protected: } }; - -class MavlinkStreamOpticalFlow : public MavlinkStream +class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamOpticalFlow::get_name_static(); + return MavlinkStreamOpticalFlowRad::get_name_static(); } static const char *get_name_static() { - return "OPTICAL_FLOW"; + return "OPTICAL_FLOW_RAD"; } uint8_t get_id() { - return MAVLINK_MSG_ID_OPTICAL_FLOW; + return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(mavlink); + return new MavlinkStreamOpticalFlowRad(mavlink); } unsigned get_size() { - return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1820,11 +1819,11 @@ private: uint64_t _flow_time; /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: - explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), _flow_time(0) {} @@ -1834,18 +1833,23 @@ protected: struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_t msg; + mavlink_optical_flow_rad_t msg; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; - msg.flow_x = flow.flow_raw_x; - msg.flow_y = flow.flow_raw_y; - msg.flow_comp_m_x = flow.flow_comp_x_m; - msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; msg.quality = flow.quality; - msg.ground_distance = flow.ground_distance_m; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; - _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); } } }; @@ -2151,7 +2155,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847..bee58f89b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_int(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -389,15 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -413,7 +419,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e5f2c6a73..a057074a7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,7 +112,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); - void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde37432..54c7b0c83 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,11 +1507,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; + log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; + log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; + log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; + log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; + log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; + log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..5264ff1c8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -200,13 +200,19 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - int16_t flow_raw_x; - int16_t flow_raw_y; - float flow_comp_x; - float flow_comp_y; - float distance; - uint8_t quality; + uint64_t timestamp; uint8_t sensor_id; + float pixel_flow_x_integral; + float pixel_flow_y_integral; + float gyro_x_rate_integral; + float gyro_y_rate_integral; + float gyro_z_rate_integral; + float ground_distance_m; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t frame_count_since_last_readout; + int16_t gyro_temperature; + uint8_t quality; }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index f3731d213..d3dc46ee0 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -66,6 +66,7 @@ struct optical_flow_s { uint32_t integration_timespan; /** Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: MC pos control offb: read altitude sp separately --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 d52138522..f2c650ddd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt) _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } - if (_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); -- cgit v1.2.3 From 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 20 Nov 2014 17:25:30 +0100 Subject: commander: Change printing in RC-loss message to integers --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d8ebee4b6..1f13aaec4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; -- cgit v1.2.3 From 9bb0ecf0ca6082355072af190018e0b5298b7e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:06:32 +0100 Subject: Airspeed calibration feedback: Improve wording --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e37019d02..465f9cdc5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } } -- cgit v1.2.3 From 6da9063560b900c0ce13bb0e078889eeaf8c71c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12b..e081955d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } -- cgit v1.2.3 From 7bed194f4a217835b78f89c9e8399e1c946ca579 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: EKF: less verbose --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules') 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 35dc39ec6..e2dbc30dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ -- cgit v1.2.3 From 2fbda61521131f66cd3aab792716f3c40bc1cac8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules') 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 19c10198c..81a13edb8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From df37f380cb792570ae07b1cc3e9eb61e3495fc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules') 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 d52138522..e3c94bc46 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions -- cgit v1.2.3 From 90f28647539e7c8ae36ac74c7526582f3f18b3e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: INAV: Less verbose --- .../position_estimator_inav/position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) (limited to 'src/modules') 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 81bbaa658..f82a0f989 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel -- cgit v1.2.3 From 1709c74f82da510fff5bf546a75c9894a19d8fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d8..705e54be3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ -- cgit v1.2.3 From b3542bec085a598f34c2e796ee5c78328d0d990c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) (limited to 'src/modules') 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 e3c94bc46..5918d6bc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } 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 f82a0f989..e736a86d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -387,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -518,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -719,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } -- cgit v1.2.3 From 828163f2f5c6b68d9f262b1dd4f57e641c1d45a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:09 +0100 Subject: Update mixing handling to allow IO safety off updates --- src/modules/px4iofirmware/mixer.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c0b8ac358..66f0969de 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0; int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while safety off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } - /* abort if we're in the mixer */ + /* disable mixing, will be enabled once load is complete */ + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + + /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } @@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); - if (length < sizeof(px4io_mixdata)) + if (length < sizeof(px4io_mixdata)) { return 0; + } - unsigned text_length = length - sizeof(px4io_mixdata); + unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; @@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* disable mixing during the update */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; -- cgit v1.2.3 From 300705321a0ffa79ab3d4d53e3c029fe8238086a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:59 +0100 Subject: Allow IO safety off system handling as long as the total system is not live --- src/modules/px4iofirmware/registers.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fbfdd35db..a1a02965f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -407,10 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); break; default: @@ -583,8 +584,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + // do not reboot if FMU is armed and IO's safety is off + // this state defines an active system. + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { // don't allow reboot while armed break; } @@ -631,9 +634,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /** * do not allow a RC config change while outputs armed + * = FMU is armed and IO's safety is off + * this state defines an active system. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } -- cgit v1.2.3 From 2f271888d2ed934c271637c22554b503ce68e535 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 19:22:55 +0100 Subject: Added performance counter for SD log performance of write() call --- src/modules/sdlog2/sdlog2.c | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f7..8638a4904 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } -- cgit v1.2.3 From 08d6cbe6bf0b5b04f63e42c6c60f5b1fe6167547 Mon Sep 17 00:00:00 2001 From: philipoe Date: Sun, 23 Nov 2014 21:23:01 +0100 Subject: commander: Decrease RC-signal-regained message length to stay within 50 character length limit at all times --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index deb048566..f77895efb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1544,7 +1544,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } -- cgit v1.2.3 From 2dae1bc542746ed373458b55a8e564607700d17d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:05:10 +1100 Subject: uavcan: break the link between poll fd indexes and controls this linkage was fragile and makes it harder to add new orb subscriptions to the uavcan code --- src/modules/uavcan/uavcan_main.cpp | 5 ++--- src/modules/uavcan/uavcan_main.hpp | 3 +++ 2 files changed, 5 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2c543462e..ccc087920 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -333,14 +333,12 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { + if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - poll_id++; } } @@ -474,6 +472,7 @@ UavcanNode::subscribe() if (_control_subs[i] > 0) { _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; + _poll_ids[i] = _poll_fds_num; _poll_fds_num++; } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 274321f0d..b791663d4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -127,4 +127,7 @@ private: orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + + // index into _poll_fds for each _control_subs handle + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From f6b0a3e07f27f34af6e6d209aec761374611e968 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:35:58 +1100 Subject: uORB: added actuator_direct topic this topic will be used to allow direct output of actuator values for uavcan, bypassing the mixer. --- src/modules/uORB/Publication.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/actuator_direct.h | 69 +++++++++++++++++++++++++++++++ 3 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/actuator_direct.h (limited to 'src/modules') diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd6..71757e1f4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -46,6 +46,7 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" +#include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" @@ -76,6 +77,7 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b91a00c1e..49dfc7834 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/actuator_direct.h" +ORB_DEFINE(actuator_direct, struct actuator_direct_s); + #include "topics/multirotor_motor_limits.h" ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h new file mode 100644 index 000000000..5f9d0f56d --- /dev/null +++ b/src/modules/uORB/topics/actuator_direct.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_direct.h + * + * Actuator direct values. + * + * Values published to this topic are the direct actuator values which + * should be passed to actuators, bypassing mixing + */ + +#ifndef TOPIC_ACTUATOR_DIRECT_H +#define TOPIC_ACTUATOR_DIRECT_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATORS_DIRECT 16 + +/** + * @addtogroup topics + * @{ + */ + +struct actuator_direct_s { + uint64_t timestamp; /**< timestamp in us since system boot */ + float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ + unsigned nvalues; /**< number of valid values */ +}; + +/** + * @} + */ + +/* actuator direct ORB */ +ORB_DECLARE(actuator_direct); + +#endif // TOPIC_ACTUATOR_DIRECT_H -- cgit v1.2.3 From b830137ec8492dc583b61689b7d56a1b359a5c5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:37:02 +1100 Subject: uavcan: added support for actuator_direct ORB topic this watches the actuator_direct topic and uses it to allow for direct output of actuator values, bypassing the mixer --- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 5 +++++ 2 files changed, 28 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ccc087920..cf40f3ea8 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -280,6 +280,7 @@ int UavcanNode::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -310,6 +311,18 @@ int UavcanNode::run() _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; + /* + * setup poll to look for actuator direct input if we are + * subscribed to the topic + */ + if (_actuator_direct_sub != -1) { + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; + _poll_fds[_poll_fds_num].events = POLLIN; + _actuator_direct_poll_fd_num = _poll_fds_num; + _poll_fds_num += 1; + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { @@ -342,6 +355,16 @@ int UavcanNode::run() } } + /* + see if we have any direct actuator updates + */ + if (_actuator_direct_sub != -1 && + (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { + // Output to the bus + _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + } + // can we mix? if (_test_in_progress) { float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b791663d4..d7eb16764 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -128,6 +129,10 @@ private: pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num; + actuator_direct_s _actuator_direct; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From ead0458e97d6e19cc0b188124063e90962820e3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 16:58:16 +1100 Subject: uavcan: don't force motors to keep spinning at zero throttle Forcing motors to keep spinning when armed should be a policy decision up at the vehicle type level, not hard coded down in the ESC driver. It isn't appropriate for fixed wing or ground vehicles for example. We could add an ioctl to enable "spin when armed" if just setting a small value in the vehicle code is inconvenient --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1d23099f3..7efd50511 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -101,10 +101,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). + if (scaled < 0.0F) { + scaled = 0.0F; + } if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); -- cgit v1.2.3 From 8e44ec2e3bd6852b74e4463c373555dd1bab47d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:00:02 +1100 Subject: uavcan: prevent crash in ESC driver passing in more than 8 actuators would crash the ESC driver. We need to check again the array size of the _esc_status.esc, which is CONNECTED_ESC_MAX --- src/modules/uavcan/actuators/esc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 7efd50511..9f682c7e1 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -76,7 +76,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } -- cgit v1.2.3 From 724ec0ec8b0d34c631b2784236a990e535700254 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:02:24 +1100 Subject: uavcan: handle all ESC output in one place moving all the ESC output handling to one place allows the limits on actuator values to apply to all types of inputs, and will make it easier to expand "uavcan status" to show actuator values --- src/modules/uavcan/uavcan_main.cpp | 80 +++++++++++++++++++++----------------- src/modules/uavcan/uavcan_main.hpp | 2 + 2 files changed, 47 insertions(+), 35 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index cf40f3ea8..e4f58b310 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -282,8 +282,7 @@ int UavcanNode::run() _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) @@ -339,6 +338,8 @@ int UavcanNode::run() node_spin_once(); // Non-blocking + bool new_output = false; + // this would be bad... if (poll_ret < 0) { log("poll error %d", errno); @@ -360,18 +361,25 @@ int UavcanNode::run() */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && - orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { - // Output to the bus - _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + !_test_in_progress) { + if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], + _actuator_direct.nvalues*sizeof(float)); + _outputs.noutputs = _actuator_direct.nvalues; + new_output = true; } // can we mix? if (_test_in_progress) { - float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; - test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - - // Output to the bus - _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + _outputs.noutputs = _test_motor.motor_number+1; + } + new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -379,39 +387,41 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - outputs.timestamp = hrt_absolute_time(); - - // iterate actuators - for (unsigned i = 0; i < outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); - // limit outputs to valid range + new_output = true; + } + } - // never go below min - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - } + if (new_output) { + // iterate actuators, checking for valid values + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(_outputs.output[i])) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = -1.0f; + } - // never go below max - if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - } + // never go below min + if (_outputs.output[i] < -1.0f) { + _outputs.output[i] = -1.0f; } - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); + // never go above max + if (_outputs.output[i] > 1.0f) { + _outputs.output[i] = 1.0f; + } } + // Output to the bus + _outputs.timestamp = hrt_absolute_time(); + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); } + // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index d7eb16764..5a036fcd7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -133,6 +133,8 @@ private: uint8_t _actuator_direct_poll_fd_num; actuator_direct_s _actuator_direct; + actuator_outputs_s _outputs; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From a7a68c88a25c2c4cb401fb4f8894de6f4c280ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:03:29 +1100 Subject: uavcan: show ESC output values in uavcan status, and add arm/disarm this makes "uavcan status" show the current output values, which is useful for debugging. It also adds "uavcan arm" and "uavcan disarm" commands, which are very useful for re-arming after a motor test. --- src/modules/uavcan/uavcan_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e4f58b310..8fa765633 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -604,6 +604,14 @@ UavcanNode::print_info() (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + 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"); + } + // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { @@ -622,7 +630,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop}"); + "\tuavcan {start|status|stop|arm|disarm}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -669,6 +677,16 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "disarm")) { + inst->arm_actuators(false); + ::exit(0); + } + if (!std::strcmp(argv[1], "stop")) { delete inst; ::exit(0); -- cgit v1.2.3 From 7ae4f6d97e398a64aeb99c52880651a2282be5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Nov 2014 08:17:50 +1100 Subject: uavcan: added add_poll_fd() helper function this makes the code clearer and avoids repeated code --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++++++--------------- src/modules/uavcan/uavcan_main.hpp | 7 ++++++- 2 files changed, 27 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8fa765633..8147a8b89 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -269,6 +269,24 @@ void UavcanNode::node_spin_once() } } +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -304,22 +322,14 @@ int UavcanNode::run() * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ - _poll_fds_num = 0; - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; + add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; - _poll_fds[_poll_fds_num].events = POLLIN; - _actuator_direct_poll_fd_num = _poll_fds_num; - _poll_fds_num += 1; + _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { @@ -490,7 +500,6 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -503,10 +512,7 @@ UavcanNode::subscribe() } if (_control_subs[i] > 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_ids[i] = _poll_fds_num; - _poll_fds_num++; + _poll_ids[i] = add_poll_fd(_control_subs[i]); } } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 5a036fcd7..98f2e5ad4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -58,6 +58,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + /** * A UAVCAN node. */ @@ -98,6 +101,8 @@ private: int init(uavcan::NodeID node_id); void node_spin_once(); int run(); + int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -126,7 +131,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic -- cgit v1.2.3 From c0b47d6a74197a0dc57c56efbd63803424a9835a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Nov 2014 15:48:33 +1100 Subject: px4io: only check SAFETY_OFF for allowing RC config changes and reboot If we check OUTPUTS_ARMED then we can't update trim values and scaling in flight, as there is no way to clear OUTPUTS_ARMED. If safety is on then it should be perfectly safe to update the mixer and RC config or reboot for fw update --- src/modules/px4iofirmware/registers.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a1a02965f..f0c2cfd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -412,7 +412,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num * text handling function. */ return mixer_handle_text(values, num_values * sizeof(*values)); - break; default: /* avoid offset wrap */ @@ -584,10 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - // do not reboot if FMU is armed and IO's safety is off - // this state defines an active system. - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -633,12 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed - * = FMU is armed and IO's safety is off - * this state defines an active system. + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } -- cgit v1.2.3 From 8e8b622f4f5c4737e0f7dad33870f23742449fe9 Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Fri, 28 Nov 2014 04:11:39 +0100 Subject: Implemented altitude and velocity hold mode --- src/modules/fw_att_control/fw_att_control_main.cpp | 23 +++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 86 +++++++++++++++++++++- 2 files changed, 106 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 e770c11a2..a295d61ac 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -751,14 +751,33 @@ FixedwingAttitudeControl::task_main() * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ - if (_vcontrol_mode.flag_control_velocity_enabled || - _vcontrol_mode.flag_control_position_enabled || + if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } + else if (_vcontrol_mode.flag_control_velocity_enabled) { + /* + * Velocity should be controlled and manual is enabled. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); 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 6017369aa..4f84f088b 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 @@ -163,6 +163,8 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + float _hold_alt; /**< hold altitude for velocity mode */ + /* land states */ bool land_noreturn_horizontal; bool land_noreturn_vertical; @@ -198,6 +200,8 @@ private: TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; + bool _was_velocity_control_mode; + bool _was_alt_control_mode; struct { float l1_period; @@ -316,6 +320,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for manual setpoint updates. + */ + bool vehicle_manual_control_setpoint_poll(); + /** * Check for airspeed updates. */ @@ -692,6 +701,22 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::vehicle_manual_control_setpoint_poll() +{ + bool manual_updated; + + /* Check if manual setpoint has changed */ + orb_check(_manual_control_sub, &manual_updated); + + if (manual_updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + } + + return manual_updated; +} + + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -852,6 +877,13 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { + static hrt_abstime functionLastCalled = 0; + float dt = 0.0f; + if (functionLastCalled > 0) { + dt = (float)hrt_elapsed_time(&functionLastCalled) * 1e-6f; + } + functionLastCalled = hrt_absolute_time(); + bool setpoint = true; math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -888,6 +920,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _was_pos_control_mode = true; + _was_velocity_control_mode = false; + + /* reset hold altitude */ + _hold_alt = _global_pos.alt; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1209,12 +1245,59 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } + } else if (_control_mode.flag_control_velocity_enabled) { + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (!_was_velocity_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = false; + } + _was_velocity_control_mode = true; + _was_pos_control_mode = false; + // Get demanded airspeed + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + // Get demanded vertical velocity from pitch control + float pitch = 0.0f; + if (_manual.x > deadBand) { + pitch = (_manual.x - deadBand) / factor; + } else if (_manual.x < - deadBand) { + pitch = (_manual.x + deadBand) / factor; + } + if (pitch > 0.0f) { + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (pitch < 0.0f) { + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (!_was_alt_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); } else { - + _was_velocity_control_mode = false; _was_pos_control_mode = false; /** MANUAL FLIGHT **/ + /* reset hold altitude */ + _hold_alt = _global_pos.alt; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1350,6 +1433,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); -- cgit v1.2.3 From f619dba6f96d22152c1c97216a46a27981ff2472 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 15:33:21 +0100 Subject: Corrected time_gps_usec values description. Fixes #1474 --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 31e616f4f..7888a50f4 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 */ }; -- cgit v1.2.3 From 6efc63d709f5afdbed29528647776d56e7f384a6 Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Sun, 30 Nov 2014 20:51:01 +0100 Subject: fixed somereview comments --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 +-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 ++++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) (limited to 'src/modules') 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 a295d61ac..2d5744b8a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -768,8 +768,7 @@ FixedwingAttitudeControl::task_main() if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } - } - else if (_vcontrol_mode.flag_control_velocity_enabled) { + } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* * Velocity should be controlled and manual 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 4f84f088b..e7c95cc86 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 @@ -164,6 +164,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for velocity mode */ + hrt_abstime _control_position_last_called; /** ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - static hrt_abstime functionLastCalled = 0; - float dt = 0.0f; - if (functionLastCalled > 0) { - dt = (float)hrt_elapsed_time(&functionLastCalled) * 1e-6f; + float dt = FLT_MIN; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { + dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } - functionLastCalled = hrt_absolute_time(); + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; -- cgit v1.2.3 From 3ce7abe9d84a6c716985339c8165bec98e481847 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:01:35 +0100 Subject: use consistent orientation naming in messages --- src/modules/commander/accelerometer_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0cb41489f..8eac0a6fd 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); -- cgit v1.2.3 From b1bd813978e48d32a66b8a49bccdda7f053ea55c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:37:54 +0100 Subject: swap fron/back > the "side" being measured is facing downwards --- src/modules/commander/accelerometer_calibration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 8eac0a6fd..49c653ad2 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; + const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -294,8 +294,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", - (!data_collected[0]) ? "front " : "", - (!data_collected[1]) ? "back " : "", + (!data_collected[0]) ? "back " : "", + (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", (!data_collected[4]) ? "up " : "", -- cgit v1.2.3 From 45c52b51ee84bead9562a51a4d410fcbe921e798 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:42:12 +0100 Subject: move natural position to the front of the pending list for QGC --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49c653ad2..d4cd97be6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", - (!data_collected[4]) ? "up " : "", - (!data_collected[5]) ? "down " : ""); + (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); -- cgit v1.2.3 From 35c985e5234f2b2c7cc511a791a1be90bae820bf Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:32:13 +0100 Subject: extended uORB structs with VTOL specific control topics --- src/modules/uORB/objects_common.cpp | 10 ++++ src/modules/uORB/topics/actuator_controls.h | 3 + src/modules/uORB/topics/vehicle_attitude.h | 17 ++++++ .../uORB/topics/vehicle_attitude_setpoint.h | 2 + src/modules/uORB/topics/vehicle_rates_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_status.h | 5 +- src/modules/uORB/topics/vtol_vehicle_status.h | 66 ++++++++++++++++++++++ 7 files changed, 104 insertions(+), 2 deletions(-) create mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h (limited to 'src/modules') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 49dfc7834..1141431cc 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/vtol_vehicle_status.h" +ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s); + #include "topics/safety.h" ORB_DEFINE(safety, struct safety_s); @@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); +ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); +ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); +ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); @@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +//Virtual control groups, used for VTOL operation +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index e768ab2f6..4d1f9a638 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); +ORB_DECLARE(actuator_controls_virtual_mc); +ORB_DECLARE(actuator_controls_virtual_fw); + #endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 40328af14..77324415a 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -80,6 +80,23 @@ struct vehicle_attitude_s { bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ + // secondary attitude, use for VTOL + float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ + float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ + float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ + float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ + float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ + float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ + float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ + float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ + float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ + float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ + float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ + float q_sec[4]; /**< Quaternion (NED) */ + float g_comp_sec[3]; /**< Compensated gravity vector */ + bool R_valid_sec; /**< Rotation matrix valid */ + bool q_valid_sec; /**< Quaternion valid */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8446e9c6e..1cfc37cc6 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_attitude_setpoint); +ORB_DECLARE(mc_virtual_attitude_setpoint); +ORB_DECLARE(fw_virtual_attitude_setpoint); #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 9f8b412a7..47d51f199 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); - +ORB_DECLARE(mc_virtual_rates_setpoint); +ORB_DECLARE(fw_virtual_rates_setpoint); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 8ec9d98d6..749d00d75 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -147,7 +147,10 @@ enum VEHICLE_TYPE { VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ VEHICLE_TYPE_KITE = 17, /* Kite | */ - VEHICLE_TYPE_ENUM_END = 18, /* | */ + VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */ + VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/ + VEHICLE_TYPE_ENUM_END = 21 /* | */ }; enum VEHICLE_BATTERY_WARNING { 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 000000000..24ecca9fa --- /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 -- cgit v1.2.3 From ad204bbb5d2bab4431b15281fde693e6bf03a0fd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:33:43 +0100 Subject: log secondary attitude and fixed wing controls for VTOL --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 13 ++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0b6861d2a..8f87d897a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -941,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; + struct actuator_controls_s act_controls1; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; + struct log_FWC_s log_FWC; struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; @@ -1022,6 +1024,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; + int act_controls_1_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -1055,6 +1058,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -1375,6 +1379,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); + // secondary attitude + log_msg.msg_type = LOG_ATT2_MSG; + log_msg.body.log_ATT.roll = buf.att.roll_sec; + log_msg.body.log_ATT.pitch = buf.att.pitch_sec; + log_msg.body.log_ATT.yaw = buf.att.yaw_sec; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; + log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; + LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ @@ -1413,6 +1429,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } + if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls1)) { + log_msg.msg_type = LOG_FWC_MSG; + log_msg.body.log_FWC.roll = buf.act_controls1.control[0]; + log_msg.body.log_FWC.pitch = buf.act_controls1.control[1]; + log_msg.body.log_FWC.yaw = buf.act_controls1.control[2]; + log_msg.body.log_FWC.thrust = buf.act_controls1.control[3]; + LOGBUFFER_WRITE_AND_COUNT(FWC); + } + /* --- LOCAL POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { log_msg.msg_type = LOG_LPOS_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 30491036a..447b3fb9c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,6 +50,7 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 +#define LOG_ATT2_MSG 41 struct log_ATT_s { float roll; float pitch; @@ -422,6 +423,14 @@ struct log_ENCD_s { float vel1; }; +/* ATTITUDE CONTROLS (ACTUATOR_1 CONTROLS) */ +#define LOG_FWC_MSG 40 +struct log_FWC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -450,7 +459,8 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -460,6 +470,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(FWC,"ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), -- cgit v1.2.3 From ff55652f9a9cdb4cdad216f4c6166681d10f278c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:34:39 +0100 Subject: adapted attitude controllers to support VTOL --- src/modules/fw_att_control/fw_att_control_main.cpp | 160 +++++++++++++++------ src/modules/mc_att_control/mc_att_control_main.cpp | 89 +++++++++--- 2 files changed, 191 insertions(+), 58 deletions(-) (limited to 'src/modules') 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 e770c11a2..daf787863 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -35,8 +35,9 @@ * @file fw_att_control_main.c * Implementation of a generic attitude controller based on classic orthogonal PIDs. * - * @author Lorenz Meier - * @author Thomas Gubler + * @author Lorenz Meier + * @author Thomas Gubler + * @author Roman Bapst * */ @@ -76,6 +77,7 @@ #include #include + /** * Fixedwing attitude control app start / stop handling function * @@ -92,12 +94,12 @@ public: FixedwingAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task. */ ~FixedwingAttitudeControl(); /** - * Start the sensors task. + * Start the main task. * * @return OK on success. */ @@ -112,9 +114,9 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, attitude control task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ + int _control_task; /**< task handle */ int _att_sub; /**< vehicle attitude subscription */ int _accel_sub; /**< accelerometer subscription */ @@ -128,13 +130,16 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ + orb_advert_t _actuators_2_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 vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ @@ -189,6 +194,8 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + param_t autostart_id; /* indicates which airframe is used */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -228,6 +235,8 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; + + param_t autostart_id; /* indicates which airframe is used */ } _parameter_handles; /**< handles for interesting parameters */ @@ -289,7 +298,7 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main attitude controller collection task. */ void task_main(); @@ -325,9 +334,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), + _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_1_pub(-1), + _actuators_virtual_fw_pub(-1), + _actuators_2_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -341,6 +352,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _att = {}; _accel = {}; _att_sp = {}; + _rates_sp = {}; _manual = {}; _airspeed = {}; _vcontrol_mode = {}; @@ -386,6 +398,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.autostart_id = param_find("SYS_AUTOSTART"); + /* fetch initial parameter values */ parameters_update(); } @@ -462,6 +476,7 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.autostart_id, &_parameters.autostart_id); /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); @@ -497,7 +512,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { @@ -529,7 +544,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); -// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); } } @@ -616,6 +630,18 @@ FixedwingAttitudeControl::task_main() parameters_update(); + /*Publish to correct actuator control topic, depending on what airframe we are using + * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing + * topic, from which the vtol_att_control module is receiving data and processing it further)*/ + if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ + _actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators); + _rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp); + + } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -679,6 +705,65 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (_parameters.autostart_id >= 13000 + && _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude! + /* The following modification to the attitude is vehicle specific and in this case applies + to tail-sitter models !!! + + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + math::Matrix<3, 3> R; //original rotation matrix + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R.set(_att.R); + R_adapted.set(_att.R); + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation + euler_angles = R_adapted.to_euler(); + //fill in new attitude data + _att.roll = euler_angles(0); + _att.pitch = euler_angles(1); + _att.yaw = euler_angles(2); + _att.R[0][0] = R_adapted(0, 0); + _att.R[0][1] = R_adapted(0, 1); + _att.R[0][2] = R_adapted(0, 2); + _att.R[1][0] = R_adapted(1, 0); + _att.R[1][1] = R_adapted(1, 1); + _att.R[1][2] = R_adapted(1, 2); + _att.R[2][0] = R_adapted(2, 0); + _att.R[2][1] = R_adapted(2, 1); + _att.R[2][2] = R_adapted(2, 2); + + // lastly, roll- and yawspeed have to be swaped + float helper = _att.rollspeed; + _att.rollspeed = -_att.yawspeed; + _att.yawspeed = helper; + } + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -696,7 +781,7 @@ FixedwingAttitudeControl::task_main() /* lock integrator until control is started */ bool lock_integrator; - if (_vcontrol_mode.flag_control_attitude_enabled) { + if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { @@ -705,10 +790,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { - _actuators_airframe.control[1] = 1.0f; + _actuators_airframe.control[7] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { - _actuators_airframe.control[1] = 0.0f; + _actuators_airframe.control[7] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } @@ -802,18 +887,18 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub > 0) { + if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - } else { + } else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* If the aircraft is on ground reset the integrators */ - if (_vehicle_status.condition_landed) { + if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); @@ -915,20 +1000,19 @@ FixedwingAttitudeControl::task_main() * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = _yaw_ctrl.get_desired_rate(); + _rates_sp.roll = _roll_ctrl.get_desired_rate(); + _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); - rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + /* publish the attitude rates setpoint */ + orb_publish(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); + } else if (_rate_sp_virtual_pub > 0) { + /* publish the virtual attitude setpoint */ + orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp); } } else { @@ -948,28 +1032,22 @@ FixedwingAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ + /* publish the actuator controls */ + if (_actuators_0_pub > 0) { //normal fixed wing airframe 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); + } else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe + orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators); } - if (_actuators_1_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); -// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", -// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], -// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], -// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } - } loop_counter++; 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 81a13edb8..c5dccd8c3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -96,12 +97,12 @@ public: MulticopterAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task */ ~MulticopterAttitudeControl(); /** - * Start the sensors task. + * Start the multicopter attitude control task. * * @return OK on success. */ @@ -109,8 +110,8 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, task_main() should exit */ + int _control_task; /**< task handle */ int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -119,10 +120,13 @@ private: int _params_sub; /**< parameter updates subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -133,6 +137,7 @@ private: struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -168,6 +173,8 @@ private: param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; + + param_t autostart_id; //what frame are we using? } _params_handles; /**< handles for interesting parameters */ struct { @@ -182,6 +189,8 @@ private: float man_pitch_max; float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + param_t autostart_id; } _params; /** @@ -229,6 +238,11 @@ private: */ void control_attitude_rates(float dt); + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -264,11 +278,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _vehicle_status_sub(-1), /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), + _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), + _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -283,6 +300,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); _params.rate_p.zero(); @@ -295,6 +314,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.man_yaw_max = 0.0f; _params.acro_rate_max.zero(); + _params.autostart_id = 0; //default + _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); @@ -324,6 +345,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.autostart_id = param_find("SYS_AUTOSTART"); + /* fetch initial parameter values */ parameters_update(); } @@ -409,6 +432,8 @@ MulticopterAttitudeControl::parameters_update() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + param_get(_params_handles.autostart_id, &_params.autostart_id); + return OK; } @@ -417,7 +442,7 @@ MulticopterAttitudeControl::parameter_update_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if parameters have changed */ orb_check(_params_sub, &updated); if (updated) { @@ -432,7 +457,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_v_control_mode_sub, &updated); if (updated) { @@ -489,6 +514,18 @@ MulticopterAttitudeControl::arming_status_poll() } } +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -585,7 +622,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* publish the attitude setpoint if needed */ - if (publish_att_sp) { + if (publish_att_sp && _vehicle_status.is_rotary_wing) { _v_att_sp.timestamp = hrt_absolute_time(); if (_att_sp_pub > 0) { @@ -682,7 +719,7 @@ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed.armed) { + if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } @@ -721,6 +758,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { + warnx("started"); + fflush(stdout); /* * do subscriptions @@ -732,10 +771,24 @@ MulticopterAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* initialize parameters cache */ parameters_update(); + /*Subscribe to correct actuator control topic, depending on what airframe we are using + * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter + * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); + _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); + + } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -783,6 +836,7 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); + vehicle_status_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -797,8 +851,8 @@ MulticopterAttitudeControl::task_main() 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 if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); } } else { @@ -819,11 +873,11 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(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 if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -846,12 +900,13 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { + if (_actuators_0_pub > 0) { //normal mutlicopter airframe orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { + orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); } + } } } -- cgit v1.2.3 From 285a0e7be9c6227e0353e356cbbd6f48e54d057e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:36:30 +0100 Subject: added more mixer geometries and took v-mixer out of multi_tables script --- src/modules/systemlib/mixer/mixer.h | 1 + src/modules/systemlib/mixer/mixer_multirotor.cpp | 19 +++++++++++++++---- src/modules/systemlib/mixer/multi_tables | 11 ++++------- 3 files changed, 20 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 17989558e..f14bdb9fd 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -466,6 +466,7 @@ public: OCTA_X, OCTA_PLUS, OCTA_COX, + TWIN_ENGINE, /**< VTOL: one engine on each wing */ MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 57e17b67d..24187c9bc 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -76,6 +76,7 @@ float constrain(float val, float min, float max) /* * These tables automatically generated by multi_tables - do not edit. */ + const MultirotorMixer::Rotor _config_quad_x[] = { { -0.707107, 0.707107, 1.00 }, { 0.707107, -0.707107, 1.00 }, @@ -88,11 +89,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { 0.000000, 1.000000, -1.00 }, { -0.000000, -1.000000, -1.00 }, }; +//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.694658, -0.719340, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.694658, -0.719340, -1.00 }, + { -0.3223, 0.9466, 0.4242 }, + { 0.3223, -0.9466, 1.0000 }, + { 0.3223, 0.9466, -0.4242 }, + { -0.3223, -0.9466, -1.0000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { { -0.927184, 0.374607, 1.00 }, @@ -154,6 +156,11 @@ const MultirotorMixer::Rotor _config_octa_cox[] = { { -0.707107, -0.707107, 1.00 }, { 0.707107, -0.707107, -1.00 }, }; +const MultirotorMixer::Rotor _config_duorotor[] = { + { -1.000000, 0.000000, 0.00 }, + { 1.000000, 0.000000, 0.00 }, +}; + const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -165,6 +172,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], + &_config_duorotor[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ @@ -177,6 +185,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 8, /* octa_x */ 8, /* octa_plus */ 8, /* octa_cox */ + 2, /* twin_engine */ }; } @@ -274,6 +283,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8c")) { geometry = MultirotorMixer::OCTA_COX; + } else if (!strcmp(geomname, "2-")) { + geometry = MultirotorMixer::TWIN_ENGINE; } else { debug("unrecognised geometry '%s'", geomname); return nullptr; diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index b5698036e..18c828578 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -6,6 +6,7 @@ proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } proc rcos {a} { expr cos([rad $a])} + set quad_x { 45 CCW -135 CCW @@ -20,12 +21,6 @@ set quad_plus { 180 CW } -set quad_v { - 68 CCW - -136 CCW - -68 CW - 136 CW -} set quad_wide { 68 CCW @@ -94,7 +89,9 @@ set octa_cox { -135 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} + +set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} + proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} -- cgit v1.2.3 From 15ad9e441ef87b3e917a057e2f4b24d4bf3d0cee Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:38:39 +0100 Subject: compute secondary attitude with reference frame rotated -90 degress around pitch axis of original reference frame (used for VTOL) --- .../attitude_estimator_ekf_main.cpp | 38 ++++++++++++++++++++++ 1 file changed, 38 insertions(+) (limited to 'src/modules') 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 e2dbc30dd..6efcbc7f9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -552,6 +552,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; + // compute secondary attitude + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R_adapted = R; + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation + euler_angles_sec = R_adapted.to_euler(); + + att.roll_sec = euler_angles_sec(0); + att.pitch_sec = euler_angles_sec(1); + att.yaw_sec = euler_angles_sec(2); + + memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); + + att.rollspeed_sec = -x_aposteriori[2]; + att.pitchspeed_sec = x_aposteriori[1]; + att.yawspeed_sec = x_aposteriori[0]; + att.rollacc_sec = -x_aposteriori[5]; + att.pitchacc_sec = x_aposteriori[4]; + att.yawacc_sec = x_aposteriori[3]; + + att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); + att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); -- cgit v1.2.3 From 9a64004d7ff60d0c4ca25877b34183631636112a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:47:07 +0100 Subject: let commander know if VTOL is in fw or in mc mode (important because of external_override) --- src/modules/commander/commander.cpp | 41 ++++++++++++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b5..bed467698 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include #include @@ -149,6 +150,9 @@ enum MAV_MODE_FLAG { /* Mavlink file descriptors */ static int mavlink_fd = 0; +/* Syste autostart ID */ +static int autostart_id; + /* flags */ static bool commander_initialized = false; static volatile bool thread_should_exit = false; /**< daemon exit flag */ @@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); + param_t _param_autostart_id = param_find("SYS_AUTOSTART"); /* welcome user */ warnx("starting"); @@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[]) struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); + /* Subscribe to vtol vehicle status topic */ + int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); + struct vtol_vehicle_status_s vtol_status; + memset(&vtol_status, 0, sizeof(vtol_status)); + vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing + + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[]) status.system_type == VEHICLE_TYPE_TRICOPTER || status.system_type == VEHICLE_TYPE_QUADROTOR || status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR) { + status.system_type == VEHICLE_TYPE_OCTOROTOR || + (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + status.is_rotary_wing = true; } else { @@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + param_get(_param_autostart_id, &autostart_id); } orb_check(sp_man_sub, &updated); @@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update vtol vehicle status*/ + orb_check(vtol_vehicle_status_sub, &updated); + + if (updated) { + /* vtol status changed */ + orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); + + /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ + if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + } + } + /* update global position estimate */ orb_check(global_position_sub, &updated); @@ -2189,7 +2218,13 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; /* TODO: check this */ - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + if (autostart_id < 13000 || autostart_id >= 14000) { + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + + } else { + control_mode.flag_external_manual_override_ok = false; + } + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; @@ -2205,7 +2240,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; break; - + case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; -- cgit v1.2.3 From 0dc202502de8e879f427c94ce0c415f8a558c2fb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:49:19 +0100 Subject: added VTOL geometries to determine number of motors --- src/modules/mavlink/mavlink_messages.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8f956ad0..a68381f9f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1358,7 +1358,10 @@ protected: /* scale outputs depending on system type */ if (system_type == MAV_TYPE_QUADROTOR || system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR) { + system_type == MAV_TYPE_OCTOROTOR || + system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR) { + /* multirotors: set number of rotor outputs depending on type */ unsigned n; @@ -1372,6 +1375,14 @@ protected: n = 6; break; + case MAV_TYPE_VTOL_DUOROTOR: + n = 2; + break; + + case MAV_TYPE_VTOL_QUADROTOR: + n = 4; + break; + default: n = 8; break; -- cgit v1.2.3 From 887ed69099e43d7f6f3852c11450cd9ef73c2d3b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:50:25 +0100 Subject: added VTOL attitude control module --- src/modules/vtol_att_control/module.mk | 40 ++ .../vtol_att_control/vtol_att_control_main.cpp | 716 +++++++++++++++++++++ 2 files changed, 756 insertions(+) create mode 100644 src/modules/vtol_att_control/module.mk create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk new file mode 100644 index 000000000..c349c2340 --- /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 000000000..62b6a4ed6 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -0,0 +1,716 @@ +/**************************************************************************** + * + * 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 _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _fw_virtual_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; + orb_advert_t _v_rates_sp_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 vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw 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 vehicle_rates_sp_mc_poll(); + void vehicle_rates_sp_fw_poll(); + 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 fill_mc_att_rates_sp(); + void fill_fw_att_rates_sp(); + 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), + _mc_virtual_v_rates_sp_sub(-1), + _fw_virtual_v_rates_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), + _v_rates_sp_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(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); + memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_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 attitude rates setpoint from mc attitude controller +*/ +void VtolAttitudeControl::vehicle_rates_sp_mc_poll() +{ + bool updated; + orb_check(_mc_virtual_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub , &_mc_virtual_v_rates_sp); + } +} + +/** +* Check for attitude rates setpoint from fw attitude controller +*/ +void VtolAttitudeControl::vehicle_rates_sp_fw_poll() +{ + bool updated; + orb_check(_fw_virtual_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub , &_fw_virtual_v_rates_sp); + } +} + +/** +* 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] = _actuators_mc_in.control[2]; //roll elevon + _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //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 */ +} + +/** +* Prepare message for mc attitude rates setpoint topic +*/ +void VtolAttitudeControl::fill_mc_att_rates_sp() +{ + _v_rates_sp.roll = _mc_virtual_v_rates_sp.roll; + _v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch; + _v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw; + _v_rates_sp.thrust = _mc_virtual_v_rates_sp.thrust; +} + +/** +* Prepare message for fw attitude rates setpoint topic +*/ +void VtolAttitudeControl::fill_fw_att_rates_sp() +{ + _v_rates_sp.roll = _fw_virtual_v_rates_sp.roll; + _v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch; + _v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw; + _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; +} + +/** +* 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)); + _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); + _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_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 + vehicle_rates_sp_mc_poll(); + vehicle_rates_sp_fw_poll(); + 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(); + fill_mc_att_rates_sp(); + + 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(); + fill_fw_att_rates_sp(); + + 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); + } + } + } + + // publish the attitude rates setpoint + 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); + } + } + + 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; +} -- cgit v1.2.3 From 0e33e52d4ae2df06467f5b862bb0734c3ba23015 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:44:34 +0100 Subject: use uORB ID to determine the correct rate_sp- and actuator topic to publish on --- src/modules/fw_att_control/fw_att_control_main.cpp | 48 +++++++++----------- src/modules/mc_att_control/mc_att_control_main.cpp | 51 ++++++++++------------ 2 files changed, 44 insertions(+), 55 deletions(-) (limited to 'src/modules') 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 daf787863..679ef1d29 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -130,12 +130,13 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure + orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -334,10 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_fw_pub(-1), _actuators_2_pub(-1), /* performance counters */ @@ -402,6 +401,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + // set correct uORB ID, depending on if vehicle is VTOL or not + if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + } + else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -629,19 +637,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - - /*Publish to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing - * topic, from which the vtol_att_control module is receiving data and processing it further)*/ - if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators); - _rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -1008,11 +1004,10 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else if (_rate_sp_virtual_pub > 0) { - /* publish the virtual attitude setpoint */ - orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp); + orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else { + /* advertise the attitude rates setpoint */ + _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { @@ -1033,11 +1028,10 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); /* publish the actuator controls */ - if (_actuators_0_pub > 0) { //normal fixed wing airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe - orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub > 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 c5dccd8c3..2431f07f4 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -124,9 +124,10 @@ private: orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ + + orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure + orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -283,9 +284,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), - _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -349,6 +348,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + // set correct uORB ID, depending on if vehicle is VTOL or not + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } + else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -776,19 +784,6 @@ MulticopterAttitudeControl::task_main() /* initialize parameters cache */ parameters_update(); - /*Subscribe to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter - * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ - if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); - _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -849,10 +844,10 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { @@ -873,11 +868,11 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); - } + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -900,11 +895,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { //normal mutlicopter airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { - orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); + } else { + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } -- cgit v1.2.3 From f7ca1d36d2a35b9781a41ffc40acfb2eaf84544f Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:46:52 +0100 Subject: renamed FWC to ATC1 --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- src/modules/sdlog2/sdlog2_messages.h | 14 +++----------- 2 files changed, 11 insertions(+), 19 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8f87d897a..6df677338 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -982,7 +982,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; - struct log_FWC_s log_FWC; struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; @@ -1429,13 +1428,14 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls1)) { - log_msg.msg_type = LOG_FWC_MSG; - log_msg.body.log_FWC.roll = buf.act_controls1.control[0]; - log_msg.body.log_FWC.pitch = buf.act_controls1.control[1]; - log_msg.body.log_FWC.yaw = buf.act_controls1.control[2]; - log_msg.body.log_FWC.thrust = buf.act_controls1.control[3]; - LOGBUFFER_WRITE_AND_COUNT(FWC); + /* --- ACTUATOR CONTROL FW VTOL --- */ + if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) { + log_msg.msg_type = LOG_ATC1_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- LOCAL POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 447b3fb9c..b78b430aa 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -150,6 +150,7 @@ struct log_GPS_s { /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ #define LOG_ATTC_MSG 9 +#define LOG_ATC1_MSG 40 struct log_ATTC_s { float roll; float pitch; @@ -423,15 +424,6 @@ struct log_ENCD_s { float vel1; }; -/* ATTITUDE CONTROLS (ACTUATOR_1 CONTROLS) */ -#define LOG_FWC_MSG 40 -struct log_FWC_s { - float roll; - float pitch; - float yaw; - float thrust; -}; - /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -469,8 +461,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), - LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(FWC,"ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), -- cgit v1.2.3 From 563de3b49c7e12c9745744b859b5aeedb0535090 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:47:56 +0100 Subject: in fw mode, publish also yaw and throttle on control group 1 for logging --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 62b6a4ed6..cc3a3665a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -402,8 +402,11 @@ void VtolAttitudeControl::fill_fw_att_control_output() _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 */ + _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon + _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon + // unused now but still logged + _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw + _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle } /** -- cgit v1.2.3 From 7a344b933738d9caedc0fe177cc4615c294eaf51 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:30:49 +0100 Subject: Display ESC data in the status output --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8147a8b89..7213c72c6 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 -- cgit v1.2.3 From f3fb32bc4790673e4a643f341b16d2434423dbea Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:43:17 +0100 Subject: Unsubscribe from the topic. --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 7213c72c6..60901e0c7 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 -- cgit v1.2.3 From fbedd3ed206531704f24a0b76657c4583a49efe8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 14:23:19 +0100 Subject: removed whitespaces removed small unused code block --- src/modules/commander/commander.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 3 +-- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 3 files changed, 2 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bed467698..4c7664cd0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2240,7 +2240,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; break; - + case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; 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 679ef1d29..956fc94de 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -77,7 +77,6 @@ #include #include - /** * Fixedwing attitude control app start / stop handling function * @@ -637,7 +636,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); 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 2431f07f4..0702e6378 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -766,8 +766,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From 4fd3e0720f37db472a689b869d3c4019f90acd25 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:55:12 +0100 Subject: added parameter file for vtol attitude control module --- src/modules/vtol_att_control/vtol_att_control_params.c | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 src/modules/vtol_att_control/vtol_att_control_params.c (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c new file mode 100644 index 000000000..b9baa2046 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -0,0 +1,6 @@ +#include + +// number of engines +PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); +// idle pwm in multicopter mode +PARAM_DEFINE_INT32(IDLE_PWM_MC,900); \ No newline at end of file -- cgit v1.2.3 From 7ea4d10bc9802a0d2a167f65e1bb843ca5643519 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:56:08 +0100 Subject: added parameter file to makefile --- src/modules/vtol_att_control/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index c349c2340..0d5155e90 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -37,4 +37,5 @@ MODULE_COMMAND = vtol_att_control -SRCS = vtol_att_control_main.cpp +SRCS = vtol_att_control_main.cpp \ + vtol_att_control_params.c -- cgit v1.2.3 From bed53e810b106f5f9206a89676d493e1e50edc1d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:57:06 +0100 Subject: introduced vtol_motor_count and idle_pwm_mc parameter --- .../vtol_att_control/vtol_att_control_main.cpp | 28 ++++++++++++++-------- 1 file changed, 18 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index cc3a3665a..c9c6368b2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -132,11 +132,13 @@ private: struct actuator_armed_s _armed; //actuator arming status struct { - float min_pwm_mc; //pwm value for idle in mc mode + param_t idle_pwm_mc; //pwm value for idle in mc mode + param_t vtol_motor_count; } _params; struct { - param_t min_pwm_mc; + param_t idle_pwm_mc; + param_t vtol_motor_count; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -145,6 +147,7 @@ private: * 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" + unsigned _motor_count; // number of motors //*****************Member functions*********************************************************************** @@ -217,7 +220,11 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); - _params_handles.min_pwm_mc = param_find("PWM_MIN"); + _params.idle_pwm_mc = PWM_LOWEST_MIN; + _params.vtol_motor_count = 0; + + _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); + _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); /* fetch initial parameter values */ parameters_update(); @@ -369,10 +376,11 @@ VtolAttitudeControl::parameters_update_poll() int VtolAttitudeControl::parameters_update() { - /* idle pwm */ - float v; - param_get(_params_handles.min_pwm_mc, &v); - _params.min_pwm_mc = v; + /* idle pwm for mc mode */ + param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); + + /* vtol motor count */ + param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); return OK; } @@ -446,7 +454,7 @@ void VtolAttitudeControl::set_idle_fw() struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i < _params.vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count++; @@ -472,11 +480,11 @@ void VtolAttitudeControl::set_idle_mc() 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; + unsigned pwm_value = _params.idle_pwm_mc; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i < _params.vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count++; } -- cgit v1.2.3 From 590489d49859c77ef2e919f2ef45e9dc81e28f9f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 17:36:16 +0100 Subject: make sure vtol attitude control module is started with idle speed set for multicopter mode --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c9c6368b2..e69c51142 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -202,7 +202,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ + flag_idle_mc = true; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -520,7 +520,11 @@ void VtolAttitudeControl::task_main() _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/* + parameters_update(); // initialize parameter cache + + // make sure we start with idle in mc mode + set_idle_mc(); + flag_idle_mc = true; /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ -- cgit v1.2.3 From 945c8df18bfc25650bd01d9358295f1c2102a61f Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 6 Dec 2014 17:37:46 +0100 Subject: added scaling of mc pitch control with airspeed --- .../vtol_att_control/vtol_att_control_main.cpp | 78 +++++++++++++++++++++- .../vtol_att_control/vtol_att_control_params.c | 9 ++- 2 files changed, 85 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e69c51142..2bd4d0c25 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,7 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _airspeed_sub; // airspeed 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 @@ -130,18 +132,26 @@ private: 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 airspeed_s _airspeed; // airspeed struct { param_t idle_pwm_mc; //pwm value for idle in mc mode param_t vtol_motor_count; + float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) + float mc_airspeed_trim; // trim airspeed in multicopter mode + float mc_airspeed_max; // max airpseed in multicopter mode } _params; struct { param_t idle_pwm_mc; param_t vtol_motor_count; + param_t mc_airspeed_min; + param_t mc_airspeed_trim; + param_t mc_airspeed_max; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ /* 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 @@ -161,6 +171,7 @@ private: void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); + void vehicle_airspeed_poll(); // Check for changes in airspeed 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 @@ -169,6 +180,7 @@ private: void fill_fw_att_rates_sp(); void set_idle_fw(); void set_idle_mc(); + void scale_mc_output(); }; namespace VTOL_att_control @@ -192,6 +204,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _airspeed_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -199,7 +212,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(-1), _v_rates_sp_pub(-1), - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { flag_idle_mc = true; @@ -219,12 +233,16 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); + memset(&_airspeed,0,sizeof(_airspeed)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); + _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); + _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); + _params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM"); /* fetch initial parameter values */ parameters_update(); @@ -352,6 +370,19 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll() } } +/** +* Check for airspeed updates. +*/ +void +VtolAttitudeControl::vehicle_airspeed_poll() { + bool updated; + orb_check(_airspeed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed); + } +} + /** * Check for parameter updates. */ @@ -376,12 +407,25 @@ VtolAttitudeControl::parameters_update_poll() int VtolAttitudeControl::parameters_update() { + float v; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); /* vtol motor count */ param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); + /* vtol mc mode min airspeed */ + param_get(_params_handles.mc_airspeed_min, &v); + _params.mc_airspeed_min = v; + + /* vtol mc mode max airspeed */ + param_get(_params_handles.mc_airspeed_max, &v); + _params.mc_airspeed_max = v; + + /* vtol mc mode trim airspeed */ + param_get(_params_handles.mc_airspeed_trim, &v); + _params.mc_airspeed_trim = v; + return OK; } @@ -496,6 +540,34 @@ void VtolAttitudeControl::set_idle_mc() close(fd); } +void +VtolAttitudeControl::scale_mc_output() { + // 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 = _params.mc_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 = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); + _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -516,6 +588,7 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -580,6 +653,7 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_airspeed_poll(); if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -593,6 +667,8 @@ void VtolAttitudeControl::task_main() 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); + // scale pitch control with airspeed + scale_mc_output(); fill_mc_att_control_output(); fill_mc_att_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index b9baa2046..44157acba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -3,4 +3,11 @@ // number of engines PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); // idle pwm in multicopter mode -PARAM_DEFINE_INT32(IDLE_PWM_MC,900); \ No newline at end of file +PARAM_DEFINE_INT32(IDLE_PWM_MC,900); +// min airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2); +// max airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30); +// trim airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10); + -- cgit v1.2.3 From 8af6ab27132563865a7ac0be2f7fa1105d614ce3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:17:37 +0100 Subject: Fix vehicle command docs to AMSL --- src/modules/uORB/topics/vehicle_command.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index f264accbb..6b4cb483b 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -65,7 +65,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ -- cgit v1.2.3 From 350e2549df6d3bb9ec6c9787c1e6112aa403417a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:18:25 +0100 Subject: uORB mission topic: Add AMSL as clarificaiton in docs --- src/modules/uORB/topics/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index e4b72f87c..22a8f3ecb 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ - float altitude; /**< altitude in meters */ + float altitude; /**< altitude in meters (AMSL) */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ -- cgit v1.2.3 From 33725ecc6ad383d43cfd6f533159e931052516c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:18:47 +0100 Subject: uORB home position: Add AMSL as clarificaiton in docs --- src/modules/uORB/topics/home_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 70071130d..d747b5f43 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -61,7 +61,7 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude in meters (AMSL) */ float x; /**< X coordinate in meters */ float y; /**< Y coordinate in meters */ -- cgit v1.2.3 From d0fb862a01fc14a83f8862b86e03c8033b481a5c Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 6 Dec 2014 20:13:42 +0100 Subject: fixed _loop_perf message --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e69c51142..329e2ef94 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -199,7 +199,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(-1), _v_rates_sp_pub(-1), - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")) { flag_idle_mc = true; -- cgit v1.2.3 From 1c0f850fac3f056a9c52a3258877e10c4ee0c8a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:45 +0100 Subject: 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(+) (limited to 'src/modules') diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 3728f2067..6e22a557e 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 -- cgit v1.2.3 From 4c25051edc4495f82927441501598c28a6834d1f Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Fri, 12 Dec 2014 22:36:01 +0100 Subject: changed telementry connection lost warnings to info in order to avoid audio message flooding in case of instable connections. --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b5..6d6b3a36c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1668,7 +1668,7 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - mavlink_log_critical(mavlink_fd, "data link %i regained", i); + mavlink_log_info(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; @@ -1682,7 +1682,7 @@ int commander_thread_main(int argc, char *argv[]) telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "data link %i lost", i); + mavlink_log_info(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status.data_link_lost_counter++; status_changed = true; -- cgit v1.2.3 From 59ec2401b6f8e6714d515d3d0f1cf2e0ee14b8bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 00:48:27 +0100 Subject: fw pos control: better check for control mode --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) (limited to 'src/modules') 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 e7c95cc86..1bb27168e 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 @@ -909,10 +909,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float throttle_max = 1.0f; /* AUTONOMOUS FLIGHT */ - - // XXX this should only execute if auto AND safety off (actuators active), - // else integrators should be constantly reset. - if (pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_auto_enabled && + pos_sp_triplet.current.valid) { if (!_was_pos_control_mode) { /* reset integrators */ @@ -1248,7 +1246,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (_control_mode.flag_control_velocity_enabled) { + } else if (_control_mode.flag_control_velocity_enabled && + _control_mode.flag_control_altitude_enabled) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; if (!_was_velocity_control_mode) { -- cgit v1.2.3 From 5cd2ee83421e79c9f283d46b788343a479418d8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 12:36:33 +0100 Subject: fw pos control: improve mode logic slightly --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 62 +++++++++++++--------- 1 file changed, 36 insertions(+), 26 deletions(-) (limited to 'src/modules') 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 1bb27168e..1592e70d8 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 @@ -200,9 +200,11 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; fwPosctrl::mTecs _mTecs; - bool _was_pos_control_mode; - bool _was_velocity_control_mode; - bool _was_alt_control_mode; + enum FW_POSCTRL_MODE { + FW_POSCTRL_MODE_AUTO, + FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_OTHER + } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. struct { float l1_period; @@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos_valid(false), _l1_control(), _mTecs(), - _was_pos_control_mode(false) + _control_mode_current(FW_POSCTRL_MODE_OTHER) { _nav_capabilities.turn_distance = 0.0f; @@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; - /* AUTONOMOUS FLIGHT */ if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + /* AUTONOMOUS FLIGHT */ - if (!_was_pos_control_mode) { + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - - _was_pos_control_mode = true; - _was_velocity_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ _hold_alt = _global_pos.alt; @@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (!_was_velocity_control_mode) { + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = false; } - _was_velocity_control_mode = true; - _was_pos_control_mode = false; + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + // Get demanded airspeed float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; // Get demanded vertical velocity from pitch control - float pitch = 0.0f; + static bool was_in_deadband = false; if (_manual.x > deadBand) { - pitch = (_manual.x - deadBand) / factor; - } else if (_manual.x < - deadBand) { - pitch = (_manual.x + deadBand) / factor; - } - if (pitch > 0.0f) { + float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (pitch < 0.0f) { + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (!_was_alt_control_mode) { + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = true; + was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, @@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed, TECS_MODE_NORMAL); } else { - _was_velocity_control_mode = false; - _was_pos_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_OTHER; /** MANUAL FLIGHT **/ -- cgit v1.2.3 From d0325f2b125a3573231443b7b4bac04b65081426 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:13:20 +0100 Subject: fw pos ctrl: takeoff special case only in takeoff --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 51 ++++++++++++---------- 1 file changed, 27 insertions(+), 24 deletions(-) (limited to 'src/modules') 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 1592e70d8..eec37f80c 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 @@ -1207,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { /* Tell the attitude controller to stop integrating while we are waiting @@ -1290,18 +1290,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed, - TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1320,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* Copy thrust output for publication */ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1336,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } -- cgit v1.2.3 From 6f89514beb09c5db3f9c71da22ceed7fe19e09c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:17:57 +0100 Subject: fix comment style and type --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 eec37f80c..167126a2b 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 @@ -1267,12 +1267,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _control_mode_current = FW_POSCTRL_MODE_POSITION; - // Get demanded airspeed + /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - // Get demanded vertical velocity from pitch control + /* Get demanded vertical velocity from pitch control */ static bool was_in_deadband = false; if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; @@ -1324,7 +1324,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, -- cgit v1.2.3 From ea4876da38deaecced36dc3d8e79afe85d1c0798 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:19:12 +0100 Subject: mavlink: distance sensor: fix max value --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 96aa48499..5908279d5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2162,7 +2162,7 @@ protected: msg.id = 0; msg.orientation = 0; msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.minimum_distance * 100; + msg.max_distance = range.maximum_distance * 100; msg.current_distance = range.distance * 100; msg.covariance = 20; -- cgit v1.2.3 From 51a7fbeee091c20eb52bafade3e2041e26a785cc Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 15 Dec 2014 22:12:11 +0100 Subject: added sanity checkto prevent false low airspeed readings during transition --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 126ec5832..e3a2dd72e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -557,6 +557,11 @@ VtolAttitudeControl::scale_mc_output() { airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } + // Sanity check if airspeed is consistent with throttle + if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param + airspeed = _params.mc_airspeed_trim; + } + /* * 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 -- cgit v1.2.3 From 89e2e08de0e9f76114b095782167722597f298fa Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 15 Dec 2014 22:47:10 +0100 Subject: removed white space noise --- ROMFS/px4fmu_common/init.d/rc.autostart | 2 +- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 ++-- 5 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 19fce6ea5..8388da1dd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -246,5 +246,5 @@ fi # if param compare SYS_AUTOSTART 13001 then - sh /etc/init.d/13001_caipirinha_vtol + sh /etc/init.d/13001_caipirinha_vtol fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 376a2e257..10ee5db9b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -20,7 +20,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.3 - + # # Default parameters for FW # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0fe6d25b..5748fe2b5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -550,7 +550,7 @@ then sh /etc/init.d/rc.mc_apps fi fi - + # # VTOL setup # @@ -589,7 +589,7 @@ then sh /etc/init.d/rc.vtol_apps fi fi - + # # Start the navigator # diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index f14bdb9fd..1fe4380ad 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -466,7 +466,7 @@ public: OCTA_X, OCTA_PLUS, OCTA_COX, - TWIN_ENGINE, /**< VTOL: one engine on each wing */ + TWIN_ENGINE, /**< VTOL: one engine on each wing */ MAX_GEOMETRY }; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e3a2dd72e..24800cce8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -436,7 +436,7 @@ 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[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] = _actuators_mc_in.control[2]; //roll elevon @@ -457,7 +457,7 @@ void VtolAttitudeControl::fill_fw_att_control_output() _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw + _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle } -- cgit v1.2.3 From 262b9fc7545805c7b93a15cbb80a2f67db5ecdf0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 21:13:13 +0100 Subject: fw pos ctl: make loop performance counter more meaningful --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules') 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 e7c95cc86..948b29bcb 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 @@ -1399,8 +1399,6 @@ FixedwingPositionControl::task_main() continue; } - perf_begin(_loop_perf); - /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); @@ -1419,6 +1417,7 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + perf_begin(_loop_perf); /* XXX Hack to get mavlink output going */ if (_mavlink_fd < 0) { @@ -1473,10 +1472,9 @@ FixedwingPositionControl::task_main() } } - + perf_end(_loop_perf); } - perf_end(_loop_perf); } _task_running = false; -- cgit v1.2.3 From 8fdd694f12cccfe90ed1dbec9bc7892269b3fefa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 21:13:50 +0100 Subject: fw pos ctl: compile with -Os --- src/modules/fw_pos_control_l1/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 15b575b50..440bab2c5 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -45,3 +45,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 669e1f96d8fed848d2fbe71d3a10adb749a5fcc4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 10:46:36 +0100 Subject: fixed parameter issue for VTOL so that parameter wiki tool can document them --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 2 +- .../vtol_att_control/vtol_att_control_main.cpp | 2 +- .../vtol_att_control/vtol_att_control_params.c | 93 +++++++++++++++++++--- 3 files changed, 86 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 8f7a28b97..fb31142fa 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -13,4 +13,4 @@ set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 param set VTOL_MOT_COUNT 2 -param set IDLE_PWM_MC 1080 +param set VTOL_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 24800cce8..de678a5b8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,7 +238,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); + _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 44157acba..6a234a3ba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -1,13 +1,88 @@ +/**************************************************************************** + * + * 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 vtol_att_control_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + #include -// number of engines +/** + * VTOL number of engines + * + * @min 1.0 + * @group VTOL Attitude Control + */ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); -// idle pwm in multicopter mode -PARAM_DEFINE_INT32(IDLE_PWM_MC,900); -// min airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2); -// max airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30); -// trim airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10); + +/** + * Idle speed of VTOL when in multicopter mode + * + * @min 900 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); + +/** + * Minimum airspeed in multicopter mode + * + * This is the minimum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); + +/** + * Maximum airspeed in multicopter mode + * + * This is the maximum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); + +/** + * Trim airspeed when in multicopter mode + * + * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); -- cgit v1.2.3 From f3b5b67eec258476480a225b6b835e46335003b4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 12:43:23 +0100 Subject: shortened parameter names to max 16 bytes --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 4 ++-- src/modules/vtol_att_control/vtol_att_control_main.cpp | 10 +++++----- src/modules/vtol_att_control/vtol_att_control_params.c | 10 +++++----- 3 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index fb31142fa..7e9a6d3dc 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -12,5 +12,5 @@ set MIXER FMU_caipirinha_vtol set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 -param set VTOL_MOT_COUNT 2 -param set VTOL_IDLE_PWM_MC 1080 +param set VT_MOT_COUNT 2 +param set VT_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de678a5b8..958907d1b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,11 +238,11 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); - _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); - _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); - _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); - _params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM"); + _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); + _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); + _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); + _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6a234a3ba..e21bccb0c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -46,7 +46,7 @@ * @min 1.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT,0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); /** * Minimum airspeed in multicopter mode @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); /** * Trim airspeed when in multicopter mode @@ -84,5 +84,5 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); -- cgit v1.2.3 From 2a09473e5fc73bf2f59f0d27b0b5c38b2c91812f Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:26:31 +0000 Subject: topics: move geofence status to its own topic --- src/modules/commander/commander.cpp | 47 ++++++++++++++-------- src/modules/navigator/navigator.h | 11 +++++- src/modules/navigator/navigator_main.cpp | 24 ++++++++++-- src/modules/uORB/objects_common.cpp | 5 ++- src/modules/uORB/topics/geofence_result.h | 65 +++++++++++++++++++++++++++++++ src/modules/uORB/topics/mission_result.h | 11 +++--- 6 files changed, 135 insertions(+), 28 deletions(-) create mode 100644 src/modules/uORB/topics/geofence_result.h (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c01..9262f9e81 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[]) struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to geofence result topic */ + int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result)); + struct geofence_result_s geofence_result; + memset(&geofence_result, 0, sizeof(geofence_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } - /* Check for geofence violation */ - if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - static bool flight_termination_printed = false; - - if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - flight_termination_printed = true; - } + /* start geofence result check */ + orb_check(geofence_result_sub, &updated); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + if (updated) { + orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); } + /* Check for geofence violation */ + if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + flight_termination_printed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9cd609955..a701e3f44 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include "navigator_mode.h" @@ -111,6 +112,11 @@ public: */ void publish_mission_result(); + /** + * Publish the geofence result + */ + void publish_geofence_result(); + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -134,6 +140,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct geofence_result_s* get_geofence_result() { return &_geofence_result; } struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } @@ -164,6 +171,7 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _geofence_result_pub; orb_advert_t _att_sp_pub; /**< publish att sp used only in very special failsafe modes when pos control is deactivated */ @@ -179,7 +187,8 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; - vehicle_attitude_setpoint_s _att_sp; + geofence_result_s _geofence_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index df620e5e7..0ab85d56e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _geofence_result_pub(-1), _att_sp_pub(-1), _vstatus{}, _control_mode{}, @@ -398,8 +399,8 @@ Navigator::task_main() have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ - _mission_result.geofence_violated = true; - publish_mission_result(); + _geofence_result.geofence_violated = true; + publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -408,8 +409,8 @@ Navigator::task_main() } } else { /* inform other apps via the mission result */ - _mission_result.geofence_violated = false; - publish_mission_result(); + _geofence_result.geofence_violated = false; + publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } @@ -641,6 +642,21 @@ Navigator::publish_mission_result() } } +void +Navigator::publish_geofence_result() +{ + + /* lazily publish the geofence result only once available */ + if (_geofence_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result); + + } else { + /* advertise and publish */ + _geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result); + } +} + void Navigator::publish_att_sp() { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1141431cc..3d5755a46 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" ORB_DEFINE(mission_result, struct mission_result_s); +#include "topics/geofence_result.h" +ORB_DEFINE(geofence_result, struct geofence_result_s); + #include "topics/fence.h" ORB_DEFINE(fence, unsigned); diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h new file mode 100644 index 000000000..b07e04499 --- /dev/null +++ b/src/modules/uORB/topics/geofence_result.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geofence_result.h + * Status of the plance concerning the geofence + * + * @author Ban Siesta + */ + +#ifndef TOPIC_GEOFENCE_RESULT_H +#define TOPIC_GEOFENCE_RESULT_H + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct geofence_result_s +{ + bool geofence_violated; /**< true if the geofence is violated */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(geofence_result); + +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c7d25d1f0..17ddaa01d 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,11 @@ /** * @file mission_result.h * Mission results that navigator needs to pass on to commander and mavlink. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Ban Siesta */ #ifndef TOPIC_MISSION_RESULT_H @@ -58,7 +60,6 @@ struct mission_result_s bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool geofence_violated; /**< true if the geofence is violated */ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; -- cgit v1.2.3 From 5bbd0743e082f247e3ff04e78e0e11b0583b9ff1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:27:25 +0000 Subject: navigator: deleted some verbose output --- src/modules/navigator/mission.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0765e9b7c..371480603 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -718,7 +718,6 @@ Mission::set_mission_item_reached() void Mission::set_current_offboard_mission_item() { - warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; -- cgit v1.2.3 From c9ca61ef5b23a370fcaf3e2a0546ab5452b65733 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:34:18 +0000 Subject: mavlink: don't slow mission updates down like this, otherwise we might miss mission results --- src/modules/mavlink/mavlink_mission.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a3c127cdc..34c088778 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void MavlinkMissionManager::send(const hrt_abstime now) { - /* update interval for slow rate limiter */ - _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); - bool updated = false; orb_check(_mission_result_sub, &updated); -- cgit v1.2.3 From f0ff914b626fbfb497143f80b19376efb524b9e1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:36:32 +0000 Subject: navigator: don't publish mission result immediately but only after every iteration of the navigator --- src/modules/navigator/datalinkloss.cpp | 8 ++++---- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator.h | 13 ++++++++----- src/modules/navigator/navigator_main.cpp | 6 ++++++ src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rcloss.cpp | 6 +++--- 7 files changed, 26 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index e789fd10d..87a6e023a 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item() case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; @@ -188,7 +188,7 @@ DataLinkLoss::advance_dll() _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { @@ -209,7 +209,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: @@ -217,7 +217,7 @@ DataLinkLoss::advance_dll() warnx("time is up, state should have been changed manually by now"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index cd55f60b0..e370796c0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 371480603..8de8faf9d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -711,7 +711,7 @@ Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } @@ -721,7 +721,7 @@ Mission::set_current_offboard_mission_item() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); save_offboard_mission_state(); } @@ -730,5 +730,5 @@ void Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a701e3f44..d9d911d9c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,11 +107,6 @@ public: */ void load_fence_from_file(const char *filename); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - /** * Publish the geofence result */ @@ -128,6 +123,7 @@ public: */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_mission_result_updated() { _mission_result_updated = true; } /** * Getters @@ -215,6 +211,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -280,6 +277,12 @@ private: */ void publish_position_setpoint_triplet(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ab85d56e..6a255878b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -139,6 +139,7 @@ Navigator::Navigator() : _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -491,6 +492,11 @@ Navigator::task_main() _pos_sp_triplet_updated = false; } + if (_mission_result_updated) { + publish_mission_result(); + _mission_result_updated = false; + } + perf_end(_loop_perf); } warnx("exiting."); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3807c5ea8..2f322031c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -65,7 +65,7 @@ NavigatorMode::run(bool active) { _first_run = false; /* Reset stay in failsafe flag */ _navigator->get_mission_result()->stay_in_failsafe = false; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 42392e739..a7cde6325 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -128,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -162,7 +162,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } break; @@ -171,7 +171,7 @@ RCLoss::advance_rcl() warnx("time is up, no RC regain, terminating"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: -- cgit v1.2.3 From 7cb4786e7934af50b5ceb370c593e27111d9dd17 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:10 +0000 Subject: navigator: report when a waypoint has been reached not when the whole mission is finished --- src/modules/navigator/mission.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8de8faf9d..3f0a6bb51 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #include @@ -149,18 +150,12 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + set_mission_item_reached(); if (_mission_item.autocontinue) { /* switch to next waypoint if 'autocontinue' flag set */ advance_mission(); set_mission_items(); - } else { - /* else just report that item reached */ - if (_mission_type == MISSION_TYPE_OFFBOARD) { - if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - set_mission_item_reached(); - } - } } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { @@ -395,7 +390,6 @@ Mission::set_mission_items() /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); - reset_mission_item_reached(); set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); -- cgit v1.2.3 From 180e17de335f337b17c0c411d0eb430cec760619 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:55 +0000 Subject: navigator: report using mission result if a DO_JUMP waypoint has been changed --- src/modules/mavlink/mavlink_mission.cpp | 8 +++++++- src/modules/navigator/mission.cpp | 12 ++++++++++++ src/modules/navigator/mission.h | 6 ++++++ src/modules/uORB/topics/mission_result.h | 3 +++ 4 files changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 34c088778..859d380fe 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -309,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now) send_mission_current(_current_seq); + if (mission_result.item_do_jump_changed) { + /* send a mission item again if the remaining DO_JUMPs has changed */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, + (uint16_t)mission_result.item_changed_index); + } + } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); @@ -808,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; break; default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3f0a6bb51..9b0a092da 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -630,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s "ERROR DO JUMP waypoint could not be written"); return false; } + report_do_jump_mission_changed(*mission_index_ptr, + mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -700,6 +702,16 @@ Mission::save_offboard_mission_state() dm_unlock(DM_KEY_MISSION_STATE); } +void +Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + _navigator->set_mission_result_updated(); +} + void Mission::set_mission_item_reached() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ea7cc0927..a8a644b0f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #ifndef NAVIGATOR_MISSION_H @@ -130,6 +131,11 @@ private: */ void save_offboard_mission_state(); + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + /** * Set a mission item as reached */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 17ddaa01d..2ddc529a3 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -61,6 +61,9 @@ struct mission_result_s bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ + bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ + unsigned item_changed_index; /**< indicate which item has changed */ + unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ }; /** -- cgit v1.2.3 From a4e27fd849610fe2dcd9426d0d5cb508e9563e97 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:39:25 +0000 Subject: navigator: reset some flags after the mission result has been published --- src/modules/navigator/navigator_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6a255878b..3f7670ec4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -646,6 +646,13 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } + + /* reset some of the flags */ + _mission_result.seq_reached = false; + _mission_result.seq_current = 0; + _mission_result.item_do_jump_changed = false; + _mission_result.item_changed_index = 0; + _mission_result.item_do_jump_remaining = 0; } void -- cgit v1.2.3 From a20b8d037bd91fddaebba7a61353c584dcf99314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:22:53 +0100 Subject: PX4IO firmware: Add required include. --- src/modules/px4iofirmware/protocol.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index c7e9ae3eb..b25a9507f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,6 +33,8 @@ #pragma once +#include + /** * @file protocol.h * -- cgit v1.2.3 From 752198352ebaa397caeb1c8cd18454231ffed389 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:32:43 +0100 Subject: IO firmware: Change to inttypes header --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b25a9507f..bd777428f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,7 +33,7 @@ #pragma once -#include +#include /** * @file protocol.h -- cgit v1.2.3 From 54b68b73fd919d9fa376cca5297bb6ff6ea631f5 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 21 Dec 2014 09:03:18 +0530 Subject: Remove vel reset --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules') 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 296919c04..0af01cba1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1052,10 +1052,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } - /* reset xy velocity estimates when landed */ - x_est[1] = 0.0f; - y_est[1] = 0.0f; - } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { -- cgit v1.2.3 From b1f462a26638f252b0849083f1c3a81c52d49053 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:04 +0000 Subject: geofence: don't fall over lines containing just a LF --- src/modules/navigator/geofence.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 0f431ded2..406137169 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename) while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; /* if the line starts with #, skip */ - if (line[textStart] == commentChar) + if (line[textStart] == commentChar) { continue; + } + + /* if there is only a linefeed, skip it */ + if (line[0] == '\n') { + continue; + } if (gotVertical) { /* Parse the line as a geofence point */ -- cgit v1.2.3 From 82383533c169e44ac769ce77f9e8ddfbd3082ed9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:30 +0000 Subject: geofence: be more verbose if import fails --- src/modules/navigator/geofence.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 406137169..4482fb36b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -297,8 +297,10 @@ Geofence::loadFromFile(const char *filename) /* Handle degree minute second format */ float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; - if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { + warnx("Scanf to parse DMS geofence vertex failed."); return ERROR; + } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); @@ -307,9 +309,10 @@ Geofence::loadFromFile(const char *filename) } else { /* Handle decimal degree format */ - - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { + warnx("Scanf to parse geofence vertex failed."); return ERROR; + } } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) -- cgit v1.2.3 From d511e39ea7a3f1e0cb672b14598e18a7df18156a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Dec 2014 17:09:43 -0500 Subject: turn on -Werror and fix resulting errors --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + src/drivers/ardrone_interface/ardrone_interface.c | 2 +- src/drivers/boards/aerocore/aerocore_led.c | 3 ++- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/px4flow/module.mk | 2 ++ src/drivers/px4flow/px4flow.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/drivers/trone/trone.cpp | 2 +- src/examples/fixedwing_control/main.c | 2 +- src/examples/fixedwing_control/module.mk | 2 ++ .../flow_position_estimator/flow_position_estimator_main.c | 2 +- src/examples/flow_position_estimator/module.mk | 2 ++ src/examples/hwtest/hwtest.c | 12 +++++++----- src/examples/matlab_csv_serial/matlab_csv_serial.c | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 9 ++++++--- src/lib/conversion/rotation.cpp | 5 +++++ src/lib/rc/st24.c | 2 +- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 5 +++-- src/modules/attitude_estimator_ekf/module.mk | 5 +++++ .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 2 +- src/modules/attitude_estimator_so3/module.mk | 2 ++ src/modules/bottle_drop/bottle_drop.cpp | 7 +++++++ src/modules/commander/commander.cpp | 2 +- src/modules/commander/module.mk | 2 ++ src/modules/ekf_att_pos_estimator/module.mk | 2 +- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 2 ++ src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 3 +++ src/modules/navigator/module.mk | 2 ++ src/modules/position_estimator_inav/module.mk | 2 ++ .../position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/sdlog2/module.mk | 2 ++ src/modules/sdlog2/sdlog2.c | 4 +++- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/module.mk | 2 ++ src/modules/systemlib/module.mk | 2 ++ src/modules/systemlib/systemlib.c | 2 +- src/modules/systemlib/systemlib.h | 2 +- src/modules/vtol_att_control/module.mk | 2 ++ src/systemcmds/mixer/module.mk | 2 ++ src/systemcmds/mtd/module.mk | 2 ++ src/systemcmds/param/param.c | 2 +- src/systemcmds/tests/module.mk | 2 ++ 48 files changed, 95 insertions(+), 35 deletions(-) (limited to 'src/modules') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a..7119c723b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # ARCHWARNINGS = -Wall \ -Wextra \ + -Werror \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 9d2c1441d..7f1b21a95 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 15, 1100, ardrone_interface_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index e40d1730c..94a716029 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -45,6 +45,7 @@ #include "board_config.h" #include +#include /* * Ideally we'd be able to get these from up_internal.h, @@ -54,7 +55,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index bccdf1190..5035600ef 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, frsky_telemetry_thread_main, - (const char **)argv); + (char * const *)argv); while (!thread_running) { usleep(200); diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 8ab9d8d55..4b8e0c0b0 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index edbb14172..17a24d104 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index 460bec7b9..ecd3e804a 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-attributes diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 09ec4bf96..62308fc65 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -106,7 +106,7 @@ struct i2c_frame { }; struct i2c_frame f; -typedef struct i2c_integral_frame { +struct i2c_integral_frame { uint16_t frame_count_since_last_readout; int16_t pixel_flow_x_integral; int16_t pixel_flow_y_integral; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 44ed03254..83086fd7c 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 2048, roboclaw_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } else if (!strcmp(argv[1], "test")) { diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83b5c987e..cf3546669 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -206,7 +206,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -uint8_t crc8(uint8_t *p, uint8_t len){ +static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6a5cbcd30..fcbb54c8e 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -423,7 +423,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, fixedwing_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index a2a9eb113..3fab9c8d4 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -41,3 +41,5 @@ SRCS = main.c \ params.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error 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 0b8c01f79..a89ccf933 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -114,7 +114,7 @@ int flow_position_estimator_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 4000, flow_position_estimator_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk index 88c9ceb93..5c6e29f8f 100644 --- a/src/examples/flow_position_estimator/module.mk +++ b/src/examples/flow_position_estimator/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = flow_position_estimator SRCS = flow_position_estimator_main.c \ flow_position_estimator_params.c + +EXTRACFLAGS = -Wno-float-equal diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index d3b10f46e..95ff346bb 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -39,13 +39,15 @@ * @author Lorenz Meier */ -#include #include -#include +#include + #include -#include -#include +#include +#include #include +#include +#include __EXPORT int ex_hwtest_main(int argc, char *argv[]); @@ -53,7 +55,7 @@ int ex_hwtest_main(int argc, char *argv[]) { warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); warnx("(run to do so)"); - warnx("usage: http://px4.io/dev/examples/write_output"); + warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index c66bebeec..a95f45d1a 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -107,7 +107,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, matlab_csv_serial_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 3eaf14148..45d541137 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -38,10 +38,13 @@ * @author Example User */ +#include +#include +#include +#include + #include #include -#include -#include #include #include @@ -100,7 +103,7 @@ int px4_daemon_app_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, px4_daemon_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index e17715733..3d3e13355 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -181,6 +181,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) x = tmp; return; } + case ROTATION_ROLL_270_YAW_270: { + // unimplemented + ASSERT(0 == 1); + return; + } case ROTATION_PITCH_90: { tmp = z; z = -x; x = tmp; return; diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index e8a791b8f..5c53a1602 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -72,7 +72,7 @@ const char *decode_states[] = {"UNSYNCED", #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 uint8_t _rxlen; static ReceiverFcPacket _rxpacket; 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 6068ab082..a9205b6fd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -134,7 +134,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 7200, attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -275,7 +275,8 @@ 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 = { 0 }; + struct attitude_estimator_ekf_params ekf_params; + memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 749b0a91c..c4bf34229 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -42,3 +42,8 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-float-equal -Wno-error + +EXTRACXXFLAGS = -Wno-error + diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e49027e47..9414225ca 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 14000, attitude_estimator_so3_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk index f52715abb..7b2e206cc 100644 --- a/src/modules/attitude_estimator_so3/module.mk +++ b/src/modules/attitude_estimator_so3/module.mk @@ -8,3 +8,5 @@ SRCS = attitude_estimator_so3_main.cpp \ attitude_estimator_so3_params.c MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e0bcbc6e9..4580b338d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -523,6 +523,9 @@ BottleDrop::task_main() } switch (_drop_state) { + case DROP_STATE_INIT: + // do nothing + break; case DROP_STATE_TARGET_VALID: { @@ -689,6 +692,10 @@ BottleDrop::task_main() orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; + + case DROP_STATE_BAY_CLOSED: + // do nothing + break; } counter++; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c01..325ab2a67 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -267,7 +267,7 @@ int commander_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 40, 3200, commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); while (!thread_running) { usleep(200); diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 27ca5c182..caec1e869 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -51,3 +51,5 @@ SRCS = commander.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 36d854ddd..07e70ddb3 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,4 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-error diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index f4ea05088..71b387b1e 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, control_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 440bab2c5..98e5c0a1e 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -47,3 +47,5 @@ SRCS = fw_pos_control_l1_main.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f17497aa8..4ba595a87 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req) /// @brief Copy file (with limited space) int -MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) { char buff[512]; int src_fd = -1, dst_fd = -1; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bef6775a9..9693a92a9 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -142,7 +142,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); - int _copy_file(const char *src_path, const char *dst_path, ssize_t length); + int _copy_file(const char *src_path, const char *dst_path, size_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b7..08a9ac76b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1638,7 +1638,7 @@ Mavlink::start(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2800, (main_t)&Mavlink::start_helper, - (const char **)argv); + (char * const *)argv); // Ensure that this shell command // does not return before the instance diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 378e3427d..2203ea1e7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1764,6 +1764,9 @@ protected: case RC_INPUT_SOURCE_PX4IO_ST24: msg.rssi |= (3 << 4); break; + case RC_INPUT_SOURCE_UNKNOWN: + // do nothing + break; } if (rc.rc_lost) { diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c44d4c35e..0d7d6b9ef 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -62,3 +62,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-sign-compare diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 0658d3f09..5ebdb721c 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -41,3 +41,5 @@ SRCS = position_estimator_inav_main.c \ inertial_filter.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error 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 0af01cba1..43829ede5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[]) position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL); + (argv) ? (char * const *) &argv[2] : (char * const *) NULL); exit(0); } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d4a00af39..d7224b77a 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -45,3 +45,5 @@ SRCS = sdlog2.c \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6df677338..0a8564da6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT - 30, 3000, sdlog2_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } @@ -1089,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (_extended_logging) { subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } else { + subs.sat_info_sub = 0; } /* close non-needed fd's */ diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 061fbf9b9..ee492f85a 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, segway_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index dfbba92d9..f37bc9327 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -44,3 +44,5 @@ SRCS = sensors.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-type-limits diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fe8b7e75a..f4dff2838 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -57,3 +57,5 @@ SRCS = err.c \ mcu_version.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 90d8dd77c..82183b0d7 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 6e22a557e..2f24215a9 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -64,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name, int scheduler, int stack_size, main_t entry, - const char *argv[]); + char * const argv[]); enum MULT_PORTS { MULT_0_US2_RXTX = 0, diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0d5155e90..02403ef76 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index cdbff75f0..40ffb8f12 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -41,3 +41,5 @@ SRCS = mixer.cpp MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index 1bc4f414e..c3baffc5d 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error \ No newline at end of file diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index e110335e7..80ee204e8 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -212,7 +212,7 @@ static void do_show(const char* search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, search_string, false); + param_foreach(do_show_print, (char *)search_string, false); exit(0); } diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 622a0faf3..1ea2a6620 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -34,3 +34,5 @@ SRCS = test_adc.c \ test_conv.cpp \ test_mount.c \ test_mtd.c + +EXTRACXXFLAGS = -Wno-error -- cgit v1.2.3 From 84d744707d07ae60dcf201a828d529057061fe47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 14:34:53 +0100 Subject: UAVCAN: Move into lib directory --- .gitmodules | 2 +- Tools/px4params/srcparser.py | 2 +- Tools/px4params/srcscanner.py | 7 ++++++- src/lib/uavcan | 1 + src/modules/uavcan/module.mk | 4 ++-- uavcan | 1 - 6 files changed, 11 insertions(+), 6 deletions(-) create mode 160000 src/lib/uavcan delete mode 160000 uavcan (limited to 'src/modules') diff --git a/.gitmodules b/.gitmodules index 4b84afac2..17a098431 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,5 +5,5 @@ path = NuttX url = git://github.com/PX4/NuttX.git [submodule "uavcan"] - path = uavcan + path = src/lib/uavcan url = git://github.com/pavel-kirienko/uavcan.git diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0a4d21d26..8e6092195 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -103,7 +103,7 @@ class SourceParser(object): Returns list of supported file extensions that can be parsed by this parser. """ - return ["cpp", "c"] + return [".cpp", ".c"] def Parse(self, contents): """ diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index d7eca72d7..1f0ea4e89 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -26,5 +26,10 @@ class SourceScanner(object): parser.Parse method. """ with codecs.open(path, 'r', 'utf-8') as f: - contents = f.read() + try: + contents = f.read() + except: + contents = '' + print('Failed reading file: %s, skipping content.' % path) + pass parser.Parse(contents) diff --git a/src/lib/uavcan b/src/lib/uavcan new file mode 160000 index 000000000..1efd24427 --- /dev/null +++ b/src/lib/uavcan @@ -0,0 +1 @@ +Subproject commit 1efd24427539fa332a15151143466ec760fa5fff diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index f92bc754f..e5d30f6c4 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(UAVCAN_DIR)/libuavcan/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 diff --git a/uavcan b/uavcan deleted file mode 160000 index 1efd24427..000000000 --- a/uavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 1efd24427539fa332a15151143466ec760fa5fff -- cgit v1.2.3 From 0aaabaee0974aaca354e95f403cca3317fd1a83e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 15:02:01 +0100 Subject: VTOL: Remove unneeded headers --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 958907d1b..9a80562f3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -74,11 +74,8 @@ #include #include "drivers/drv_pwm_output.h" -#include #include -#include - #include -- cgit v1.2.3 From d14fdf896b0081403908599a3164d09941d363c9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 10:56:32 -0500 Subject: rotate_3f() implement missing ROTATION_ROLL_270_YAW_270 --- src/examples/fixedwing_control/module.mk | 2 +- src/lib/conversion/rotation.cpp | 4 ++-- src/modules/attitude_estimator_ekf/module.mk | 5 ++--- 3 files changed, 5 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 3fab9c8d4..8707776ae 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wno-frame-larger-than diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 3d3e13355..1fe36d395 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -182,8 +182,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) return; } case ROTATION_ROLL_270_YAW_270: { - // unimplemented - ASSERT(0 == 1); + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; return; } case ROTATION_PITCH_90: { diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index c4bf34229..96ba45242 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,7 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wno-error - -EXTRACXXFLAGS = -Wno-error +EXTRACFLAGS = -Wno-float-equal +EXTRACXXFLAGS = -Wno-frame-larger-than -- cgit v1.2.3 From 5b600a815c13af56e879029196a24e544c110b97 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 11:15:45 -0500 Subject: Replace use of -Wno-error and only ignore specific warnings --- src/examples/fixedwing_control/module.mk | 2 +- src/modules/attitude_estimator_ekf/module.mk | 4 ++-- src/modules/commander/module.mk | 3 ++- src/modules/ekf_att_pos_estimator/module.mk | 3 ++- src/modules/position_estimator_inav/module.mk | 3 ++- src/modules/sdlog2/module.mk | 3 ++- src/modules/vtol_att_control/module.mk | 3 ++- src/systemcmds/mixer/module.mk | 3 ++- src/systemcmds/mtd/module.mk | 3 ++- src/systemcmds/tests/module.mk | 3 ++- 10 files changed, 19 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 8707776ae..f6c882ead 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-frame-larger-than +EXTRACFLAGS = -Wframe-larger-than=1200 diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 96ba45242..3a3e1cc69 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 -EXTRACXXFLAGS = -Wno-frame-larger-than +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index caec1e869..dac2ce59a 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,4 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=1900 + diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 07e70ddb3..4311fb3f9 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wno-error +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000 + diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 5ebdb721c..45c876299 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,4 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=3500 + diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d7224b77a..f1118000e 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,4 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=1200 + diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 02403ef76..0cf3072c8 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -40,4 +40,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wno-write-strings + diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index 40ffb8f12..0fb899c67 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -42,4 +42,5 @@ MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2048 + diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index c3baffc5d..bca1cdcc1 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -7,4 +7,5 @@ SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error \ No newline at end of file +EXTRACFLAGS = -Wno-error + diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1ea2a6620..6eed3922c 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,4 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_mtd.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2500 + -- cgit v1.2.3 From 9292c8f405b0ed208443df0b1f9ebd497bb518ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 20 Dec 2014 10:27:02 -0700 Subject: add interrupt latency printout command and mean/variance to interval performance counter --- src/drivers/stm32/drv_hrt.c | 8 ++++-- src/modules/systemlib/perf_counter.c | 53 ++++++++++++++++++++++++++++-------- src/modules/systemlib/perf_counter.h | 9 +++++- src/systemcmds/perf/perf.c | 6 +++- 4 files changed, 60 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 281f918d7..603c2eb9d 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -253,9 +253,11 @@ static uint16_t latency_baseline; static uint16_t latency_actual; /* latency histogram */ -#define LATENCY_BUCKET_COUNT 8 -static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /* timer-specific functions */ static void hrt_tim_init(void); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index d6d8284d2..f9e90652d 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -41,7 +41,7 @@ #include #include #include - +#include #include "perf_counter.h" /** @@ -84,7 +84,8 @@ struct perf_ctr_interval { uint64_t time_last; uint64_t time_least; uint64_t time_most; - + float mean; + float M2; }; /** @@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name) case PC_INTERVAL: ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; default: @@ -156,15 +158,23 @@ perf_count(perf_counter_t handle) case 1: pci->time_least = now - pci->time_last; pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; break; default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - break; - } + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } pci->time_last = now; pci->event_count++; @@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + float rms = sqrtf(pci->M2 / (pci->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, pci->time_least, - pci->time_most); + pci->time_most, + (double)(1000 * pci->mean), + (double)(1000 * rms)); break; } @@ -365,6 +378,21 @@ perf_print_all(int fd) } } +extern const uint16_t latency_bucket_count; +extern uint32_t latency_counters[]; +extern const uint16_t latency_buckets[]; + +void +perf_print_latency(int fd) +{ + dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { + printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]); + } + // print the overflow bucket value + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); +} + void perf_reset_all(void) { @@ -374,4 +402,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { + latency_counters[i] = 0; + } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 668d9dfdf..d06606a5d 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -94,7 +94,7 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. - * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * If a call is made without a corresponding perf_begin call, or if perf_cancel * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. @@ -142,6 +142,13 @@ __EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); */ __EXPORT extern void perf_print_all(int fd); +/** + * Print hrt latency counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +__EXPORT extern void perf_print_latency(int fd); + /** * Reset all of the performance counters. */ diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b08a2e3b7..a788dfc11 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { + perf_print_latency(0 /* stdout */); + fflush(stdout); + return 0; } - printf("Usage: perf \n"); + printf("Usage: perf [reset | latency]\n"); return -1; } -- cgit v1.2.3