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];
}
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; */
}
