public MultiCopter(string frame = "quadx")
{
self = this;
motors = Motor.build_motors(MAVLink.MAV_TYPE.QUADROTOR, (int)MissionPlanner.GCSViews.ConfigurationView.ConfigFrameType.Frame.Plus);
motor_speed = new double[motors.Length];
mass = 1.5;// # Kg
frame_height = 0.1;
hover_throttle = 0.51;
terminal_velocity = 15.0;
terminal_rotation_rate = 4 * (360.0 * deg2rad);
thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
last_time = DateTime.Now;
}
public MultiCopter(string frame = "quad")
{
self = this;
motors = Motor.build_motors(frame);
motor_speed = new double[motors.Length];
mass = 1.5;// # Kg
frame_height = 0.1;
hover_throttle = 0.45;
terminal_velocity = 15.0;
terminal_rotation_rate = 4 * (360.0 * deg2rad);
thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
last_time = DateTime.Now;
}
private void ConnectComPort_Click(object sender, EventArgs e)
{
if (threadrun == 0)
{
OutputLog.Clear();
if (MainV2.comPort.BaseStream.IsOpen == false)
{
// CustomMessageBox.Show("Please connect first");
// return;
}
try
{
quad = new HIL.MultiCopter();
if (RAD_JSBSim.Checked)
{
simPort = 5124;
recvPort = 5123;
}
SetupUDPRecv();
if (chkSITL.Checked)
{
SITLSEND = new UdpClient(SITLIP, 5501);
OutputLog.AppendText("SITL to " + SITLIP + " port " + 5501 + " \n");
}
if (RAD_softXplanes.Checked)
{
SetupUDPXplanes();
SetupUDPMavLink();
}
else
{
if (RAD_JSBSim.Checked)
{
System.Diagnostics.ProcessStartInfo _procstartinfo = new System.Diagnostics.ProcessStartInfo();
_procstartinfo.WorkingDirectory = Path.GetDirectoryName(Application.ExecutablePath);
_procstartinfo.Arguments = "--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=jsbsim/fgout.xml --script=jsbsim/rascal_test.xml";
_procstartinfo.FileName = "JSBSim.exe";
// Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar +
_procstartinfo.UseShellExecute = true;
//_procstartinfo.RedirectStandardOutput = true;
System.Diagnostics.Process.Start(_procstartinfo);
System.Threading.Thread.Sleep(2000);
SetupTcpJSBSim();
}
SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw
}
OutputLog.AppendText("Sim Link Started\n");
}
catch (Exception ex) { OutputLog.AppendText("Socket setup problem. Do you have this open already? " + ex.ToString()); }
// set to highest to try prevent any timer issues
System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
{
Name = "Main simu Serial/UDP listener",
IsBackground = true,
Priority = System.Threading.ThreadPriority.Lowest
};
t11.Start();
timer_servo_graph.Start();
}
else
{
timer_servo_graph.Stop();
threadrun = 0;
if (JSBSimSEND != null && JSBSimSEND.Connected)
{
try
{
JSBSimSEND.Client.Send(ASCIIEncoding.ASCII.GetBytes("\n\nexit\n"));
}
catch { }
}
if (SimulatorRECV != null)
SimulatorRECV.Close();
if (SimulatorRECV != null && SimulatorRECV.Connected)
SimulatorRECV.Disconnect(true);
if (MavLink != null)
MavLink.Close();
position.Clear();
if (XplanesSEND != null)
XplanesSEND.Close();
// if (comPort.BaseStream.IsOpen)
// comPort.stopall(true);
OutputLog.AppendText("Sim Link Stopped\n");
System.Threading.Thread.Sleep(1000);
Application.DoEvents();
}
}
public MultiCopter(string frame = "quadx")
{
self = this;
motors = Motor.build_motors(frame);
motor_speed = new double[motors.Length];
mass = 1.5;// # Kg
frame_height = 0.1;
hover_throttle = 0.45;
terminal_velocity = 15.0;
terminal_rotation_rate = 4 * (360.0 * deg2rad);
thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
last_time = DateTime.Now;
}
public MultiCopter(string frame = "quadx")
{
self = this;
motors = Motor.build_motors(MAVLink.MAV_TYPE.QUADROTOR, (int)MissionPlanner.GCSViews.ConfigurationView.ConfigFrameType.Frame.Plus);
motor_speed = new double[motors.Length];
mass = 1.5;// # Kg
frame_height = 0.1;
hover_throttle = 0.51;
terminal_velocity = 15.0;
terminal_rotation_rate = 4 * (360.0 * deg2rad);
thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
last_time = DateTime.Now;
}
private void ConnectComPort_Click(object sender, EventArgs e)
{
if (threadrun == 0)
{
OutputLog.Clear();
if (MainV2.comPort.BaseStream.IsOpen == false)
{
// CustomMessageBox.Show("Please connect first");
// return;
}
try
{
// reset/create
lastfdmdata = new FGNetFDM();
quad = new HIL.MultiCopter();
if (RAD_JSBSim.Checked)
{
simPort = 5124;
recvPort = 5123;
}
SetupUDPRecv();
if (RAD_softXplanes.Checked)
{
SetupUDPXplanes();
SetupUDPMavLink();
}
else
{
SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw
}
OutputLog.AppendText("Sim Link Started\n");
}
catch (Exception ex) { OutputLog.AppendText("Socket setup problem. Do you have this open already? " + ex.ToString()); }
// set to highest to try prevent any timer issues
System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
{
Name = "Main simu Serial/UDP listener",
IsBackground = true,
Priority = System.Threading.ThreadPriority.Lowest
};
t11.Start();
timer_servo_graph.Start();
}
else
{
timer_servo_graph.Stop();
threadrun = 0;
if (JSBSimSEND != null && JSBSimSEND.Connected)
{
try
{
JSBSimSEND.Client.Send(ASCIIEncoding.ASCII.GetBytes("\n\nexit\n"));
}
catch { }
}
if (SimulatorRECV != null)
SimulatorRECV.Close();
if (SimulatorRECV != null && SimulatorRECV.Connected)
SimulatorRECV.Disconnect(true);
if (SITLRCRECV != null)
SITLRCRECV.Close();
if (MavLink != null)
MavLink.Close();
position.Clear();
if (XplanesSEND != null)
XplanesSEND.Close();
// if (comPort.BaseStream.IsOpen)
// comPort.stopall(true);
OutputLog.AppendText("Sim Link Stopped\n");
System.Threading.Thread.Sleep(1000);
Application.DoEvents();
}
}