using System.Text; using System.Numerics; using System.Windows.Forms; using static DroneSimulator.NetClient; using DroneClient; using TelemetryIO.Models; using TelemetryIO; using DroneClient.Models; namespace DroneSimulator { public partial class Form_Main : Form { private NetClient netClient = new NetClient(); private Telemetry telemetry = Telemetry.Instance; private MonitorContainer monitoring = MonitorContainer.Instance; private System.Windows.Forms.Timer copyDataTimer = new System.Windows.Forms.Timer(); private System.Windows.Forms.Timer MonitoringTimer = new System.Windows.Forms.Timer(); TelemetryServer tele_server; string connectIco = Path.Combine(Application.StartupPath, "Images", "connect.ico"); string disconnectIco = Path.Combine(Application.StartupPath, "Images", "disconnect.ico"); public Form_Main() { InitializeComponent(); tele_server = new TelemetryServer(); if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); } private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e) { if (TeleClientStatusPicture.InvokeRequired) { TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); })); } else { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); } monitoring.resetMonitorContainer(); } private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e) { if (TeleClientStatusPicture.InvokeRequired) { TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); })); } else { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); } } private void MonitoringTimer_Tick(object? sender, EventArgs e) { if (monitoring.isMonitor == 1) { byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)]; for (int i = 0; i < monitoring.monitorFillLevel; i++) { int slot = monitoring.monitorItems[i].slotNo; int offset = monitoring.monitorItems[i].offset; int len = monitoring.monitorItems[i].len; if(len == 0)continue; Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float)); } if (tele_server != null) { tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length)); } } } private void CopyDataTimer_Tick(object? sender, EventArgs e) { /*if (dataDrone != null) { float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ }; Array.Copy(imu, telemetry.imu, 6); }*/ if (monitoring.isMonitor == 1) MonitoringTimer.Start(); else MonitoringTimer.Stop(); } private void ConnectionCallback(object o) { ConnectData data = (ConnectData)o; if (!data.Connect) { try { Invoke((MethodInvoker)delegate { button_Connect.Text = "Connect"; button_Connect.BackColor = Color.Transparent; MessageBox.Show("Connection closed"); }); } catch { } return; } } Drone dataDrone = new Drone(); private void ReceiveCallback(object o) { ReceiveData data = (ReceiveData)o; List? send = dataDrone.DataStream(data.Buffer, data.Size); if (send == null) return; try { foreach (byte[]? b in send) { if (b != null) data.Server?.Send(b); } } catch { } } private void button_Connect_Click(object sender, EventArgs e) { var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback); switch (done) { case NetClient.ClientState.Error: { MessageBox.Show("Error connecting to server"); break; } case NetClient.ClientState.Connected: { button_Connect.Text = "Disconnect"; button_Connect.BackColor = Color.LimeGreen; break; } case NetClient.ClientState.Stop: { button_Connect.Text = "Connect"; button_Connect.BackColor = Color.Transparent; break; } } if (done != NetClient.ClientState.Connected) return; copyDataTimer.Interval = 10; copyDataTimer.Tick += CopyDataTimer_Tick; tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value)); var t = Task.Run(() => tele_server.Open()); tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected; tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected; MonitoringTimer.Interval = 25; MonitoringTimer.Tick += MonitoringTimer_Tick; copyDataTimer.Start(); button1.BackColor = Color.LimeGreen; } private void Form_Main_FormClosing(object sender, FormClosingEventArgs e) { netClient?.Close(); netClient = null; } private void timer_Test_Tick(object sender, EventArgs e) { label_Acc_X.Text = dataDrone.AccX.ToString(); label_Acc_Y.Text = dataDrone.AccY.ToString(); label_Acc_Z.Text = dataDrone.AccZ.ToString(); label_time_acc.Text = dataDrone.TimeAcc.ToString(); label_Gyr_X.Text = dataDrone.GyrX.ToString(); label_Gyr_Y.Text = dataDrone.GyrY.ToString(); label_Gyr_Z.Text = dataDrone.GyrZ.ToString(); label_time_gyr.Text = dataDrone.TimeGyr.ToString(); float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ }; Array.Copy(imu, telemetry.imu, 6); label_Pos_X.Text = dataDrone.PosX.ToString(); label_Pos_Y.Text = dataDrone.PosY.ToString(); label_Pos_L.Text = dataDrone.LaserRange.ToString(); label_time_range.Text = dataDrone.TimeRange.ToString(); float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange }; Array.Copy(pos, telemetry.pos, 3); label_OF_X.Text = dataDrone.OF.X.ToString(); label_OF_Y.Text = dataDrone.OF.Y.ToString(); label_time_of.Text = dataDrone.TimeRange.ToString(); float[] of = { dataDrone.OF.X, dataDrone.OF.Y }; Array.Copy(of, telemetry.of, 2); if (monitoring.isMonitor == 1) MonitoringTimer.Start(); else MonitoringTimer.Stop(); netClient.SendData(dataDrone.SendReqest()); } private void trackBar_Power_Scroll(object sender, EventArgs e) { float pow = (float)trackBar_Power.Value / 100; label_Pow.Text = pow.ToString(); dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow; } private void button_UU_MouseDown(object sender, MouseEventArgs e) { float pow = ((float)trackBar_Value.Value) / 10.0f; if (sender == button_UU) { dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow; dataDrone.MotorDL += pow; dataDrone.MotorDR += pow; } if (sender == button_DD) { dataDrone.MotorUL += pow; dataDrone.MotorUR += pow; dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow; } if (sender == button_LL) { dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; } if (sender == button_RR) { dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; } if (sender == button_ML) { dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow; dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow; } if (sender == button_MR) { dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow; dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow; } } private void button_UU_MouseUp(object sender, MouseEventArgs e) { trackBar_Power_Scroll(null, null); } private void button1_Click(object sender, EventArgs e) { copyDataTimer.Interval = 10; copyDataTimer.Tick += CopyDataTimer_Tick; tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value)); var t = Task.Run(() => tele_server.Open()); tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected; tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected; MonitoringTimer.Interval = 25; MonitoringTimer.Tick += MonitoringTimer_Tick; copyDataTimer.Start(); button1.BackColor = Color.LimeGreen; } } }