public ResultsDron RefreshState(ParamsDron paramsDron)
{
connection.SendMessage(paramsDron);
simulator.Simulate(paramsDron);
return(simulator.GetOutput());
}
public string SendMessage(ParamsDron paramsDron)
{
Socket client = StartClient();
// Generamos la data
string data = $"alp={paramsDron.Alp.ToString()};bet={paramsDron.Bet.ToString()}";
byte[] byteData = Encoding.ASCII.GetBytes(data);
// Enviamos la data
client.BeginSend(byteData, 0, byteData.Length, 0,
new AsyncCallback(SendCallback), client);
sendDone.WaitOne();
// Recibimos la data
try
{
// Creamos el object stateObject
StateObject state = new StateObject();
state.workSocket = client;
// Begin receiving the data from the remote device.
client.BeginReceive(state.buffer, 0, StateObject.BufferSize, 0,
new AsyncCallback(ReceiveCallback), state);
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
CloseClient(client);
return(response);
}
public void Simulate(ParamsDron paramsDron)
{
double u_alp = paramsDron.Alp;
double u_bet = paramsDron.Bet;
time[1] = time[0];
time[0] = DateTime.Now.Millisecond;
double dt = (time[0] > time[1]) ? 0.001 * (time[0] - time[1]) : 1 + (0.001 * (time[0] - time[1]));
double CL = CL0;
double CD = 0;
double CY = 0;
double Cm = 0;
double Cn = 0;
double Cl = 0;
double e = e0;
// Ley de control
vectorControl[0] = u_alp;
vectorControl[1] = u_bet;
// calculo de los coeficientes -
for (int i = 1; i < 9; i++)
{
CL = CL + vectorEstado[i] * derivadasEstabilidad[i, 0];
CY = CY + vectorEstado[i] * derivadasEstabilidad[i, 1];
Cl = Cl + vectorEstado[i] * derivadasEstabilidad[i, 2];
Cm = Cm + vectorEstado[i] * derivadasEstabilidad[i, 3];
Cn = Cn + vectorEstado[i] * derivadasEstabilidad[i, 4];
}
for (int i = 0; i < 2; i++)
{
CL = CL + vectorControl[i] * derivadasControl[i, 0];
CY = CY + vectorControl[i] * derivadasControl[i, 1];
Cl = Cl + vectorControl[i] * derivadasControl[i, 2];
Cm = Cm + vectorControl[i] * derivadasControl[i, 3];
Cn = Cn + vectorControl[i] * derivadasControl[i, 4];
e = e + vectorControl[i] * derivadasControl[i, 5];
}
CD = CD0 + CL * CL / (3.141592 * e * AR);
// Mecanica de vuelo
double q_dyn = 0.5 * rho * S * vectorEstado[0] * vectorEstado[0];
double L = q_dyn * CL;
double D = q_dyn * CD;
double Y = q_dyn * CY;
double l = q_dyn * b * Cl;
double m = q_dyn * c * Cm;
double n = q_dyn * b * Cn;
// Fuerzas
double X = -Math.Cos(vectorEstado[(int)nombreVariables.alp]) / Math.Cos(vectorEstado[(int)nombreVariables.bet]) * D - Math.Cos(vectorEstado[(int)nombreVariables.alp]) * Math.Tan(vectorEstado[(int)nombreVariables.bet]) * Y + Math.Sin(vectorEstado[(int)nombreVariables.alp]) * L;
double Z = -Math.Sin(vectorEstado[(int)nombreVariables.alp]) / Math.Cos(vectorEstado[(int)nombreVariables.bet]) * D - Math.Sin(vectorEstado[(int)nombreVariables.alp]) * Math.Tan(vectorEstado[(int)nombreVariables.bet]) * Y - Math.Cos(vectorEstado[(int)nombreVariables.alp]) * L;
// Derivadas
double u = Math.Cos(vectorEstado[(int)nombreVariables.alp]) * Math.Cos(vectorEstado[(int)nombreVariables.bet]) * vectorEstado[(int)nombreVariables.v];
double v = Math.Cos(vectorEstado[(int)nombreVariables.alp]) * Math.Sin(vectorEstado[(int)nombreVariables.v]);
double w = Math.Sin(vectorEstado[(int)nombreVariables.alp]) * vectorEstado[(int)nombreVariables.v];
double roll = vectorEstado[(int)nombreVariables.phi];
double pitch = vectorEstado[(int)nombreVariables.the];
double yaw = vectorEstado[(int)nombreVariables.psi];
double p = vectorEstado[(int)nombreVariables.p];
double q = vectorEstado[(int)nombreVariables.q];
double r = vectorEstado[(int)nombreVariables.r];
double du = (X - g * mass * Math.Sin(pitch) - q * w + r * v) / mass;
double dv = (Y - g * mass * Math.Cos(pitch) * Math.Sin(roll) - r * u + p * w) / mass;
double dw = (Z - g * mass * Math.Cos(pitch) * Math.Cos(roll) - p * v + q * u) / mass;
double dp = Izz / (Ixx * Izz - Ixz * Ixz) * l + Ixz / (Ixx * Izz - Ixz * Ixz) * n + (Ixz * (Ixx - Iyy + Izz) / (Ixx * Izz - Ixz * Ixz)) * p * q + ((Izz * (Iyy - Izz) - Ixz * Ixz) / (Ixx * Izz - Ixz * Ixz)) * r * q;
double dq = m / Iyy;//+ (Izz - Ixx) / Iyy * p * r + Ixz / Iyy * (r*r - p*p);
double dr = Ixx / (Ixx * Izz - Ixz * Ixz) * n + Ixz / (Ixx * Izz - Ixz * Ixz) * l + ((Ixx * (Ixx - Iyy) + Ixz * Ixz) / (Ixx * Izz - Ixz * Ixz)) * p * q + (Ixz * (Iyy - Ixx - Izz) / (Ixx * Izz - Ixz * Ixz)) * r * q;
double droll = p + (q * Math.Sin(roll) + r * Math.Cos(roll)) * Math.Tan(pitch);
double dpitch = q / Math.Cos(roll); // - r * Math.Sin(roll);
double dyaw = (q * Math.Sin(roll) + r * Math.Cos(roll)) / Math.Cos(pitch);
/*double dx = u * Math.Cos(yaw) * Math.Cos(pitch) + v * (Math.Cos(yaw) * Math.Sin(pitch) * Math.Sin(roll) - Math.Cos(roll) * Math.Sin(yaw)) + w * (Math.Sin(pitch) * Math.Cos(roll) * Math.Cos(yaw) + Math.Sin(roll) * Math.Sin(yaw));
* double dy = u * Math.Sin(yaw) * Math.Cos(pitch) + v * (Math.Sin(yaw) * Math.Sin(pitch) * Math.Sin(roll) + Math.Cos(roll) * Math.Cos(yaw)) + w * (Math.Sin(pitch) * Math.Cos(roll) * Math.Sin(yaw) - Math.Sin(roll) * Math.Cos(yaw));
* double dz = -u * Math.Sin(pitch) + v * Math.Cos(pitch) * Math.Sin(roll) + w * Math.Cos(pitch) * Math.Cos(roll);*/
// Actualizamos
vectorEstado[(int)nombreVariables.v] = vectorEstado[(int)nombreVariables.v] + dt * Math.Sqrt(du * du + dv * dv + dw * dw);
vectorEstado[(int)nombreVariables.p] = vectorEstado[(int)nombreVariables.p] + dt * dp;
vectorEstado[(int)nombreVariables.q] = vectorEstado[(int)nombreVariables.q] + dt * dq;
vectorEstado[(int)nombreVariables.r] = vectorEstado[(int)nombreVariables.r] + dt * dr;
vectorEstado[(int)nombreVariables.phi] = vectorEstado[(int)nombreVariables.phi] + dt * droll;
vectorEstado[(int)nombreVariables.psi] = vectorEstado[(int)nombreVariables.psi] + dt * dyaw;
vectorEstado[(int)nombreVariables.the] = vectorEstado[(int)nombreVariables.the] + dt * dpitch;
vectorEstado[(int)nombreVariables.alp] = vectorEstado[(int)nombreVariables.alp] - dt * Math.Atan2(dw, Math.Sqrt(du * du + dv * dv));
vectorEstado[(int)nombreVariables.bet] = vectorEstado[(int)nombreVariables.bet] + dt * Math.Atan2(dv, du);
}
public ResultsDron Post([FromBody] ParamsDron paramDron)
{
return(DronRepository.Shared.RefreshState(paramDron));
}
