void quadrotorDrag(double[] uin, DragParameters parameter, double dt, ref double[] y)
        {
            int    i;
            double absoluteVelocity;
            double absoluteAngularVelocity;

            /*  initialize vectors */
            for (i = 0; i < 6; i++)
            {
                y[i] = 0.0;
            }

            /*  Input variables */
            /*  Constants */
            /*  temporarily used vector */
            absoluteVelocity = Math.Sqrt((MatlabHelpers.rt_powd_snf(uin[0], 2.0) + MatlabHelpers.rt_powd_snf(uin[1], 2.0))
                                         + MatlabHelpers.rt_powd_snf(uin[2], 2.0));
            absoluteAngularVelocity = Math.Sqrt((MatlabHelpers.rt_powd_snf(uin[3], 2.0) + MatlabHelpers.rt_powd_snf(uin[4],
                                                                                                                    2.0)) + MatlabHelpers.rt_powd_snf(uin[5], 2.0));

            /*  system outputs */
            /*  calculate drag force */
            y[0] = parameter.C_wxy * absoluteVelocity * uin[0];
            y[1] = parameter.C_wxy * absoluteVelocity * uin[1];
            y[2] = parameter.C_wz * absoluteVelocity * uin[2];

            /*  calculate draq torque */
            y[3] = parameter.C_mxy * absoluteAngularVelocity * uin[3];
            y[4] = parameter.C_mxy * absoluteAngularVelocity * uin[4];
            y[5] = parameter.C_mz * absoluteAngularVelocity * uin[5];
        }
Exemple #2
0
        void quadrotorPropulsion(double[] xin, double[] uin, PropulsionParameters parameter, double dt, double[] y, double[] xpred)
        {
            double[] v_1 = new double[4];
            int      i;

            double[] F_m = new double[4];
            double[] U   = new double[4];
            double[] M_e = new double[4];
            double[] I   = new double[4];
            double[] F   = new double[3];
            double   b_F_m;
            double   temp;
            double   d0;

            /*  initialize vectors */
            for (i = 0; i < 4; i++)
            {
                xpred[i] = xin[i];

                /*  motorspeed */
                v_1[i] = 0.0;
            }

            y = new double[14];
            y.Initialize();
//			memset(&y[0], 0, 14U * sizeof(double));
            for (i = 0; i < 4; i++)
            {
                F_m[i] = 0.0;
                U[i]   = 0.0;
                M_e[i] = 0.0;
                I[i]   = 0.0;
            }

            for (i = 0; i < 3; i++)
            {
                F[i] = 0.0;
            }

            /*  Input variables */
            U[0] = uin[6];
            U[1] = uin[7];
            U[2] = uin[8];
            U[3] = uin[9];

            /*  Constants */
            v_1[0] = -uin[2] + parameter.l_m * uin[4];
            v_1[1] = -uin[2] - parameter.l_m * uin[3];
            v_1[2] = -uin[2] - parameter.l_m * uin[4];
            v_1[3] = -uin[2] + parameter.l_m * uin[3];

            /*  calculate thrust for all 4 rotors */
            for (i = 0; i < 4; i++)
            {
                /*  if the flow speed at infinity is negative */
                if (v_1 [i] < 0.0)
                {
                    b_F_m = (parameter.CT2s * MatlabHelpers.rt_powd_snf(v_1 [i], 2.0) + parameter.CT1s *
                             v_1 [i] * xin [i]) + parameter.CT0s * MatlabHelpers.rt_powd_snf(xin [i], 2.0);

                    /*  if the flow speed at infinity is positive */
                }
                else
                {
                    b_F_m = (-parameter.CT2s * MatlabHelpers.rt_powd_snf(v_1 [i], 2.0) + parameter.CT1s *
                             v_1 [i] * xin [i]) + parameter.CT0s * MatlabHelpers.rt_powd_snf(xin [i], 2.0);
                }

                /*  sum up all rotor forces */
                /*  Identification of Roxxy2827-34 motor with 10x4.5 propeller */
                /*  temporarily used Expressions */
                temp = (U [i] * parameter.beta_m - parameter.Psi * xin [i]) / (2.0 *
                                                                               parameter.R_A);
                temp += Math.Sqrt(MatlabHelpers.rt_powd_snf(temp, 2.0) + U [i] * parameter.alpha_m /
                                  parameter.R_A);
                d0 = parameter.Psi * temp;

                /*  electrical torque motor 1-4 */
                /*  new version */
                /*  old version */
                /*  fx   = (Psi/R_A*(U-Psi*omega_m) - M_m)/J_M; */
                /*  A    = -(Psi^2/R_A)/J_M; */
                /*  B(1) =  Psi/(J_M*R_A); */
                /*  B(2) = -1/J_M; */
                /*  system outputs. Use euler solver to predict next time step */
                /*  predicted motor speed */
                /*  electric torque */
                /* y = [M_e I]; */
                /*  system jacobian */
                /*  A       = 1 + dt*A; */
                /*  input jacobian */
                /*  B       = A*B*dt; */
                M_e [i]   = d0;
                I [i]     = temp;
                xpred [i] = xin [i] + dt * (1.0 / parameter.J_M * (d0 - (parameter.k_t * b_F_m
                                                                         + parameter.k_m * xin [i])));
                F_m [i] = b_F_m;
                F [2]  += b_F_m;
            }

            /*  System output, i.e. force and torque of quadrotor */
            y[0] = 0.0;
            y[1] = 0.0;
            y[2] = F[2];

            /*  torque for rotating quadrocopter around x-axis is the mechanical torque */
            y[3] = (F_m[3] - F_m[1]) * parameter.l_m;

            /*  torque for rotating quadrocopter around y-axis is the mechanical torque */
            y[4] = (F_m[0] - F_m[2]) * parameter.l_m;

            /*  torque for rotating quadrocopter around z-axis is the electrical torque */
            y[5] = ((-M_e[0] - M_e[2]) + M_e[1]) + M_e[3];

            /*  motor speeds (rad/s) */
            for (i = 0; i < 4; i++)
            {
                y[i + 6] = xpred[i];
            }

            /*  motor current (A) */
            for (i = 0; i < 4; i++)
            {
                y[i + 10] = I[i];
            }

            /*  M_e(1:4) / Psi; */
        }